mirror of https://github.com/ArduPilot/ardupilot
DataFlash: log each ESC separately
this saves log space for ESCs that are not initialised (eg. only 3rd ESC on fixed wing) and is a bit easier to deal with as a user
This commit is contained in:
parent
294d944cbd
commit
1d0122e6c2
|
@ -436,17 +436,10 @@ struct PACKED log_Ubx3 {
|
|||
struct PACKED log_Esc {
|
||||
LOG_PACKET_HEADER;
|
||||
uint32_t time_ms;
|
||||
uint8_t escs_present;
|
||||
|
||||
int16_t esc0_rpm;
|
||||
int16_t esc0_voltage;
|
||||
int16_t esc0_current;
|
||||
int16_t esc0_temperature;
|
||||
|
||||
int16_t esc1_rpm;
|
||||
int16_t esc1_voltage;
|
||||
int16_t esc1_current;
|
||||
int16_t esc1_temperature;
|
||||
int16_t rpm;
|
||||
int16_t voltage;
|
||||
int16_t current;
|
||||
int16_t temperature;
|
||||
};
|
||||
|
||||
// messages for all boards
|
||||
|
@ -505,13 +498,21 @@ struct PACKED log_Esc {
|
|||
{ LOG_UBX3_MSG, sizeof(log_Ubx3), \
|
||||
"UBX3", "IBfff", "TimeMS,Instance,hAcc,vAcc,sAcc" }, \
|
||||
{ LOG_ESC1_MSG, sizeof(log_Esc), \
|
||||
"ESC1", "IBcccccccc", "TimeMS,ESCs,RPM0,Volt0,Curr0,Temp0,RPM1,Volt1,Curr1,Temp1" }, \
|
||||
"ESC1", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
|
||||
{ LOG_ESC2_MSG, sizeof(log_Esc), \
|
||||
"ESC2", "IBcccccccc", "TimeMS,ESCs,RPM2,Volt2,Curr2,Temp2,RPM3,Volt3,Curr3,Temp3" }, \
|
||||
"ESC2", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
|
||||
{ LOG_ESC3_MSG, sizeof(log_Esc), \
|
||||
"ESC3", "IBcccccccc", "TimeMS,ESCs,RPM6,Volt6,Curr6,Temp6,RPM7,Volt7,Curr7,Temp7" }, \
|
||||
"ESC3", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
|
||||
{ LOG_ESC4_MSG, sizeof(log_Esc), \
|
||||
"ESC4", "IBcccccccc", "TimeMS,ESCs,RPM6,Volt6,Curr6,Temp6,RPM7,Volt7,Curr7,Temp7" }
|
||||
"ESC4", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
|
||||
{ LOG_ESC5_MSG, sizeof(log_Esc), \
|
||||
"ESC5", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
|
||||
{ LOG_ESC6_MSG, sizeof(log_Esc), \
|
||||
"ESC6", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
|
||||
{ LOG_ESC7_MSG, sizeof(log_Esc), \
|
||||
"ESC7", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \
|
||||
{ LOG_ESC8_MSG, sizeof(log_Esc), \
|
||||
"ESC8", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }
|
||||
|
||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||
#define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES
|
||||
|
@ -552,6 +553,10 @@ struct PACKED log_Esc {
|
|||
#define LOG_ESC2_MSG 155
|
||||
#define LOG_ESC3_MSG 156
|
||||
#define LOG_ESC4_MSG 157
|
||||
#define LOG_ESC5_MSG 158
|
||||
#define LOG_ESC6_MSG 159
|
||||
#define LOG_ESC7_MSG 160
|
||||
#define LOG_ESC8_MSG 161
|
||||
|
||||
// message types 200 to 210 reversed for GPS driver use
|
||||
// message types 211 to 220 reversed for autotune use
|
||||
|
|
|
@ -756,6 +756,7 @@ void DataFlash_Class::Log_Write_RCOUT(void)
|
|||
chan12 : hal.rcout->read(11)
|
||||
};
|
||||
WriteBlock(&pkt, sizeof(pkt));
|
||||
Log_Write_ESC();
|
||||
}
|
||||
|
||||
// Write a BARO packet
|
||||
|
@ -1057,13 +1058,13 @@ void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, c
|
|||
WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
// Write a Camera packet
|
||||
// Write ESC status messages
|
||||
void DataFlash_Class::Log_Write_ESC(void)
|
||||
{
|
||||
static struct esc_status_s esc_status;
|
||||
static int _esc_status_sub = -1;
|
||||
struct esc_status_s esc_status;
|
||||
|
||||
if (_esc_status_sub < 0) {
|
||||
if (_esc_status_sub == -1) {
|
||||
// subscribe to ORB topic on first call
|
||||
_esc_status_sub = orb_subscribe(ORB_ID(esc_status));
|
||||
}
|
||||
|
@ -1072,41 +1073,26 @@ void DataFlash_Class::Log_Write_ESC(void)
|
|||
bool esc_updated = false;
|
||||
orb_check(_esc_status_sub, &esc_updated);
|
||||
if (esc_updated && (OK == orb_copy(ORB_ID(esc_status), _esc_status_sub, &esc_status))) {
|
||||
for (int msg_index = 0; msg_index < (esc_status.esc_count+1)/2; msg_index++) {
|
||||
int first_esc_index = msg_index*2;
|
||||
bool both = first_esc_index+1 < esc_status.esc_count;
|
||||
if (esc_status.esc_count > 8) {
|
||||
esc_status.esc_count = 8;
|
||||
}
|
||||
uint32_t time_ms = hal.scheduler->millis();
|
||||
for (uint8_t i = 0; i < esc_status.esc_count; i++) {
|
||||
// skip logging ESCs with a esc_address of zero, and this
|
||||
// are probably not populated. The Pixhawk itself should
|
||||
// be address zero
|
||||
if (esc_status.esc[i].esc_address != 0) {
|
||||
struct log_Esc pkt = {
|
||||
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_ESC1_MSG + i)),
|
||||
time_ms : time_ms,
|
||||
rpm : (int16_t)(esc_status.esc[i].esc_rpm/10),
|
||||
voltage : (int16_t)(esc_status.esc[i].esc_voltage*100.f + .5f),
|
||||
current : (int16_t)(esc_status.esc[i].esc_current*100.f + .5f),
|
||||
temperature : (int16_t)(esc_status.esc[i].esc_temperature*100.f + .5f)
|
||||
};
|
||||
|
||||
struct log_Esc pkt = {
|
||||
LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(LOG_ESC1_MSG + msg_index)),
|
||||
time_ms : hal.scheduler->millis(),
|
||||
escs_present : static_cast<uint8_t>(both ? 2 : 1),
|
||||
esc0_rpm : 0,
|
||||
esc0_voltage : 0,
|
||||
esc0_current : 0,
|
||||
esc0_temperature:0,
|
||||
esc1_rpm : 0,
|
||||
esc1_voltage : 0,
|
||||
esc1_current : 0,
|
||||
esc1_temperature:0,
|
||||
};
|
||||
|
||||
int16_t values[4] = {};
|
||||
values[0] = static_cast<int16_t>(esc_status.esc[first_esc_index].esc_rpm/10); // should allow for +-320 kRPM
|
||||
values[1] = static_cast<int16_t>(esc_status.esc[first_esc_index].esc_voltage*100.f + .5f); // maintain compat to CURR msg
|
||||
values[2] = static_cast<int16_t>(esc_status.esc[first_esc_index].esc_current*100.f + .5f); // maintain compat to CURR msg
|
||||
values[3] = static_cast<int16_t>(esc_status.esc[first_esc_index].esc_temperature*100.f + .5f); // enough for logging self-desoldering FETs
|
||||
memcpy(&pkt.esc0_rpm, values, sizeof(values));
|
||||
|
||||
if (both) {
|
||||
// only iff even number of ESCs present, otherwise this part stays at 0.
|
||||
values[0] = static_cast<int16_t>(esc_status.esc[first_esc_index+1].esc_rpm/10); // should allow for +-320 kRPM
|
||||
values[1] = static_cast<int16_t>(esc_status.esc[first_esc_index+1].esc_voltage*100.f + .5f); // maintain compat to CURR msg
|
||||
values[2] = static_cast<int16_t>(esc_status.esc[first_esc_index+1].esc_current*100.f + .5f); // maintain compat to CURR msg
|
||||
values[3] = static_cast<int16_t>(esc_status.esc[first_esc_index+1].esc_temperature*100.f + .5f); // enough for logging self-desoldering FETs
|
||||
memcpy(&pkt.esc1_rpm, values, sizeof(values));
|
||||
WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue