mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 15:38:29 -04:00
AP_Logger: added logging of GPS yaw
some GPS modules can give true yaw, whether moving or not
This commit is contained in:
parent
64ed76326b
commit
a2e75876bf
libraries/AP_Logger
@ -138,6 +138,10 @@ void AP_Logger::Write_GPS(uint8_t i, uint64_t time_us)
|
||||
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);
|
||||
|
||||
struct log_GPS pkt = {
|
||||
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GPS_MSG+i)),
|
||||
time_us : time_us,
|
||||
@ -152,6 +156,7 @@ void AP_Logger::Write_GPS(uint8_t i, uint64_t time_us)
|
||||
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));
|
||||
|
@ -212,6 +212,7 @@ struct PACKED log_GPS {
|
||||
float ground_speed;
|
||||
float ground_course;
|
||||
float vel_z;
|
||||
float yaw;
|
||||
uint8_t used;
|
||||
};
|
||||
|
||||
@ -1188,10 +1189,10 @@ struct PACKED log_Arm_Disarm {
|
||||
#define GPA_MULTS "FBBBB-CF"
|
||||
|
||||
// see "struct GPS_State" and "Write_GPS":
|
||||
#define GPS_LABELS "TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,U"
|
||||
#define GPS_FMT "QBIHBcLLefffB"
|
||||
#define GPS_UNITS "s---SmDUmnhn-"
|
||||
#define GPS_MULTS "F---0BGGB000-"
|
||||
#define GPS_LABELS "TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U"
|
||||
#define GPS_FMT "QBIHBcLLeffffB"
|
||||
#define GPS_UNITS "s---SmDUmnhnh-"
|
||||
#define GPS_MULTS "F---0BGGB000--"
|
||||
|
||||
#define GYR_LABELS "TimeUS,SampleUS,GyrX,GyrY,GyrZ"
|
||||
#define GYR_FMT "QQfff"
|
||||
|
Loading…
Reference in New Issue
Block a user