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

Dataflash: log rally points

Browse Source
This commit is contained in:
Randy Mackay 2016-07-04 11:14:26 +09:00
parent 11975223dd
commit 032e834849
3 changed files with 36 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/DataFlash/DataFlash.h
View File

@ -21,6 +21,7 @@
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <DataFlash/LogStructure.h>
#include <AP_Motors/AP_Motors.h>
#include <AP_Rally/AP_Rally.h>
#include <stdint.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
@ -149,6 +150,7 @@ public:
const AP_Motors &motors,
const AC_AttitudeControl &attitude_control,
const AC_PosControl &pos_control);
void Log_Write_Rally(const AP_Rally &rally);
void Log_Write(const char *name, const char *labels, const char *fmt, ...);

20
libraries/DataFlash/LogFile.cpp
View File

@ -1877,3 +1877,23 @@ void DataFlash_Class::Log_Write_Rate(const AP_AHRS &ahrs,
};
WriteBlock(&pkt_rate, sizeof(pkt_rate));
}
// Write rally points
void DataFlash_Class::Log_Write_Rally(const AP_Rally &rally)
{
RallyLocation rally_point;
for (uint8_t i=0; i<rally.get_rally_total(); i++) {
if (rally.get_rally_point_with_index(i, rally_point)) {
struct log_Rally pkt_rally = {
LOG_PACKET_HEADER_INIT(LOG_RALLY_MSG),
time_us : AP_HAL::micros64(),
total : rally.get_rally_total(),
sequence : i,
latitude : rally_point.lat,
longitude : rally_point.lng,
altitude : rally_point.alt
};
WriteBlock(&pkt_rally, sizeof(pkt_rally));
}
}
}

15
libraries/DataFlash/LogStructure.h
View File

@ -686,6 +686,16 @@ struct PACKED log_SbpRAW2 {
uint8_t data2[192];
};
struct PACKED log_Rally {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t total;
uint8_t sequence;
int32_t latitude;
int32_t longitude;
int16_t altitude;
};
// #endif // SBP_HW_LOGGING
/*
@ -887,7 +897,9 @@ Format characters in the format string for binary log messages
{ LOG_GIMBAL3_MSG, sizeof(log_Gimbal3), \
"GMB3", "Ihhh", "TimeMS,rl_torque_cmd,el_torque_cmd,az_torque_cmd" }, \
{ LOG_RATE_MSG, sizeof(log_Rate), \
"RATE", "Qffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut" }
"RATE", "Qffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut" }, \
{ LOG_RALLY_MSG, sizeof(log_Rally), \
"RALY", "QBBLLh", "TimeUS,Tot,Seq,Lat,Lng,Alt" }
// #if SBP_HW_LOGGING
#define LOG_SBP_STRUCTURES \
@ -1002,6 +1014,7 @@ enum LogMessages {
LOG_GIMBAL2_MSG,
LOG_GIMBAL3_MSG,
LOG_RATE_MSG,
LOG_RALLY_MSG,
};
enum LogOriginType {