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

DataFlash: added logging of ESC feedback (currently only available with UAVCAN ESCs)

Browse Source
This commit is contained in:
Holger Steinhaus 2014-11-13 11:38:33 +01:00 committed by Andrew Tridgell
parent 2f0bc1f202
commit 294d944cbd
2 changed files with 87 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/DataFlash/DataFlash.h
View File

@ -14,6 +14,9 @@
#include <AP_AHRS.h>
#include <stdint.h>
#include <uORB/topics/esc_status.h>
#if HAL_CPU_CLASS < HAL_CPU_CLASS_75 && defined(APM_BUILD_DIRECTORY)
#if (APM_BUILD_TYPE(APM_BUILD_ArduCopter) || defined(__AVR_ATmega1280__))
#define DATAFLASH_NO_CLI
@ -71,6 +74,7 @@ public:
void Log_Write_Message(const char *message);
void Log_Write_Message_P(const prog_char_t *message);
void Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, const Location &current_loc);
void Log_Write_ESC(void);
bool logging_started(void) const { return log_write_started; }
@ -429,6 +433,22 @@ struct PACKED log_Ubx3 {
float sAcc;
};
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;
};
// messages for all boards
#define LOG_BASE_STRUCTURES \
{ LOG_FORMAT_MSG, sizeof(log_Format), \
@ -483,7 +503,15 @@ struct PACKED log_Ubx3 {
{ LOG_UBX2_MSG, sizeof(log_Ubx2), \
"UBX2", "IBbBbB", "TimeMS,Instance,ofsI,magI,ofsQ,magQ" }, \
{ LOG_UBX3_MSG, sizeof(log_Ubx3), \
"UBX3", "IBfff", "TimeMS,Instance,hAcc,vAcc,sAcc" }
"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" }, \
{ LOG_ESC2_MSG, sizeof(log_Esc), \
"ESC2", "IBcccccccc", "TimeMS,ESCs,RPM2,Volt2,Curr2,Temp2,RPM3,Volt3,Curr3,Temp3" }, \
{ LOG_ESC3_MSG, sizeof(log_Esc), \
"ESC3", "IBcccccccc", "TimeMS,ESCs,RPM6,Volt6,Curr6,Temp6,RPM7,Volt7,Curr7,Temp7" }, \
{ LOG_ESC4_MSG, sizeof(log_Esc), \
"ESC4", "IBcccccccc", "TimeMS,ESCs,RPM6,Volt6,Curr6,Temp6,RPM7,Volt7,Curr7,Temp7" }
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES
@ -520,6 +548,10 @@ struct PACKED log_Ubx3 {
#define LOG_UBX1_MSG 151
#define LOG_UBX2_MSG 152
#define LOG_UBX3_MSG 153
#define LOG_ESC1_MSG 154
#define LOG_ESC2_MSG 155
#define LOG_ESC3_MSG 156
#define LOG_ESC4_MSG 157
// message types 200 to 210 reversed for GPS driver use
// message types 211 to 220 reversed for autotune use

54
libraries/DataFlash/LogFile.cpp
View File

@ -1056,3 +1056,57 @@ void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, c
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write a Camera packet
void DataFlash_Class::Log_Write_ESC(void)
{
static struct esc_status_s esc_status;
static int _esc_status_sub = -1;
if (_esc_status_sub < 0) {
// subscribe to ORB topic on first call
_esc_status_sub = orb_subscribe(ORB_ID(esc_status));
}
// check for new ESC status data
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));
}
WriteBlock(&pkt, sizeof(pkt));
}
}
}