diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 608e85badf..065eecefc3 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -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, diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 77b0080120..32b1301575 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -6,7 +6,6 @@ #include #include #include -#include #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) { diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index abe3fae164..ed0149bc50 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -120,6 +120,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include #include #include #include @@ -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,