CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp
Views: 1798
1
/*

2
   This program is free software: you can redistribute it and/or modify

3
   it under the terms of the GNU General Public License as published by

4
   the Free Software Foundation, either version 3 of the License, or

5
   (at your option) any later version.

6


7
   This program is distributed in the hope that it will be useful,

8
   but WITHOUT ANY WARRANTY; without even the implied warranty of

9
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

10
   GNU General Public License for more details.

11


12
   You should have received a copy of the GNU General Public License

13
   along with this program.  If not, see <http://www.gnu.org/licenses/>.

14
 */

15


16
#include "AP_ESC_Telem_SITL.h"

17
#include "AP_ESC_Telem.h"

18
#include <AP_HAL/AP_HAL.h>

19
#include <SITL/SITL.h>

20


21
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL

22


23
#include <AP_Math/AP_Math.h>

24


25
extern const AP_HAL::HAL& hal;

26


27
AP_ESC_Telem_SITL::AP_ESC_Telem_SITL()

28
{

29
}

30


31
void AP_ESC_Telem_SITL::update()

32
{

33
    SITL::SIM* sitl = AP::sitl();

34


35
    if (!sitl) {

36
        return;

37
    }

38


39
#if HAL_WITH_ESC_TELEM

40


41
    if (AP_HAL::millis64() < 6000) {

42
        // this prevents us sending blank data at startup, which triggers

43
        // ESC telem messages for all channels

44
        return;

45
    }

46
    uint32_t mask = sitl->state.motor_mask;

47


48
    /*

49
      mask out motors we should not be providing telemetry for. On

50
      AP_Periph SIM_CAN_SRV_MSK are the outputs we will provide

51
      telemetry for, on the main firmware it is the ones we don't

52
      provide telemetry for

53
    */

54
#if defined(HAL_BUILD_AP_PERIPH)

55
    mask &= uint32_t(sitl->can_servo_mask);

56
#else

57
    mask &= ~uint32_t(sitl->can_servo_mask);

58
#endif

59
    uint8_t bit;

60


61
    while ((bit = __builtin_ffs(mask)) != 0) {

62
        uint8_t motor = bit-1;

63
        mask &= ~(1U<<motor);

64


65
        const float min_rpm = hal.util->get_soft_armed()? sitl->esc_rpm_armed : 0;

66
        update_rpm(motor, MAX(min_rpm, sitl->state.rpm[motor]));

67


68
        // some fake values so that is_telemetry_active() returns true

69
        TelemetryData t {

70
            .temperature_cdeg = 3200,

71
            .voltage = 16.8f,

72
            .current = 0.8f,

73
            .consumption_mah = 1.0f,

74
            .motor_temp_cdeg = 3500,

75
#if AP_EXTENDED_ESC_TELEM_ENABLED

76
            .input_duty = 1,

77
            .output_duty = 2,

78
            .flags = 3,

79
            .power_percentage = 4,

80
#endif

81
        };

82


83
        update_telem_data(motor, t,

84
            AP_ESC_Telem_Backend::TelemetryType::CURRENT

85
                | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE

86
                | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION

87
                | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE

88
                | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE

89
#if AP_EXTENDED_ESC_TELEM_ENABLED

90
                | AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE

91
                | AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY

92
                | AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY

93
                | AP_ESC_Telem_Backend::TelemetryType::FLAGS

94
#endif

95
                );

96
    }

97
#endif

98
}

99


100
#endif

101


102