Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DAL: re-pack GPS replay data 

this fixes antenna offset for blended GPS, and reduces the average
amount of data in logs for GPS replay
This commit is contained in:
Andrew Tridgell 2020-11-08 08:59:56 +11:00
parent cb830dcc11
commit 5be818f8a1
3 changed files with 49 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_DAL/AP_DAL_GPS.cpp
View File

@ -23,30 +23,31 @@ void AP_DAL_GPS::start_frame()
for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) {
log_RGPI &RGPI = _RGPI[i];
const log_RGPI old_RGPI = RGPI;
RGPI.status = (GPS_Status)gps.status(i);
const Location &loc = gps.location(i);
RGPI.last_message_time_ms = gps.last_message_time_ms(i);
RGPI.lat = loc.lat;
RGPI.lng = loc.lng;
RGPI.alt = loc.alt;
RGPI.have_vertical_velocity = gps.have_vertical_velocity(i);
RGPI.horizontal_accuracy_returncode = gps.horizontal_accuracy(i, RGPI.hacc);
RGPI.vertical_accuracy_returncode = gps.vertical_accuracy(i, RGPI.vacc);
RGPI.hdop = gps.get_hdop(i);
RGPI.num_sats = gps.num_sats(i);
RGPI.get_lag_returncode = gps.get_lag(i, RGPI.lag_sec);
WRITE_REPLAY_BLOCK_IFCHANGED(RGPI, RGPI, old_RGPI);
log_RGPJ &RGPJ = _RGPJ[i];
const log_RGPI old_RGPI = RGPI;
const log_RGPJ old_RGPJ = RGPJ;
RGPI.status = (GPS_Status)gps.status(i);
RGPI.antenna_offset = gps.get_antenna_offset(i);
const Location &loc = gps.location(i);
RGPJ.last_message_time_ms = gps.last_message_time_ms(i);
RGPJ.lat = loc.lat;
RGPJ.lng = loc.lng;
RGPJ.alt = loc.alt;
RGPI.have_vertical_velocity = gps.have_vertical_velocity(i);
RGPI.horizontal_accuracy_returncode = gps.horizontal_accuracy(i, RGPJ.hacc);
RGPI.vertical_accuracy_returncode = gps.vertical_accuracy(i, RGPJ.vacc);
RGPJ.hdop = gps.get_hdop(i);
RGPI.num_sats = gps.num_sats(i);
RGPI.get_lag_returncode = gps.get_lag(i, RGPI.lag_sec);
RGPJ.velocity = gps.velocity(i);
RGPJ.speed_accuracy_returncode = gps.speed_accuracy(i, RGPJ.sacc);
RGPJ.gps_yaw_deg_returncode = gps.gps_yaw_deg(i, RGPJ.yaw_deg, RGPJ.yaw_accuracy_deg);
RGPI.speed_accuracy_returncode = gps.speed_accuracy(i, RGPJ.sacc);
RGPI.gps_yaw_deg_returncode = gps.gps_yaw_deg(i, RGPJ.yaw_deg, RGPJ.yaw_accuracy_deg);
WRITE_REPLAY_BLOCK_IFCHANGED(RGPI, RGPI, old_RGPI);
WRITE_REPLAY_BLOCK_IFCHANGED(RGPJ, RGPJ, old_RGPJ);
// also fetch antenna offset for this frame

21
libraries/AP_DAL/AP_DAL_GPS.h
View File

@ -32,9 +32,9 @@ public:
}
const Location &location(uint8_t instance) const {
Location &loc = tmp_location[instance];
loc.lat = _RGPI[instance].lat;
loc.lng = _RGPI[instance].lng;
loc.alt = _RGPI[instance].alt;
loc.lat = _RGPJ[instance].lat;
loc.lng = _RGPJ[instance].lng;
loc.alt = _RGPJ[instance].alt;
return loc;
}
bool have_vertical_velocity(uint8_t instance) const {
@ -44,7 +44,7 @@ public:
return have_vertical_velocity(primary_sensor());
}
bool horizontal_accuracy(uint8_t instance, float &hacc) const {
hacc = _RGPI[instance].hacc;
hacc = _RGPJ[instance].hacc;
return _RGPI[instance].horizontal_accuracy_returncode;
}
bool horizontal_accuracy(float &hacc) const {
@ -52,7 +52,7 @@ public:
}
bool vertical_accuracy(uint8_t instance, float &vacc) const {
vacc = _RGPI[instance].vacc;
vacc = _RGPJ[instance].vacc;
return _RGPI[instance].vertical_accuracy_returncode;
}
bool vertical_accuracy(float &vacc) const {
@ -60,14 +60,14 @@ public:
}
uint16_t get_hdop(uint8_t instance) const {
return _RGPI[instance].hdop;
return _RGPJ[instance].hdop;
}
uint16_t get_hdop() const {
return get_hdop(primary_sensor());
}
uint32_t last_message_time_ms(uint8_t instance) const {
return _RGPI[instance].last_message_time_ms;
return _RGPJ[instance].last_message_time_ms;
}
uint8_t num_sats(uint8_t instance) const {
@ -94,7 +94,7 @@ public:
bool speed_accuracy(uint8_t instance, float &sacc) const {
sacc = _RGPJ[instance].sacc;
return _RGPJ[instance].speed_accuracy_returncode;
return _RGPI[instance].speed_accuracy_returncode;
}
bool speed_accuracy(float &sacc) const {
return speed_accuracy(primary_sensor(), sacc);
@ -107,7 +107,7 @@ public:
bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg) const {
yaw_deg = _RGPJ[instance].yaw_deg;
accuracy_deg = _RGPJ[instance].yaw_accuracy_deg;
return _RGPJ[instance].gps_yaw_deg_returncode;
return _RGPI[instance].gps_yaw_deg_returncode;
}
uint8_t num_sensors(void) const {
@ -125,7 +125,7 @@ public:
// return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin
const Vector3f &get_antenna_offset(uint8_t instance) const {
return antenna_offset[instance];
return _RGPI[instance].antenna_offset;
}
void start_frame();
@ -136,7 +136,6 @@ public:
}
void handle_message(const log_RGPI &msg) {
_RGPI[msg.instance] = msg;
antenna_offset[msg.instance] = AP::gps().get_antenna_offset(msg.instance);
}
void handle_message(const log_RGPJ &msg) {
_RGPJ[msg.instance] = msg;

35
libraries/AP_DAL/LogStructure.h
View File

@ -198,35 +198,36 @@ struct log_RGPH {
};
// @LoggerMessage: RGPI
// @Description: Replay Data GPS Instance
// @Description: Replay Data GPS Instance, infrequently changing data
struct log_RGPI {
uint32_t last_message_time_ms;
int32_t lat;
int32_t lng;
int32_t alt;
float hacc;
float vacc;
Vector3f antenna_offset;
float lag_sec;
uint16_t hdop;
uint8_t have_vertical_velocity:1;
uint8_t horizontal_accuracy_returncode:1;
uint8_t vertical_accuracy_returncode:1;
uint8_t get_lag_returncode:1;
uint8_t speed_accuracy_returncode:1;
uint8_t gps_yaw_deg_returncode:1;
uint8_t status;
uint8_t have_vertical_velocity;
uint8_t horizontal_accuracy_returncode;
uint8_t vertical_accuracy_returncode;
uint8_t num_sats;
uint8_t get_lag_returncode;
uint8_t instance;
uint8_t _end;
};
// @LoggerMessage: RGPJ
// @Description: Replay Data GPS Instance - more data
// @Description: Replay Data GPS Instance - rapidly changing data
struct log_RGPJ {
uint32_t last_message_time_ms;
Vector3f velocity;
uint32_t speed_accuracy_returncode;
float sacc;
float yaw_deg;
float yaw_accuracy_deg;
uint8_t gps_yaw_deg_returncode;
int32_t lat;
int32_t lng;
int32_t alt;
float hacc;
float vacc;
uint16_t hdop;
uint8_t instance;
uint8_t _end;
};
@ -410,9 +411,9 @@ struct log_RBOH {
{ LOG_RGPH_MSG, RLOG_SIZE(RGPH), \
"RGPH", "BB", "NumInst,Primary", "--", "--" }, \
{ LOG_RGPI_MSG, RLOG_SIZE(RGPI), \
"RGPI", "IiiifffHBBBBBBB", "LMT,lat,lon,alt,ha,va,lg,hdp,st,hvv,harc,varc,ns,lgrc,I", "--------------#", "---------------" }, \
"RGPI", "ffffBBBB", "OX,OY,OZ,Lg,Flags,Stat,NSats,I", "-------#", "--------" }, \
{ LOG_RGPJ_MSG, RLOG_SIZE(RGPJ), \
"RGPJ", "fffIfffBB", "vx,vy,vz,sarc,sa,yd,yda,ydrc,I", "--------#", "---------" }, \
"RGPJ", "IffffffiiiffHB", "TS,VX,VY,VZ,SA,Y,YA,Lat,Lon,Alt,HA,VA,HD,I", "-------------#", "--------------" }, \
{ LOG_RMGH_MSG, RLOG_SIZE(RMGH), \
"RMGH", "BBfBBB", "Dec,NumInst,AutoDec,NumEna,LOE,C", "------", "------" }, \
{ LOG_RMGI_MSG, RLOG_SIZE(RMGI), \