Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Logger: added CSRV logging for CAN servo status

Browse Source
This commit is contained in:
Andrew Tridgell 2020-01-08 08:36:42 +11:00
parent 21cc58e513
commit 4e50759828
3 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Logger/AP_Logger.h
View File

@ -255,6 +255,7 @@ public:
void Write_Camera(const Location &current_loc, uint64_t timestamp_us=0);
void Write_Trigger(const Location &current_loc);
void Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t temperature, uint16_t current_tot);
void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct);
void Write_Attitude(const Vector3f &targets);
void Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets);
void Write_Current();

17
libraries/AP_Logger/LogFile.cpp
View File

@ -798,6 +798,23 @@ void AP_Logger::Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t vo
WriteBlock(&pkt, sizeof(pkt));
}
/*
write servo status from CAN servo
*/
void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct)
{
const struct log_CSRV pkt {
LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG),
time_us : time_us,
id : id,
position : position,
force : force,
speed : speed,
power_pct : power_pct
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write a Yaw PID packet
void AP_Logger::Write_PID(uint8_t msg_type, const PID_Info &info)
{

13
libraries/AP_Logger/LogStructure.h
View File

@ -941,6 +941,16 @@ struct PACKED log_Esc {
uint16_t current_tot;
};
struct PACKED log_CSRV {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t id;
float position;
float force;
float speed;
uint8_t power_pct;
};
struct PACKED log_AIRSPEED {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -1513,6 +1523,8 @@ struct PACKED log_Arm_Disarm {
"ESC7", ESC_FMT, ESC_LABELS, ESC_UNITS, ESC_MULTS }, \
{ LOG_ESC8_MSG, sizeof(log_Esc), \
"ESC8", ESC_FMT, ESC_LABELS, ESC_UNITS, ESC_MULTS }, \
{ LOG_CSRV_MSG, sizeof(log_CSRV), \
"CSRV","QBfffB","TimeUS,Id,Pos,Force,Speed,Pow", "s#---%", "F-0000" }, \
{ LOG_COMPASS2_MSG, sizeof(log_Compass), \
"MAG2",MAG_FMT, MAG_LABELS, MAG_UNITS, MAG_MULTS }, \
{ LOG_COMPASS3_MSG, sizeof(log_Compass), \
@ -1680,6 +1692,7 @@ enum LogMessages : uint8_t {
LOG_ESC6_MSG,
LOG_ESC7_MSG,
LOG_ESC8_MSG,
LOG_CSRV_MSG,
LOG_BAR2_MSG,
LOG_ARSP_MSG,
LOG_ATTITUDE_MSG,