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

Dataflash: add Log_Write_Cmd method

This commit is contained in:
Randy Mackay 2014-03-12 17:17:13 +09:00
parent 7ff379850c
commit 150faafaf4
2 changed files with 52 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/DataFlash/DataFlash.h
View File

@ -12,6 +12,7 @@
#include <AP_InertialSensor.h>
#include <AP_Baro.h>
#include <AP_AHRS.h>
#include <AP_Mission.h>
#include <stdint.h>
class DataFlash_Class
@ -58,6 +59,7 @@ public:
#if AP_AHRS_NAVEKF_AVAILABLE
void Log_Write_EKF(AP_AHRS_NavEKF &ahrs);
#endif
void Log_Write_Cmd(uint16_t cmd_total, const AP_Mission::Mission_Command& cmd);
void Log_Write_Message(const char *message);
void Log_Write_Message_P(const prog_char_t *message);
@ -318,6 +320,19 @@ struct PACKED log_EKF4 {
int16_t sqrtvarVT;
};
struct PACKED log_Cmd {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint16_t command_total;
uint16_t command_number;
uint8_t waypoint_id;
uint8_t waypoint_options;
uint16_t waypoint_param1;
int32_t waypoint_altitude;
int32_t waypoint_latitude;
int32_t waypoint_longitude;
};
#define LOG_COMMON_STRUCTURES \
{ LOG_FORMAT_MSG, sizeof(log_Format), \
"FMT", "BBnNZ", "Type,Length,Name,Format,Columns" }, \
@ -352,7 +367,9 @@ struct PACKED log_EKF4 {
{ LOG_EKF3_MSG, sizeof(log_EKF3), \
"EKF3","Icccccchhhc","TimeMS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \
{ 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), \
"CMD", "IHHBBHeLL","TimeMS,CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" }
// message types for common messages
#define LOG_FORMAT_MSG 128
@ -372,6 +389,7 @@ struct PACKED log_EKF4 {
#define LOG_EKF3_MSG 142
#define LOG_EKF4_MSG 143
#define LOG_GPS2_MSG 144
#define LOG_CMD_MSG 145
#include "DataFlash_Block.h"
#include "DataFlash_File.h"

33
libraries/DataFlash/LogFile.cpp
View File

@ -942,3 +942,36 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
}
#endif
// Write a command processing packet
void DataFlash_Class::Log_Write_Cmd(uint16_t cmd_total, const AP_Mission::Mission_Command& cmd)
{
if (cmd.id == MAV_CMD_DO_JUMP) {
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 : 0,
waypoint_param1 : cmd.content.jump.target,
waypoint_altitude : 0,
waypoint_latitude : cmd.content.jump.num_times,
waypoint_longitude : 0
};
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));
}
}