Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: added separate logging of each GPS

Browse Source
This commit is contained in:
Andrew Tridgell 2014-04-10 10:29:30 +10:00
parent a821b0e198
commit 95304e27c9
2 changed files with 32 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/DataFlash/DataFlash.h
View File

@ -48,7 +48,7 @@ public:
void EnableWrites(bool enable) { _writes_enabled = enable; }
void Log_Write_Format(const struct LogStructure *structure);
void Log_Write_Parameter(const char *name, float value);
void Log_Write_GPS(const AP_GPS &gps, int32_t relative_alt);
void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, int32_t relative_alt);
void Log_Write_IMU(const AP_InertialSensor &ins);
void Log_Write_RCIN(void);
void Log_Write_RCOUT(void);

60
libraries/DataFlash/LogFile.cpp
View File

@ -662,42 +662,44 @@ void DataFlash_Class::Log_Write_Parameters(void)
// Write an GPS packet
void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, int32_t relative_alt)
void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i, int32_t relative_alt)
{
const struct Location &loc = gps.location(0);
struct log_GPS pkt = {
LOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
status : (uint8_t)gps.status(0),
gps_week_ms : gps.time_week_ms(0),
gps_week : gps.time_week(0),
num_sats : gps.num_sats(0),
hdop : gps.get_hdop(0),
latitude : loc.lat,
longitude : loc.lng,
rel_altitude : relative_alt,
altitude : loc.alt,
ground_speed : (uint32_t)(gps.ground_speed(0) * 100),
ground_course : gps.ground_course_cd(0),
vel_z : gps.velocity(0).z,
apm_time : hal.scheduler->millis()
};
WriteBlock(&pkt, sizeof(pkt));
if (i == 0) {
const struct Location &loc = gps.location(i);
struct log_GPS pkt = {
LOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
status : (uint8_t)gps.status(i),
gps_week_ms : gps.time_week_ms(i),
gps_week : gps.time_week(i),
num_sats : gps.num_sats(i),
hdop : gps.get_hdop(i),
latitude : loc.lat,
longitude : loc.lng,
rel_altitude : relative_alt,
altitude : loc.alt,
ground_speed : (uint32_t)(gps.ground_speed(i) * 100),
ground_course : gps.ground_course_cd(i),
vel_z : gps.velocity(i).z,
apm_time : hal.scheduler->millis()
};
WriteBlock(&pkt, sizeof(pkt));
}
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
if (gps.num_sensors() > 1) {
const struct Location &loc2 = gps.location(1);
if (i > 0) {
const struct Location &loc2 = gps.location(i);
struct log_GPS2 pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_GPS2_MSG),
status : (uint8_t)gps.status(1),
gps_week_ms : gps.time_week_ms(1),
gps_week : gps.time_week(1),
num_sats : gps.num_sats(1),
hdop : gps.get_hdop(1),
status : (uint8_t)gps.status(i),
gps_week_ms : gps.time_week_ms(i),
gps_week : gps.time_week(i),
num_sats : gps.num_sats(i),
hdop : gps.get_hdop(i),
latitude : loc2.lat,
longitude : loc2.lng,
altitude : loc2.alt,
ground_speed : (uint32_t)(gps.ground_speed(1)*100),
ground_course : gps.ground_course_cd(1),
vel_z : gps.velocity(1).z,
ground_speed : (uint32_t)(gps.ground_speed(i)*100),
ground_course : gps.ground_course_cd(i),
vel_z : gps.velocity(i).z,
apm_time : hal.scheduler->millis(),
dgps_numch : 0,
dgps_age : 0