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

DataFlash: Log_Write_MavCmd replaces Log_Write_Cmd

This commit is contained in:
Randy Mackay 2014-03-16 17:40:30 +09:00
parent 3a6698f54b
commit 869f325c66
2 changed files with 28 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/DataFlash/DataFlash.h
View File

@ -12,7 +12,6 @@
#include <AP_InertialSensor.h> #include <AP_InertialSensor.h>
#include <AP_Baro.h> #include <AP_Baro.h>
#include <AP_AHRS.h> #include <AP_AHRS.h>
#include <AP_Mission.h>
#include <stdint.h> #include <stdint.h>
class DataFlash_Class class DataFlash_Class
@ -59,7 +58,7 @@ public:
#if AP_AHRS_NAVEKF_AVAILABLE #if AP_AHRS_NAVEKF_AVAILABLE
void Log_Write_EKF(AP_AHRS_NavEKF &ahrs); void Log_Write_EKF(AP_AHRS_NavEKF &ahrs);
#endif #endif
void Log_Write_Cmd(uint16_t cmd_total, const AP_Mission::Mission_Command& cmd); void Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);
void Log_Write_Message(const char *message); void Log_Write_Message(const char *message);
void Log_Write_Message_P(const prog_char_t *message); void Log_Write_Message_P(const prog_char_t *message);
@ -324,13 +323,15 @@ struct PACKED log_Cmd {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint32_t time_ms; uint32_t time_ms;
uint16_t command_total; uint16_t command_total;
uint16_t command_number; uint16_t sequence;
uint8_t waypoint_id; uint16_t command;
uint8_t waypoint_options; float param1;
uint16_t waypoint_param1; float param2;
int32_t waypoint_altitude; float param3;
int32_t waypoint_latitude; float param4;
int32_t waypoint_longitude; float latitude;
float longitude;
float altitude;
}; };
#define LOG_COMMON_STRUCTURES \ #define LOG_COMMON_STRUCTURES \
@ -369,7 +370,7 @@ struct PACKED log_Cmd {
{ LOG_EKF4_MSG, sizeof(log_EKF4), \ { LOG_EKF4_MSG, sizeof(log_EKF4), \
"EKF4","Icccccchhhc","TimeMS,SVN,SVE,SVD,SPN,SPE,SPD,SMX,SMY,SMZ,SVT" }, \ "EKF4","Icccccchhhc","TimeMS,SVN,SVE,SVD,SPN,SPE,SPD,SMX,SMY,SMZ,SVT" }, \
{ LOG_CMD_MSG, sizeof(log_Cmd), \ { LOG_CMD_MSG, sizeof(log_Cmd), \
"CMD", "IHHBBHeLL","TimeMS,CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" } "CMD", "IHHHfffffff","TimeMS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt" }
// message types for common messages // message types for common messages
#define LOG_FORMAT_MSG 128 #define LOG_FORMAT_MSG 128

36
libraries/DataFlash/LogFile.cpp
View File

@ -943,35 +943,21 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
#endif #endif
// Write a command processing packet // Write a command processing packet
void DataFlash_Class::Log_Write_Cmd(uint16_t cmd_total, const AP_Mission::Mission_Command& cmd) void DataFlash_Class::Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd)
{ {
if (cmd.id == MAV_CMD_DO_JUMP) {
struct log_Cmd pkt = { struct log_Cmd pkt = {
LOG_PACKET_HEADER_INIT(LOG_CMD_MSG), LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
time_ms : hal.scheduler->millis(), time_ms : hal.scheduler->millis(),
command_total : cmd_total, command_total : (uint16_t)cmd_total,
command_number : cmd.index, sequence : (uint16_t)mav_cmd.seq,
waypoint_id : cmd.id, command : (uint16_t)mav_cmd.command,
waypoint_options : 0, param1 : (float)mav_cmd.param1,
waypoint_param1 : cmd.content.jump.target, param2 : (float)mav_cmd.param2,
waypoint_altitude : 0, param3 : (float)mav_cmd.param3,
waypoint_latitude : cmd.content.jump.num_times, param4 : (float)mav_cmd.param4,
waypoint_longitude : 0 latitude : (float)mav_cmd.x,
longitude : (float)mav_cmd.y,
altitude : (float)mav_cmd.z
}; };
WriteBlock(&pkt, sizeof(pkt)); WriteBlock(&pkt, sizeof(pkt));
}else{
struct log_Cmd pkt = {
LOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
time_ms : hal.scheduler->millis(),
command_total : cmd_total,
command_number : cmd.index,
waypoint_id : cmd.id,
waypoint_options : cmd.content.location.options,
waypoint_param1 : (uint16_t)cmd.p1,
waypoint_altitude : cmd.content.location.alt,
waypoint_latitude : cmd.content.location.lat,
waypoint_longitude : cmd.content.location.lng
};
WriteBlock(&pkt, sizeof(pkt));
}
} }