aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Logger: add Write_Command for writing out received mavlink commands

Browse Source
This commit is contained in:
Peter Barker 2019-08-23 11:21:39 +10:00 committed by Andrew Tridgell
parent 5010de9560
commit 631725f817
3 changed files with 50 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

@ -257,6 +257,7 @@ public:
void Write_Mode(uint8_t mode, uint8_t reason);
void Write_EntireMission();
void Write_Command(const mavlink_command_int_t &packet, MAV_RESULT result, bool was_command_long=false);
void Write_Mission_Cmd(const AP_Mission &mission,
const AP_Mission::Mission_Command &cmd);
void Write_Origin(uint8_t origin_type, const Location &loc);

26
libraries/AP_Logger/LogFile.cpp
View File

@ -398,6 +398,32 @@ void AP_Logger::Write_Vibration()
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_Command(const mavlink_command_int_t &packet,
const MAV_RESULT result,
bool was_command_long)
{
const struct log_MAVLink_Command pkt{
LOG_PACKET_HEADER_INIT(LOG_MAVLINK_COMMAND_MSG),
time_us : AP_HAL::micros64(),
target_system : packet.target_system,
target_component: packet.target_component,
frame : packet.frame,
command : packet.command,
current : packet.current,
autocontinue : packet.autocontinue,
param1 : packet.param1,
param2 : packet.param2,
param3 : packet.param3,
param4 : packet.param4,
x : packet.x,
y : packet.y,
z : packet.z,
result : (uint8_t)result,
was_command_long:was_command_long,
};
return WriteBlock(&pkt, sizeof(pkt));
}
bool AP_Logger_Backend::Write_Mission_Cmd(const AP_Mission &mission,
const AP_Mission::Mission_Command &cmd)
{

23
libraries/AP_Logger/LogStructure.h
View File

@ -682,6 +682,26 @@ struct PACKED log_Cmd {
uint8_t frame;
};
struct PACKED log_MAVLink_Command {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t target_system;
uint8_t target_component;
uint8_t frame;
uint16_t command;
uint8_t current;
uint8_t autocontinue;
float param1;
float param2;
float param3;
float param4;
int32_t x;
int32_t y;
float z;
uint8_t result;
bool was_command_long;
};
struct PACKED log_Radio {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -1307,6 +1327,8 @@ struct PACKED log_Arm_Disarm {
"POWR","QffHB","TimeUS,Vcc,VServo,Flags,Safety", "svv--", "F00--" }, \
{ LOG_CMD_MSG, sizeof(log_Cmd), \
"CMD", "QHHHffffLLfB","TimeUS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt,Frame", "s-------DUm-", "F-------GG0-" }, \
{ LOG_MAVLINK_COMMAND_MSG, sizeof(log_MAVLink_Command), \
"MAVC", "QBBBHBBffffiifBB","TimeUS,TS,TC,Fr,Cmd,Cur,AC,P1,P2,P3,P4,X,Y,Z,Res,WL", "s---------------", "F---------------" }, \
{ LOG_RADIO_MSG, sizeof(log_Radio), \
"RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed", "s-------", "F-------" }, \
{ LOG_CAMERA_MSG, sizeof(log_Camera), \
@ -1637,6 +1659,7 @@ enum LogMessages : uint8_t {
LOG_AHR2_MSG,
LOG_SIMSTATE_MSG,
LOG_CMD_MSG,
LOG_MAVLINK_COMMAND_MSG,
LOG_RADIO_MSG,
LOG_ATRP_MSG,
LOG_CAMERA_MSG,