diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index aa85556cfe..6f646063f8 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -858,7 +858,7 @@ void AP_GPS::update_instance(uint8_t instance) #ifndef HAL_BUILD_AP_PERIPH if (data_should_be_logged && should_log()) { - AP::logger().Write_GPS(instance); + Write_GPS(instance); } if (state[instance].status >= GPS_OK_FIX_3D) { @@ -1845,7 +1845,7 @@ void AP_GPS::calc_blended_state(void) #ifndef HAL_BUILD_AP_PERIPH if (timing[GPS_BLENDED_INSTANCE].last_message_time_ms > last_blended_message_time_ms && should_log()) { - AP::logger().Write_GPS(GPS_BLENDED_INSTANCE); + Write_GPS(GPS_BLENDED_INSTANCE); } #endif } @@ -1917,6 +1917,57 @@ uint32_t AP_GPS::get_itow(uint8_t instance) const 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 { AP_GPS &gps() diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 7e27311266..a52a090e2a 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -676,6 +676,10 @@ private: // used for flight testing with GPS yaw loss bool _force_disable_gps_yaw; + + // logging support + void Write_GPS(uint8_t instance); + }; namespace AP { diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h new file mode 100644 index 0000000000..9718f0cf18 --- /dev/null +++ b/libraries/AP_GPS/LogStructure.h @@ -0,0 +1,211 @@ +#pragma once + +#include + +#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------------" },