mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_GPS: move gps logging structures into AP_GPS
This commit is contained in:
Peter Barker
Peter Barker
parent
82a84c8f35
commit
c2cba52ad8
@ -858,7 +858,7 @@ void AP_GPS::update_instance(uint8_t instance)
|
|||||||
|
|
||||||
#ifndef HAL_BUILD_AP_PERIPH
|
#ifndef HAL_BUILD_AP_PERIPH
|
||||||
if (data_should_be_logged && should_log()) {
|
if (data_should_be_logged && should_log()) {
|
||||||
AP::logger().Write_GPS(instance);
|
Write_GPS(instance);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (state[instance].status >= GPS_OK_FIX_3D) {
|
if (state[instance].status >= GPS_OK_FIX_3D) {
|
||||||
@ -1845,7 +1845,7 @@ void AP_GPS::calc_blended_state(void)
|
|||||||
#ifndef HAL_BUILD_AP_PERIPH
|
#ifndef HAL_BUILD_AP_PERIPH
|
||||||
if (timing[GPS_BLENDED_INSTANCE].last_message_time_ms > last_blended_message_time_ms &&
|
if (timing[GPS_BLENDED_INSTANCE].last_message_time_ms > last_blended_message_time_ms &&
|
||||||
should_log()) {
|
should_log()) {
|
||||||
AP::logger().Write_GPS(GPS_BLENDED_INSTANCE);
|
Write_GPS(GPS_BLENDED_INSTANCE);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
@ -1917,6 +1917,57 @@ uint32_t AP_GPS::get_itow(uint8_t instance) const
|
|||||||
return drivers[instance]->get_last_itow();
|
return drivers[instance]->get_last_itow();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Logging support:
|
||||||
|
// Write an GPS packet
|
||||||
|
void AP_GPS::Write_GPS(uint8_t i)
|
||||||
|
{
|
||||||
|
const uint64_t time_us = AP_HAL::micros64();
|
||||||
|
const struct Location &loc = location(i);
|
||||||
|
|
||||||
|
float yaw_deg=0, yaw_accuracy_deg=0;
|
||||||
|
gps_yaw_deg(i, yaw_deg, yaw_accuracy_deg);
|
||||||
|
|
||||||
|
const struct log_GPS pkt {
|
||||||
|
LOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
|
||||||
|
time_us : time_us,
|
||||||
|
instance : i,
|
||||||
|
status : (uint8_t)status(i),
|
||||||
|
gps_week_ms : time_week_ms(i),
|
||||||
|
gps_week : time_week(i),
|
||||||
|
num_sats : num_sats(i),
|
||||||
|
hdop : get_hdop(i),
|
||||||
|
latitude : loc.lat,
|
||||||
|
longitude : loc.lng,
|
||||||
|
altitude : loc.alt,
|
||||||
|
ground_speed : ground_speed(i),
|
||||||
|
ground_course : ground_course(i),
|
||||||
|
vel_z : velocity(i).z,
|
||||||
|
yaw : yaw_deg,
|
||||||
|
used : (uint8_t)(AP::gps().primary_sensor() == i)
|
||||||
|
};
|
||||||
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||||
|
|
||||||
|
/* write auxiliary accuracy information as well */
|
||||||
|
float hacc = 0, vacc = 0, sacc = 0;
|
||||||
|
horizontal_accuracy(i, hacc);
|
||||||
|
vertical_accuracy(i, vacc);
|
||||||
|
speed_accuracy(i, sacc);
|
||||||
|
struct log_GPA pkt2{
|
||||||
|
LOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
|
||||||
|
time_us : time_us,
|
||||||
|
instance : i,
|
||||||
|
vdop : get_vdop(i),
|
||||||
|
hacc : (uint16_t)MIN((hacc*100), UINT16_MAX),
|
||||||
|
vacc : (uint16_t)MIN((vacc*100), UINT16_MAX),
|
||||||
|
sacc : (uint16_t)MIN((sacc*100), UINT16_MAX),
|
||||||
|
yaw_accuracy : yaw_accuracy_deg,
|
||||||
|
have_vv : (uint8_t)have_vertical_velocity(i),
|
||||||
|
sample_ms : last_message_time_ms(i),
|
||||||
|
delta_ms : last_message_delta_time_ms(i)
|
||||||
|
};
|
||||||
|
AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
|
||||||
|
}
|
||||||
|
|
||||||
namespace AP {
|
namespace AP {
|
||||||
|
|
||||||
AP_GPS &gps()
|
AP_GPS &gps()
|
||||||
|
|||||||
@ -676,6 +676,10 @@ private:
|
|||||||
|
|
||||||
// used for flight testing with GPS yaw loss
|
// used for flight testing with GPS yaw loss
|
||||||
bool _force_disable_gps_yaw;
|
bool _force_disable_gps_yaw;
|
||||||
|
|
||||||
|
// logging support
|
||||||
|
void Write_GPS(uint8_t instance);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
namespace AP {
|
namespace AP {
|
||||||
|
|||||||
211
libraries/AP_GPS/LogStructure.h
Normal file
211
libraries/AP_GPS/LogStructure.h
Normal file
@ -0,0 +1,211 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <AP_Logger/LogStructure.h>
|
||||||
|
|
||||||
|
#define LOG_IDS_FROM_GPS \
|
||||||
|
LOG_GPS_MSG, \
|
||||||
|
LOG_GPA_MSG, \
|
||||||
|
LOG_GPS_RAW_MSG, \
|
||||||
|
LOG_GPS_RAWH_MSG, \
|
||||||
|
LOG_GPS_RAWS_MSG, \
|
||||||
|
LOG_GPS_UBX1_MSG, \
|
||||||
|
LOG_GPS_UBX2_MSG
|
||||||
|
|
||||||
|
|
||||||
|
// @LoggerMessage: GPS
|
||||||
|
// @Description: Information received from GNSS systems attached to the autopilot
|
||||||
|
// @Field: TimeUS: Time since system startup
|
||||||
|
// @Field: I: GPS instance number
|
||||||
|
// @Field: Status: GPS Fix type; 2D fix, 3D fix etc.
|
||||||
|
// @Field: GMS: milliseconds since start of GPS Week
|
||||||
|
// @Field: GWk: weeks since 5 Jan 1980
|
||||||
|
// @Field: NSats: number of satellites visible
|
||||||
|
// @Field: HDop: horizontal precision
|
||||||
|
// @Field: Lat: latitude
|
||||||
|
// @Field: Lng: longitude
|
||||||
|
// @Field: Alt: altitude
|
||||||
|
// @Field: Spd: ground speed
|
||||||
|
// @Field: GCrs: ground course
|
||||||
|
// @Field: VZ: vertical speed
|
||||||
|
// @Field: Yaw: vehicle yaw
|
||||||
|
// @Field: U: boolean value indicating whether this GPS is in use
|
||||||
|
struct PACKED log_GPS {
|
||||||
|
LOG_PACKET_HEADER;
|
||||||
|
uint64_t time_us;
|
||||||
|
uint8_t instance;
|
||||||
|
uint8_t status;
|
||||||
|
uint32_t gps_week_ms;
|
||||||
|
uint16_t gps_week;
|
||||||
|
uint8_t num_sats;
|
||||||
|
uint16_t hdop;
|
||||||
|
int32_t latitude;
|
||||||
|
int32_t longitude;
|
||||||
|
int32_t altitude;
|
||||||
|
float ground_speed;
|
||||||
|
float ground_course;
|
||||||
|
float vel_z;
|
||||||
|
float yaw;
|
||||||
|
uint8_t used;
|
||||||
|
};
|
||||||
|
|
||||||
|
// @LoggerMessage: GPA
|
||||||
|
// @Description: GPS accuracy information
|
||||||
|
// @Field: I: GPS instance number
|
||||||
|
// @Field: TimeUS: Time since system startup
|
||||||
|
// @Field: VDop: vertical degree of procession
|
||||||
|
// @Field: HAcc: horizontal position accuracy
|
||||||
|
// @Field: VAcc: vertical position accuracy
|
||||||
|
// @Field: SAcc: speed accuracy
|
||||||
|
// @Field: YAcc: yaw accuracy
|
||||||
|
// @Field: VV: true if vertical velocity is available
|
||||||
|
// @Field: SMS: time since system startup this sample was taken
|
||||||
|
// @Field: Delta: system time delta between the last two reported positions
|
||||||
|
struct PACKED log_GPA {
|
||||||
|
LOG_PACKET_HEADER;
|
||||||
|
uint64_t time_us;
|
||||||
|
uint8_t instance;
|
||||||
|
uint16_t vdop;
|
||||||
|
uint16_t hacc;
|
||||||
|
uint16_t vacc;
|
||||||
|
uint16_t sacc;
|
||||||
|
float yaw_accuracy;
|
||||||
|
uint8_t have_vv;
|
||||||
|
uint32_t sample_ms;
|
||||||
|
uint16_t delta_ms;
|
||||||
|
};
|
||||||
|
|
||||||
|
/*
|
||||||
|
UBlox logging
|
||||||
|
*/
|
||||||
|
|
||||||
|
// @LoggerMessage: UBX1
|
||||||
|
// @Description: uBlox-specific GPS information (part 1)
|
||||||
|
// @Field: TimeUS: Time since system startup
|
||||||
|
// @Field: Instance: GPS instance number
|
||||||
|
// @Field: noisePerMS: noise level as measured by GPS
|
||||||
|
// @Field: jamInd: jamming indicator; higher is more likely jammed
|
||||||
|
// @Field: aPower: antenna power indicator; 2 is don't know
|
||||||
|
// @Field: agcCnt: automatic gain control monitor
|
||||||
|
// @Field: config: bitmask for messages which haven't been seen
|
||||||
|
struct PACKED log_Ubx1 {
|
||||||
|
LOG_PACKET_HEADER;
|
||||||
|
uint64_t time_us;
|
||||||
|
uint8_t instance;
|
||||||
|
uint16_t noisePerMS;
|
||||||
|
uint8_t jamInd;
|
||||||
|
uint8_t aPower;
|
||||||
|
uint16_t agcCnt;
|
||||||
|
uint32_t config;
|
||||||
|
};
|
||||||
|
|
||||||
|
// @LoggerMessage: UBX2
|
||||||
|
// @Description: uBlox-specific GPS information (part 2)
|
||||||
|
// @Field: TimeUS: Time since system startup
|
||||||
|
// @Field: Instance: GPS instance number
|
||||||
|
// @Field: ofsI: imbalance of I part of complex signal
|
||||||
|
// @Field: magI: magnitude of I part of complex signal
|
||||||
|
// @Field: ofsQ: imbalance of Q part of complex signal
|
||||||
|
// @Field: magQ: magnitude of Q part of complex signal
|
||||||
|
struct PACKED log_Ubx2 {
|
||||||
|
LOG_PACKET_HEADER;
|
||||||
|
uint64_t time_us;
|
||||||
|
uint8_t instance;
|
||||||
|
int8_t ofsI;
|
||||||
|
uint8_t magI;
|
||||||
|
int8_t ofsQ;
|
||||||
|
uint8_t magQ;
|
||||||
|
};
|
||||||
|
|
||||||
|
// @LoggerMessage: GRAW
|
||||||
|
// @Description: Raw uBlox data
|
||||||
|
// @Field: TimeUS: Time since system startup
|
||||||
|
// @Field: WkMS: receiver TimeOfWeek measurement
|
||||||
|
// @Field: Week: GPS week
|
||||||
|
// @Field: numSV: number of space vehicles seen
|
||||||
|
// @Field: sv: space vehicle number of first vehicle
|
||||||
|
// @Field: cpMes: carrier phase measurement
|
||||||
|
// @Field: prMes: pseudorange measurement
|
||||||
|
// @Field: doMes: Doppler measurement
|
||||||
|
// @Field: mesQI: measurement quality index
|
||||||
|
// @Field: cno: carrier-to-noise density ratio
|
||||||
|
// @Field: lli: loss of lock indicator
|
||||||
|
struct PACKED log_GPS_RAW {
|
||||||
|
LOG_PACKET_HEADER;
|
||||||
|
uint64_t time_us;
|
||||||
|
int32_t iTOW;
|
||||||
|
int16_t week;
|
||||||
|
uint8_t numSV;
|
||||||
|
uint8_t sv;
|
||||||
|
double cpMes;
|
||||||
|
double prMes;
|
||||||
|
float doMes;
|
||||||
|
int8_t mesQI;
|
||||||
|
int8_t cno;
|
||||||
|
uint8_t lli;
|
||||||
|
};
|
||||||
|
|
||||||
|
// @LoggerMessage: GRXH
|
||||||
|
// @Description: Raw uBlox data - header
|
||||||
|
// @Field: TimeUS: Time since system startup
|
||||||
|
// @Field: rcvTime: receiver TimeOfWeek measurement
|
||||||
|
// @Field: week: GPS week
|
||||||
|
// @Field: leapS: GPS leap seconds
|
||||||
|
// @Field: numMeas: number of space-vehicle measurements to follow
|
||||||
|
// @Field: recStat: receiver tracking status bitfield
|
||||||
|
struct PACKED log_GPS_RAWH {
|
||||||
|
LOG_PACKET_HEADER;
|
||||||
|
uint64_t time_us;
|
||||||
|
double rcvTow;
|
||||||
|
uint16_t week;
|
||||||
|
int8_t leapS;
|
||||||
|
uint8_t numMeas;
|
||||||
|
uint8_t recStat;
|
||||||
|
};
|
||||||
|
|
||||||
|
// @LoggerMessage: GRXS
|
||||||
|
// @Description: Raw uBlox data - space-vehicle data
|
||||||
|
// @Field: TimeUS: Time since system startup
|
||||||
|
// @Field: prMes: Pseudorange measurement
|
||||||
|
// @Field: cpMes: Carrier phase measurement
|
||||||
|
// @Field: doMes: Doppler measurement
|
||||||
|
// @Field: gnss: GNSS identifier
|
||||||
|
// @Field: sv: Satellite identifier
|
||||||
|
// @Field: freq: GLONASS frequency slot
|
||||||
|
// @Field: lock: carrier phase locktime counter
|
||||||
|
// @Field: cno: carrier-to-noise density ratio
|
||||||
|
// @Field: prD: estimated pseudorange measurement standard deviation
|
||||||
|
// @Field: cpD: estimated carrier phase measurement standard deviation
|
||||||
|
// @Field: doD: estimated Doppler measurement standard deviation
|
||||||
|
// @Field: trk: tracking status bitfield
|
||||||
|
struct PACKED log_GPS_RAWS {
|
||||||
|
LOG_PACKET_HEADER;
|
||||||
|
uint64_t time_us;
|
||||||
|
double prMes;
|
||||||
|
double cpMes;
|
||||||
|
float doMes;
|
||||||
|
uint8_t gnssId;
|
||||||
|
uint8_t svId;
|
||||||
|
uint8_t freqId;
|
||||||
|
uint16_t locktime;
|
||||||
|
uint8_t cno;
|
||||||
|
uint8_t prStdev;
|
||||||
|
uint8_t cpStdev;
|
||||||
|
uint8_t doStdev;
|
||||||
|
uint8_t trkStat;
|
||||||
|
};
|
||||||
|
|
||||||
|
#define LOG_STRUCTURE_FROM_GPS \
|
||||||
|
{ LOG_GPS_MSG, sizeof(log_GPS), \
|
||||||
|
"GPS", "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#---SmDUmnhnh-", "F----0BGGB000--" }, \
|
||||||
|
{ LOG_GPA_MSG, sizeof(log_GPA), \
|
||||||
|
"GPA", "QBCCCCfBIH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta", "s#mmmnd-ss", "F-BBBB0-CC" }, \
|
||||||
|
{ LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
|
||||||
|
"UBX1", "QBHBBHI", "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt,config", "s#-----", "F------" }, \
|
||||||
|
{ LOG_GPS_UBX2_MSG, sizeof(log_Ubx2), \
|
||||||
|
"UBX2", "QBbBbB", "TimeUS,Instance,ofsI,magI,ofsQ,magQ", "s#----", "F-----" }, \
|
||||||
|
{ LOG_GPS_RAW_MSG, sizeof(log_GPS_RAW), \
|
||||||
|
"GRAW", "QIHBBddfBbB", "TimeUS,WkMS,Week,numSV,sv,cpMes,prMes,doMes,mesQI,cno,lli", "s--S-------", "F--0-------" }, \
|
||||||
|
{ LOG_GPS_RAWH_MSG, sizeof(log_GPS_RAWH), \
|
||||||
|
"GRXH", "QdHbBB", "TimeUS,rcvTime,week,leapS,numMeas,recStat", "s-----", "F-----" }, \
|
||||||
|
{ LOG_GPS_RAWS_MSG, sizeof(log_GPS_RAWS), \
|
||||||
|
"GRXS", "QddfBBBHBBBBB", "TimeUS,prMes,cpMes,doMes,gnss,sv,freq,lock,cno,prD,cpD,doD,trk", "s------------", "F------------" },
|
||||||
Loading…
Reference in New Issue
Block a user