zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Logger: move gps logging structures into AP_GPS

zr-v5.1
Peter Barker 4 years ago committed by Peter Barker
parent
c2cba52ad8
commit
1c4e1aab05
3 changed files with 7 additions and 264 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      libraries/AP_Logger/AP_Logger.h
  2. 53
      libraries/AP_Logger/LogFile.cpp
  3. 217
      libraries/AP_Logger/LogStructure.h

1
libraries/AP_Logger/AP_Logger.h
Unescape View File

@ -290,7 +290,6 @@ public: @@ -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
Unescape View File

@ -6,7 +6,6 @@ @@ -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, @@ -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
Unescape View File

@ -120,6 +120,7 @@ const struct MultiplierStructure log_Multipliers[] = { @@ -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 { @@ -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 { @@ -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 { @@ -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 { @@ -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 { @@ -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 \ @@ -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 { @@ -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 { @@ -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 { @@ -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 { @@ -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,

Write Preview
Loading…
Cancel
Save