Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Logger: move gps logging structures into AP_GPS

Browse Source
This commit is contained in:
Peter Barker 2021-02-10 10:08:38 +11:00 committed by Peter Barker
parent c2cba52ad8
commit 1c4e1aab05
3 changed files with 7 additions and 264 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Logger/AP_Logger.h
View File

@ -290,7 +290,6 @@ public:
void Write_Event(LogEvent id);
void Write_Error(LogErrorSubsystem sub_system,
LogErrorCode error_code);
void Write_GPS(uint8_t instance);
void Write_IMU();
bool Write_ISBH(uint16_t seqno,
AP_InertialSensor::IMU_SENSOR_TYPE sensor_type,

53
libraries/AP_Logger/LogFile.cpp
View File

@ -6,7 +6,6 @@
#include <AP_Math/AP_Math.h>
#include <AP_Param/AP_Param.h>
#include <AP_RSSI/AP_RSSI.h>
#include <AP_GPS/AP_GPS.h>
#include "AP_Logger.h"
#include "AP_Logger_File.h"
@ -125,58 +124,6 @@ bool AP_Logger_Backend::Write_Parameter(const AP_Param *ap,
return Write_Parameter(name, ap->cast_to_float(type));
}
// Write an GPS packet
void AP_Logger::Write_GPS(uint8_t i)
{
const AP_GPS &gps = AP::gps();
const uint64_t time_us = AP_HAL::micros64();
const struct Location &loc = gps.location(i);
float yaw_deg=0, yaw_accuracy_deg=0;
gps.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)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,
altitude : loc.alt,
ground_speed : gps.ground_speed(i),
ground_course : gps.ground_course(i),
vel_z : gps.velocity(i).z,
yaw : yaw_deg,
used : (uint8_t)(gps.primary_sensor() == i)
};
WriteBlock(&pkt, sizeof(pkt));
/* write auxiliary accuracy information as well */
float hacc = 0, vacc = 0, sacc = 0;
gps.horizontal_accuracy(i, hacc);
gps.vertical_accuracy(i, vacc);
gps.speed_accuracy(i, sacc);
struct log_GPA pkt2{
LOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
time_us : time_us,
instance : i,
vdop : gps.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)gps.have_vertical_velocity(i),
sample_ms : gps.last_message_time_ms(i),
delta_ms : gps.last_message_delta_time_ms(i)
};
WriteBlock(&pkt2, sizeof(pkt2));
}
// Write an RCIN packet
void AP_Logger::Write_RCIN(void)
{

217
libraries/AP_Logger/LogStructure.h
View File

@ -120,6 +120,7 @@ const struct MultiplierStructure log_Multipliers[] = {
#include <AP_DAL/LogStructure.h>
#include <AP_NavEKF2/LogStructure.h>
#include <AP_NavEKF3/LogStructure.h>
#include <AP_GPS/LogStructure.h>
#include <AP_BattMonitor/LogStructure.h>
#include <AP_AHRS/LogStructure.h>
#include <AP_Camera/LogStructure.h>
@ -209,38 +210,6 @@ struct PACKED log_Error {
uint8_t error_code;
};
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;
};
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;
};
struct PACKED log_Message {
LOG_PACKET_HEADER;
@ -512,72 +481,6 @@ struct PACKED log_TERRAIN {
uint16_t loaded;
};
/*
UBlox logging
*/
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;
};
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;
};
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;
};
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;
};
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;
};
struct PACKED log_Esc {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -1106,75 +1009,6 @@ struct PACKED log_PSCZ {
// @Field: UnitIds: each character refers to a UNIT message. The unit at an offset corresponds to the field at the same offset in FMT.Format
// @Field: MultIds: each character refers to a MULT message. The multiplier at an offset corresponds to the field at the same offset in FMT.Format
// @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
// @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
// @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
// @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
// @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
// @LoggerMessage: GYR
// @Description: IMU gyroscope data
@ -1513,25 +1347,6 @@ struct PACKED log_PSCZ {
// @Field: SysID: system ID this data is for
// @Field: RTT: round trip time for this system
// @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
// @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
// @LoggerMessage: UNIT
// @Description: Message mapping from single character to SI unit
// @Field: TimeUS: Time since system startup
@ -1611,10 +1426,7 @@ struct PACKED log_PSCZ {
"MULT", "Qbd", "TimeUS,Id,Mult", "s--","F--" }, \
{ LOG_PARAMETER_MSG, sizeof(log_Parameter), \
"PARM", "QNf", "TimeUS,Name,Value", "s--", "F--" }, \
{ 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_STRUCTURE_FROM_GPS \
{ LOG_IMU_MSG, sizeof(log_IMU), \
"IMU", "QBffffffIIfBBHH", "TimeUS,I,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,EG,EA,T,GH,AH,GHz,AHz", "s#EEEooo--O--zz", "F-000000-----00" }, \
{ LOG_MESSAGE_MSG, sizeof(log_Message), \
@ -1665,16 +1477,6 @@ LOG_STRUCTURE_FROM_CAMERA \
"SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4", "sddhmDU????", "FBBB0GG????" }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
"TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded", "s-DU-mm--", "F-GG-00--" }, \
{ 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------------" }, \
{ LOG_ESC_MSG, sizeof(log_Esc), \
"ESC", "QBeCCcHcf", "TimeUS,Instance,RPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qvAO-O%", "F-BBBB-B-" }, \
{ LOG_CSRV_MSG, sizeof(log_CSRV), \
@ -1772,7 +1574,6 @@ enum LogMessages : uint8_t {
LOG_PARAMETER_MSG = 64,
LOG_IDS_FROM_NAVEKF2,
LOG_IDS_FROM_NAVEKF3,
LOG_GPS_MSG,
LOG_IMU_MSG,
LOG_MESSAGE_MSG,
LOG_RCIN_MSG,
@ -1789,25 +1590,22 @@ enum LogMessages : uint8_t {
LOG_ATRP_MSG,
LOG_IDS_FROM_CAMERA,
LOG_TERRAIN_MSG,
LOG_GPS_UBX1_MSG,
LOG_GPS_UBX2_MSG,
LOG_ESC_MSG,
LOG_CSRV_MSG,
LOG_CESC_MSG,
LOG_ARSP_MSG,
LOG_IDS_FROM_BATTMONITOR,
LOG_MAG_MSG,
LOG_MODE_MSG,
LOG_GPS_RAW_MSG,
// LOG_GPS_RAWH_MSG is used as a check for duplicates. Do not add between this and LOG_FORMAT_MSG
LOG_GPS_RAWH_MSG,
LOG_IDS_FROM_GPS,
// LOG_MODE_MSG is used as a check for duplicates. Do not add between this and LOG_FORMAT_MSG
LOG_MODE_MSG,
LOG_FORMAT_MSG = 128, // this must remain #128
LOG_IDS_FROM_DAL,
LOG_GPS_RAWS_MSG,
LOG_ACC_MSG,
LOG_GYR_MSG,
LOG_PIDR_MSG,
@ -1818,7 +1616,6 @@ enum LogMessages : uint8_t {
LOG_DSTL_MSG,
LOG_VIBE_MSG,
LOG_RPM_MSG,
LOG_GPA_MSG,
LOG_RFND_MSG,
LOG_MAV_STATS,
LOG_FORMAT_UNITS_MSG,
@ -1861,7 +1658,7 @@ enum LogMessages : uint8_t {
};
static_assert(_LOG_LAST_MSG_ <= 255, "Too many message formats");
static_assert(LOG_GPS_RAWH_MSG < 128, "Duplicate message format IDs");
static_assert(LOG_MODE_MSG < 128, "Duplicate message format IDs");
enum LogOriginType {
ekf_origin = 0,