Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MSP: generalise ESC telemetry to allow data from other ESCs

Browse Source 
enable with HAL_WITH_ESC_TELEM
This commit is contained in:
Andy Piper 2021-02-23 22:05:05 +00:00 committed by Andrew Tridgell
parent b63d19533d
commit fcc8853244
2 changed files with 16 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
View File

@ -17,7 +17,7 @@
#include <AP_Baro/AP_Baro.h> #include <AP_Baro/AP_Baro.h>
#include <AP_Airspeed/AP_Airspeed.h> #include <AP_Airspeed/AP_Airspeed.h>
#include <AP_BattMonitor/AP_BattMonitor.h> #include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_BLHeli/AP_BLHeli.h> #include <AP_ESC_Telem/AP_ESC_Telem.h>
#include <RC_Channel/RC_Channel.h> #include <RC_Channel/RC_Channel.h>
#include <AP_Common/AP_FWVersion.h> #include <AP_Common/AP_FWVersion.h>
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
@ -65,7 +65,7 @@ void AP_MSP_Telem_Backend::setup_wfq_scheduler(void)
set_scheduler_entry(ALTITUDE, 250, 250); // 4Hz altitude(cm) and velocity(cm/s) set_scheduler_entry(ALTITUDE, 250, 250); // 4Hz altitude(cm) and velocity(cm/s)
set_scheduler_entry(ANALOG, 250, 250); // 4Hz rssi + batt set_scheduler_entry(ANALOG, 250, 250); // 4Hz rssi + batt
set_scheduler_entry(BATTERY_STATE, 500, 500); // 2Hz battery set_scheduler_entry(BATTERY_STATE, 500, 500); // 2Hz battery
#ifdef HAVE_AP_BLHELI_SUPPORT #if HAL_WITH_ESC_TELEM
set_scheduler_entry(ESC_SENSOR_DATA, 500, 500); // 2Hz ESC telemetry set_scheduler_entry(ESC_SENSOR_DATA, 500, 500); // 2Hz ESC telemetry
#endif #endif
set_scheduler_entry(RTC_DATETIME, 1000, 1000); // 1Hz RTC set_scheduler_entry(RTC_DATETIME, 1000, 1000); // 1Hz RTC
@ -113,7 +113,7 @@ bool AP_MSP_Telem_Backend::is_packet_ready(uint8_t idx, bool queue_empty)
case ALTITUDE: // Altitude and Vario case ALTITUDE: // Altitude and Vario
case ANALOG: // Rssi, Battery, mAh, Current case ANALOG: // Rssi, Battery, mAh, Current
case BATTERY_STATE: // voltage, capacity, current, mAh case BATTERY_STATE: // voltage, capacity, current, mAh
#ifdef HAVE_AP_BLHELI_SUPPORT #if HAL_WITH_ESC_TELEM
case ESC_SENSOR_DATA: // esc temp + rpm case ESC_SENSOR_DATA: // esc temp + rpm
#endif #endif
case RTC_DATETIME: // RTC case RTC_DATETIME: // RTC
@ -450,7 +450,7 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_command(uint16_t cmd_msp,
return msp_process_out_battery_state(dst); return msp_process_out_battery_state(dst);
case MSP_UID: case MSP_UID:
return msp_process_out_uid(dst); return msp_process_out_uid(dst);
#ifdef HAVE_AP_BLHELI_SUPPORT #if HAL_WITH_ESC_TELEM
case MSP_ESC_SENSOR_DATA: case MSP_ESC_SENSOR_DATA:
return msp_process_out_esc_sensor_data(dst); return msp_process_out_esc_sensor_data(dst);
#endif #endif
@ -884,16 +884,18 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_battery_state(sbuf_t *dst
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(sbuf_t *dst) MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(sbuf_t *dst)
{ {
#ifdef HAVE_AP_BLHELI_SUPPORT #if HAL_WITH_ESC_TELEM
AP_BLHeli *blheli = AP_BLHeli::get_singleton(); AP_ESC_Telem& telem = AP::esc_telem();
if (blheli && blheli->have_telem_data()) { if (telem.get_last_telem_data_ms(0)) {
const uint8_t num_motors = blheli->get_num_motors(); const uint8_t num_motors = telem.get_num_active_escs();
sbuf_write_u8(dst, num_motors); sbuf_write_u8(dst, num_motors);
for (uint8_t i = 0; i < num_motors; i++) { for (uint8_t i = 0; i < num_motors; i++) {
AP_BLHeli::telem_data td {}; int16_t temp = 0;
blheli->get_telem_data(i, td); float rpm = 0.0f;
sbuf_write_u8(dst, td.temperature); // deg telem.get_rpm(i, rpm);
sbuf_write_u16(dst, td.rpm / 100); telem.get_temperature(i, temp);
sbuf_write_u8(dst, uint8_t(temp)); // deg
sbuf_write_u16(dst, uint16_t(rpm * 0.1));
} }
} }
#endif #endif

4
libraries/AP_MSP/AP_MSP_Telem_Backend.h
View File

@ -89,7 +89,7 @@ protected:
ALTITUDE, ALTITUDE,
ANALOG, ANALOG,
BATTERY_STATE, BATTERY_STATE,
#ifdef HAVE_AP_BLHELI_SUPPORT #if HAL_WITH_ESC_TELEM
ESC_SENSOR_DATA, ESC_SENSOR_DATA,
#endif #endif
RTC_DATETIME, RTC_DATETIME,
@ -106,7 +106,7 @@ protected:
MSP_ALTITUDE, MSP_ALTITUDE,
MSP_ANALOG, MSP_ANALOG,
MSP_BATTERY_STATE, MSP_BATTERY_STATE,
#ifdef HAVE_AP_BLHELI_SUPPORT #if HAL_WITH_ESC_TELEM
MSP_ESC_SENSOR_DATA, MSP_ESC_SENSOR_DATA,
#endif #endif
MSP_RTC MSP_RTC