Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_PiccoloCAN: Bug fix for ESC temperature data 

- Fixes issues with temperature data in telemetry and logs
- Use the maximum of available temperature values for the ESC (where available)
This commit is contained in:
Oliver 2021-12-03 17:38:01 +11:00 committed by Andrew Tridgell
parent a281c3ef91
commit 80c7c9f16a
1 changed files with 18 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp
View File

@ -376,7 +376,8 @@ void AP_PiccoloCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
if (is_esc_present(ii)) {
dataAvailable = true;
temperature[idx] = esc.fetTemperature;
// Provide the maximum ESC temperature in the telemetry stream
temperature[idx] = MAX(esc.fetTemperature, esc.escTemperature);
voltage[idx] = esc.voltage;
current[idx] = esc.current;
totalcurrent[idx] = 0;
@ -690,22 +691,28 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame)
// There are no common error bits between the Gen-1 and Gen-2 ICD
} else if (decodeESC_StatusBPacket(&frame, &esc.voltage, &esc.current, &esc.dutyCycle, &esc.escTemperature, &esc.motorTemperature)) {
AP_ESC_Telem_Backend::TelemetryData t {
.temperature_cdeg = int16_t(esc.escTemperature * 100),
.voltage = float(esc.voltage) * 0.01f,
.current = float(esc.current) * 0.01f,
};
AP_ESC_Telem_Backend::TelemetryData telem {};
update_telem_data(addr, t,
telem.voltage = float(esc.voltage) * 0.01f;
telem.current = float(esc.current) * 0.01f;
telem.motor_temp_cdeg = int16_t(esc.motorTemperature * 100);
update_telem_data(addr, telem,
AP_ESC_Telem_Backend::TelemetryType::CURRENT
| AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
| AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
| AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE);
esc.newTelemetry = true;
} else if (decodeESC_StatusCPacket(&frame, &esc.fetTemperature, &esc.pwmFrequency, &esc.timingAdvance)) {
AP_ESC_Telem_Backend::TelemetryData t { };
t.motor_temp_cdeg = int16_t(esc.fetTemperature * 100);
update_telem_data(addr, t, AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE);
// Use the higher reported value of 'escTemperature' and 'fetTemperature'
const int16_t escTemp = MAX(esc.fetTemperature, esc.escTemperature);
AP_ESC_Telem_Backend::TelemetryData telem {};
telem.temperature_cdeg = int16_t(escTemp * 100);
update_telem_data(addr, telem, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
esc.newTelemetry = true;
} else if (decodeESC_WarningErrorStatusPacket(&frame, &esc.warnings, &esc.errors)) {
@ -1053,4 +1060,3 @@ uint32_t getServoPacketID(const void* pkt)
}
#endif // HAL_PICCOLO_CAN_ENABLE