zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
mission-4.1.18
Andrew Tridgell 10 years ago
parent
294d944cbd
commit
1d0122e6c2
2 changed files with 43 additions and 52 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 35
      libraries/DataFlash/DataFlash.h
  2. 60
      libraries/DataFlash/LogFile.cpp

35
libraries/DataFlash/DataFlash.h
Unescape View File

@ -436,17 +436,10 @@ struct PACKED log_Ubx3 { @@ -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 { @@ -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 { @@ -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

60
libraries/DataFlash/LogFile.cpp
Unescape View File

@ -756,6 +756,7 @@ void DataFlash_Class::Log_Write_RCOUT(void) @@ -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 @@ -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) @@ -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;
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));
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)
};
WriteBlock(&pkt, sizeof(pkt));
}
WriteBlock(&pkt, sizeof(pkt));
}
}
}

Write Preview
Loading…
Cancel
Save