Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_MSP: fix DJI FPV temperature and RPM #17569

Merged
merged 4 commits into from Jul 6, 2021
Merged

AP_MSP: fix DJI FPV temperature and RPM #17569

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c733578
AP_ESC_Telem: add method to get average motor RPM
shellixyz Jul 5, 2021
00ee612
AP_ESC_Telem: add method to get the temperature of the motor with hig…
shellixyz Jul 5, 2021
48dc29f
AP_MSP: fix DJI FPV temperature and RPM
shellixyz May 26, 2021
d67ac33
AP_OSD: rename BLH elements to ESC
shellixyz Jun 3, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
43 changes: 24 additions & 19 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View file
Open in desktop
Expand Up @@ -34,39 +34,28 @@ AP_ESC_Telem::AP_ESC_Telem()
_singleton = this;
}

// return the average motor frequency in Hz for selected motors
float AP_ESC_Telem::get_average_motor_frequency_hz(uint32_t servo_channel_mask) const
// return the average motor RPM
float AP_ESC_Telem::get_average_motor_rpm(uint32_t servo_channel_mask) const
{
float motor_freq = 0.0f;
float rpm_avg = 0.0f;
uint8_t valid_escs = 0;

// average the rpm of each motor and convert to Hz
// average the rpm of each motor
for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
if (BIT_IS_SET(servo_channel_mask,i)) {
float rpm;
if (get_rpm(i, rpm)) {
motor_freq += rpm * (1.0f / 60.0f);
rpm_avg += rpm;
valid_escs++;
}
}
}

if (valid_escs > 0) {
motor_freq /= valid_escs;
rpm_avg /= valid_escs;
}

return motor_freq;
}

// return the average motor frequency in Hz for dynamic filtering
float AP_ESC_Telem::get_average_motor_frequency_hz() const
{
return get_average_motor_frequency_hz(0xFFFFFFFF);
}

// return the average motor rpm for selected motor channels
float AP_ESC_Telem::get_average_motor_rpm(uint32_t servo_channel_mask) const
{
return get_average_motor_frequency_hz(servo_channel_mask)*60.f; // Hz to rpm
return rpm_avg;
}

// return all the motor frequencies in Hz for dynamic filtering
Expand Down Expand Up @@ -156,6 +145,22 @@ bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const
return true;
}

// get the highest ESC temperature in centi-degrees if available, returns true if there is valid data for at least one ESC
bool AP_ESC_Telem::get_highest_motor_temperature(int16_t& temp) const
{
uint8_t valid_escs = 0;

for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
int16_t temp_temp;
if (get_motor_temperature(i, temp_temp)) {
temp = MAX(temp, temp_temp);
valid_escs++;
}
}

return valid_escs > 0;
}

// get an individual ESC's current in Ampere if available, returns true on success
bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const
{
Expand Down
16 changes: 11 additions & 5 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.h
View file
Open in desktop
Expand Up @@ -27,12 +27,21 @@ class AP_ESC_Telem {
// get an individual ESC's raw rpm if available
bool get_raw_rpm(uint8_t esc_index, float& rpm) const;

// return the average motor RPM
float get_average_motor_rpm(uint32_t servo_channel_mask) const;

// return the average motor RPM
float get_average_motor_rpm() const { return get_average_motor_rpm(0xFFFFFFFF); }

// get an individual ESC's temperature in centi-degrees if available, returns true on success
bool get_temperature(uint8_t esc_index, int16_t& temp) const;

// get an individual motor's temperature in centi-degrees if available, returns true on success
bool get_motor_temperature(uint8_t esc_index, int16_t& temp) const;

// get the highest ESC temperature in centi-degrees if available, returns true if there is valid data for at least one ESC
bool get_highest_motor_temperature(int16_t& temp) const;

// get an individual ESC's current in Ampere if available, returns true on success
bool get_current(uint8_t esc_index, float& amps) const;

Expand All @@ -45,14 +54,11 @@ class AP_ESC_Telem {
// get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success
bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const;

// return the average selected motor rpm
float get_average_motor_rpm(uint32_t esc_mask) const;

// return the average motor frequency in Hz for dynamic filtering
float get_average_motor_frequency_hz(uint32_t esc_mask) const;
float get_average_motor_frequency_hz(uint32_t servo_channel_mask) const { return get_average_motor_rpm(servo_channel_mask) * (1.0f / 60.0f); };

// return the average motor frequency in Hz for dynamic filtering
float get_average_motor_frequency_hz() const;
float get_average_motor_frequency_hz() const { return get_average_motor_frequency_hz(0xFFFFFFFF); }

// return all of the motor frequencies in Hz for dynamic filtering
uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const;
Expand Down
8 changes: 4 additions & 4 deletions libraries/AP_MSP/AP_MSP.cpp
View file
Open in desktop
Expand Up @@ -42,9 +42,9 @@ const AP_Param::GroupInfo AP_MSP::var_info[] = {
// @Param: _OPTIONS
// @DisplayName: MSP OSD Options
// @Description: A bitmask to set some MSP specific options
// @Bitmask: 0:EnableTelemetryMode
// @Bitmask: 0:EnableTelemetryMode, 1: DJIWorkarounds
// @User: Standard
AP_GROUPINFO("_OPTIONS", 2, AP_MSP, _options, 0),
AP_GROUPINFO("_OPTIONS", 2, AP_MSP, _options, OPTION_TELEMETRY_DJI_WORKAROUNDS),

AP_GROUPEND
};
Expand Down Expand Up @@ -149,8 +149,8 @@ void AP_MSP::init_osd()
_osd_item_settings[OSD_HOME_DIST] = &osd->screen[0].home_dist;
_osd_item_settings[OSD_NUMERICAL_HEADING] = &osd->screen[0].heading;
_osd_item_settings[OSD_NUMERICAL_VARIO] = &osd->screen[0].vspeed;
#ifdef HAVE_AP_BLHELI_SUPPORT
_osd_item_settings[OSD_ESC_TMP] = &osd->screen[0].blh_temp;
#if HAL_WITH_ESC_TELEM
_osd_item_settings[OSD_ESC_TMP] = &osd->screen[0].esc_temp;
#endif
_osd_item_settings[OSD_RTC_DATETIME] = &osd->screen[0].clk;
#endif // OSD_ENABLED
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_MSP/AP_MSP.h
View file
Open in desktop
Expand Up @@ -59,6 +59,7 @@ class AP_MSP

enum msp_option_e : uint8_t {
OPTION_TELEMETRY_MODE = 1U<<0,
OPTION_TELEMETRY_DJI_WORKAROUNDS = 1U<<1
};

AP_MSP_Telem_Backend *_backends[MSP_MAX_INSTANCES];
Expand Down
17 changes: 17 additions & 0 deletions libraries/AP_MSP/AP_MSP_Telem_DJI.cpp
View file
Open in desktop
Expand Up @@ -104,4 +104,21 @@ MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_fc_variant(sbuf_t *dst)
return MSP_RESULT_ACK;
}

MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_esc_sensor_data(sbuf_t *dst)
{
#if HAL_WITH_ESC_TELEM
const auto msp = AP::msp();
if (msp && (msp->_options & AP_MSP::OPTION_TELEMETRY_DJI_WORKAROUNDS)) {
AP_ESC_Telem& telem = AP::esc_telem();
int16_t highest_temperature = 0;
telem.get_highest_motor_temperature(highest_temperature);
sbuf_write_u8(dst, uint8_t(highest_temperature / 100)); // deg, report max temperature
sbuf_write_u16(dst, uint16_t(telem.get_average_motor_rpm() * 0.1f)); // rpm, report average RPM across all motors
} else {
return AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(dst);
}
#endif
return MSP_RESULT_ACK;
}

#endif //HAL_MSP_ENABLED
1 change: 1 addition & 0 deletions libraries/AP_MSP/AP_MSP_Telem_DJI.h
View file
Open in desktop
Expand Up @@ -57,6 +57,7 @@ class AP_MSP_Telem_DJI : public AP_MSP_Telem_Backend
MSP::MSPCommandResult msp_process_out_api_version(MSP::sbuf_t *dst) override;
MSP::MSPCommandResult msp_process_out_fc_version(MSP::sbuf_t *dst) override;
MSP::MSPCommandResult msp_process_out_fc_variant(MSP::sbuf_t *dst) override;
MSP::MSPCommandResult msp_process_out_esc_sensor_data(MSP::sbuf_t *dst) override;

enum : uint8_t {
DJI_FLAG_ARM = 0,
Expand Down
12 changes: 6 additions & 6 deletions libraries/AP_OSD/AP_OSD.h
View file
Open in desktop
Expand Up @@ -156,9 +156,9 @@ class AP_OSD_Screen : public AP_OSD_AbstractScreen
AP_OSD_Setting aspd2{false, 0, 0};
AP_OSD_Setting vspeed{true, 24, 9};
#if HAL_WITH_ESC_TELEM
AP_OSD_Setting blh_temp {false, 24, 13};
AP_OSD_Setting blh_rpm{false, 22, 12};
AP_OSD_Setting blh_amps{false, 24, 14};
AP_OSD_Setting esc_temp {false, 24, 13};
AP_OSD_Setting esc_rpm{false, 22, 12};
AP_OSD_Setting esc_amps{false, 24, 14};
#endif
AP_OSD_Setting gps_latitude{true, 9, 13};
AP_OSD_Setting gps_longitude{true, 9, 14};
Expand Down Expand Up @@ -228,9 +228,9 @@ class AP_OSD_Screen : public AP_OSD_AbstractScreen
void draw_speed(uint8_t x, uint8_t y, float angle_rad, float magnitude);
void draw_distance(uint8_t x, uint8_t y, float distance);
#if HAL_WITH_ESC_TELEM
void draw_blh_temp(uint8_t x, uint8_t y);
void draw_blh_rpm(uint8_t x, uint8_t y);
void draw_blh_amps(uint8_t x, uint8_t y);
void draw_esc_temp(uint8_t x, uint8_t y);
void draw_esc_rpm(uint8_t x, uint8_t y);
void draw_esc_amps(uint8_t x, uint8_t y);
#endif
void draw_gps_latitude(uint8_t x, uint8_t y);
void draw_gps_longitude(uint8_t x, uint8_t y);
Expand Down
64 changes: 32 additions & 32 deletions libraries/AP_OSD/AP_OSD_Screen.cpp
View file
Open in desktop
Expand Up @@ -344,54 +344,55 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
// @Description: Vertical position on screen
// @Range: 0 15
AP_SUBGROUPINFO(vspeed, "VSPEED", 20, AP_OSD_Screen, AP_OSD_Setting),

#if HAL_WITH_ESC_TELEM
// @Param: BLHTEMP_EN
// @DisplayName: BLHTEMP_EN
// @Param: ESCTEMP_EN
// @DisplayName: ESCTEMP_EN
// @Description: Displays first esc's temp
// @Values: 0:Disabled,1:Enabled

// @Param: BLHTEMP_X
// @DisplayName: BLHTEMP_X
// @Param: ESCTEMP_X
// @DisplayName: ESCTEMP_X
// @Description: Horizontal position on screen
// @Range: 0 29

// @Param: BLHTEMP_Y
// @DisplayName: BLHTEMP_Y
// @Param: ESCTEMP_Y
// @DisplayName: ESCTEMP_Y
// @Description: Vertical position on screen
// @Range: 0 15
AP_SUBGROUPINFO(blh_temp, "BLHTEMP", 21, AP_OSD_Screen, AP_OSD_Setting),
AP_SUBGROUPINFO(esc_temp, "ESCTEMP", 21, AP_OSD_Screen, AP_OSD_Setting),

// @Param: BLHRPM_EN
// @DisplayName: BLHRPM_EN
// @Param: ESCRPM_EN
// @DisplayName: ESCRPM_EN
// @Description: Displays first esc's rpm
// @Values: 0:Disabled,1:Enabled

// @Param: BLHRPM_X
// @DisplayName: BLHRPM_X
// @Param: ESCRPM_X
// @DisplayName: ESCRPM_X
// @Description: Horizontal position on screen
// @Range: 0 29

// @Param: BLHRPM_Y
// @DisplayName: BLHRPM_Y
// @Param: ESCRPM_Y
// @DisplayName: ESCRPM_Y
// @Description: Vertical position on screen
// @Range: 0 15
AP_SUBGROUPINFO(blh_rpm, "BLHRPM", 22, AP_OSD_Screen, AP_OSD_Setting),
AP_SUBGROUPINFO(esc_rpm, "ESCRPM", 22, AP_OSD_Screen, AP_OSD_Setting),

// @Param: BLHAMPS_EN
// @DisplayName: BLHAMPS_EN
// @Param: ESCAMPS_EN
// @DisplayName: ESCAMPS_EN
// @Description: Displays first esc's current
// @Values: 0:Disabled,1:Enabled

// @Param: BLHAMPS_X
// @DisplayName: BLHAMPS_X
// @Param: ESCAMPS_X
// @DisplayName: ESCAMPS_X
// @Description: Horizontal position on screen
// @Range: 0 29

// @Param: BLHAMPS_Y
// @DisplayName: BLHAMPS_Y
// @Param: ESCAMPS_Y
// @DisplayName: ESCAMPS_Y
// @Description: Vertical position on screen
// @Range: 0 15
AP_SUBGROUPINFO(blh_amps, "BLHAMPS", 23, AP_OSD_Screen, AP_OSD_Setting),
AP_SUBGROUPINFO(esc_amps, "ESCAMPS", 23, AP_OSD_Screen, AP_OSD_Setting),
#endif
// @Param: GPSLAT_EN
// @DisplayName: GPSLAT_EN
Expand Down Expand Up @@ -1622,19 +1623,18 @@ void AP_OSD_Screen::draw_vspeed(uint8_t x, uint8_t y)
}

#if HAL_WITH_ESC_TELEM
void AP_OSD_Screen::draw_blh_temp(uint8_t x, uint8_t y)
void AP_OSD_Screen::draw_esc_temp(uint8_t x, uint8_t y)
{
int16_t etemp;
// first parameter is index into array of ESC's. Hardwire to zero (first) for now.
if (!AP::esc_telem().get_temperature(0, etemp)) {
return;
}

uint8_t esc_temp = uint8_t(etemp / 100);
backend->write(x, y, false, "%3d%c", (int)u_scale(TEMPERATURE, esc_temp), u_icon(TEMPERATURE));
backend->write(x, y, false, "%3d%c", (int)u_scale(TEMPERATURE, etemp / 100), u_icon(TEMPERATURE));
}

void AP_OSD_Screen::draw_blh_rpm(uint8_t x, uint8_t y)
void AP_OSD_Screen::draw_esc_rpm(uint8_t x, uint8_t y)
{
float rpm;
// first parameter is index into array of ESC's. Hardwire to zero (first) for now.
Expand All @@ -1646,14 +1646,14 @@ void AP_OSD_Screen::draw_blh_rpm(uint8_t x, uint8_t y)
backend->write(x, y, false, format, krpm, SYM_KILO, SYM_RPM);
}

void AP_OSD_Screen::draw_blh_amps(uint8_t x, uint8_t y)
void AP_OSD_Screen::draw_esc_amps(uint8_t x, uint8_t y)
{
float esc_amps;
float amps;
// first parameter is index into array of ESC's. Hardwire to zero (first) for now.
if (!AP::esc_telem().get_current(0, esc_amps)) {
if (!AP::esc_telem().get_current(0, amps)) {
return;
}
backend->write(x, y, false, "%4.1f%c", esc_amps, SYM_AMP);
backend->write(x, y, false, "%4.1f%c", amps, SYM_AMP);
}
#endif

Expand Down Expand Up @@ -2055,9 +2055,9 @@ void AP_OSD_Screen::draw(void)
DRAW_SETTING(vtx_power);

#if HAL_WITH_ESC_TELEM
DRAW_SETTING(blh_temp);
DRAW_SETTING(blh_rpm);
DRAW_SETTING(blh_amps);
DRAW_SETTING(esc_temp);
DRAW_SETTING(esc_rpm);
DRAW_SETTING(esc_amps);
#endif

DRAW_SETTING(gps_latitude);
Expand Down