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

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:
Andrew Tridgell 2014-11-20 21:53:12 +11:00
parent 294d944cbd
commit 1d0122e6c2
2 changed files with 42 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/DataFlash/DataFlash.h
View File

@ -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

58
libraries/DataFlash/LogFile.cpp
View File

@ -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));
}
}
}