Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_DAL: move gyro bias metadata to new RISJ message

Browse Source 
These fields are constant per-instance, so logging them in RISI
inflated the replay log on every frame and broke replay of logs
captured before this PR landed (RISI gained two floats).

Restore RISI to its original layout and add RISJ, which carries
gyro_bias_limit and gyro_bias_init_dps plus the instance and is
written only when changed via WRITE_REPLAY_BLOCK_IFCHANGED.

Seed _RISJ[] with the legacy defaults (0.5 rad/s clamp,
2.5 deg/s init) in the DAL constructor so replays of older logs
that have no RISJ messages still feed sane values to the EKF.
This commit is contained in:
Andy Piper
2026-05-13 18:29:03 +10:00
committed by Peter Barker
parent 2af1c28f73
commit 97b5b0448a
4 changed files with 40 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_DAL/AP_DAL.h
+3
View File
@@ -270,6 +270,9 @@ public:
void handle_message(const log_RISI &msg) {
_ins.handle_message(msg);
}
void handle_message(const log_RISJ &msg) {
_ins.handle_message(msg);
}
void handle_message(const log_RASH &msg) {
if (_airspeed == nullptr) {
libraries/AP_DAL/AP_DAL_InertialSensor.cpp
+15 -4
View File
@@ -8,7 +8,13 @@ AP_DAL_InertialSensor::AP_DAL_InertialSensor()
for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {
_RISI[i].instance = i;
}
// seed RISJ with legacy defaults so replay of older logs (with no
// RISJ messages) still gives the EKF sane gyro bias clamp/init values.
for (uint8_t i=0; i<ARRAY_SIZE(_RISJ); i++) {
_RISJ[i].instance = i;
_RISJ[i].gyro_bias_limit = 0.5f;
_RISJ[i].gyro_bias_init_dps = 2.5f;
}
}
void AP_DAL_InertialSensor::start_frame()
@@ -41,13 +47,18 @@ void AP_DAL_InertialSensor::start_frame()
RISI.get_delta_angle_ret = ins.get_delta_angle(i, RISI.delta_angle, RISI.delta_angle_dt);
}
RISI.gyro_bias_limit = ins.get_gyro_bias_limit_rads(i);
RISI.gyro_bias_init_dps = ins.get_gyro_bias_init_dps(i);
update_filtered(i);
WRITE_REPLAY_BLOCK_IFCHANGED(RISI, RISI, old_RISI);
// RISJ holds per-instance gyro bias metadata. These are constants
// set by the backend, so the message is only written when changed.
log_RISJ &RISJ = _RISJ[i];
const log_RISJ old_RISJ = RISJ;
RISJ.gyro_bias_limit = ins.get_gyro_bias_limit_rads(i);
RISJ.gyro_bias_init_dps = ins.get_gyro_bias_init_dps(i);
WRITE_REPLAY_BLOCK_IFCHANGED(RISJ, RISJ, old_RISJ);
// update sensor position
pos[i] = ins.get_imu_pos_offset(i);
}
libraries/AP_DAL/AP_DAL_InertialSensor.h
+6 -2
View File
@@ -33,8 +33,8 @@ public:
uint8_t get_first_usable_gyro(void) const { return _RISH.first_usable_gyro; };
bool use_gyro(uint8_t instance) const { return _RISI[instance].use_gyro; }
float get_gyro_bias_limit(uint8_t instance) const { return _RISI[instance].gyro_bias_limit; }
float get_gyro_bias_init_dps(uint8_t instance) const { return _RISI[instance].gyro_bias_init_dps; }
float get_gyro_bias_limit(uint8_t instance) const { return _RISJ[instance].gyro_bias_limit; }
float get_gyro_bias_init_dps(uint8_t instance) const { return _RISJ[instance].gyro_bias_init_dps; }
const Vector3f &get_gyro(uint8_t i) const { return gyro_filtered[i]; }
const Vector3f &get_gyro() const { return get_gyro(_primary_gyro); }
bool get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const {
@@ -59,10 +59,14 @@ public:
pos[msg.instance] = AP::ins().get_imu_pos_offset(msg.instance);
update_filtered(msg.instance);
}
void handle_message(const log_RISJ &msg) {
_RISJ[msg.instance] = msg;
}
private:
struct log_RISH _RISH;
struct log_RISI _RISI[INS_MAX_INSTANCES];
struct log_RISJ _RISJ[INS_MAX_INSTANCES];
float alpha;
// sensor positions
libraries/AP_DAL/LogStructure.h
+16 -5
View File
@@ -19,6 +19,7 @@
LOG_RFRN_MSG, \
LOG_RISH_MSG, \
LOG_RISI_MSG, \
LOG_RISJ_MSG, \
LOG_RBRH_MSG, \
LOG_RBRI_MSG, \
LOG_RRNH_MSG, \
@@ -125,8 +126,6 @@ struct log_RISH {
// @Field: DAZ: z-axis delta-angle
// @Field: DVDT: delta-velocity-delta-time
// @Field: DADT: delta-angle-delta-time
// @Field: GBL: gyro bias limit (rad/s) for the EKF gyro bias state clamp
// @Field: GBI: initial gyro bias 1-sigma uncertainty (deg/s)
// @Field: Flags: use-accel, use-gyro, delta-vel-valid, delta-accel-valid
// @Field: I: IMU instance
struct log_RISI {
@@ -134,8 +133,6 @@ struct log_RISI {
Vector3f delta_angle;
float delta_velocity_dt;
float delta_angle_dt;
float gyro_bias_limit;
float gyro_bias_init_dps;
uint8_t use_accel:1;
uint8_t use_gyro:1;
uint8_t get_delta_velocity_ret:1;
@@ -144,6 +141,18 @@ struct log_RISI {
uint8_t _end;
};
// @LoggerMessage: RISJ
// @Description: Replay Inertial Sensor instance metadata (low-rate, only logged when changed)
// @Field: GBL: gyro bias limit (rad/s) for the EKF gyro bias state clamp
// @Field: GBI: initial gyro bias 1-sigma uncertainty (deg/s)
// @Field: I: IMU instance
struct log_RISJ {
float gyro_bias_limit;
float gyro_bias_init_dps;
uint8_t instance;
uint8_t _end;
};
// @LoggerMessage: REV2
// @Description: Replay Event (EKF2)
// @Field: Event: external event injected into EKF
@@ -619,7 +628,9 @@ struct log_RTER {
{ LOG_RISH_MSG, RLOG_SIZE(RISH), \
"RISH", "HBBfBB", "LR,PG,PA,LD,AC,GC", "------", "------" }, \
{ LOG_RISI_MSG, RLOG_SIZE(RISI), \
"RISI", "ffffffffffBB", "DVX,DVY,DVZ,DAX,DAY,DAZ,DVDT,DADT,GBL,GBI,Flags,I", "-----------#", "------------" }, \
"RISI", "ffffffffBB", "DVX,DVY,DVZ,DAX,DAY,DAZ,DVDT,DADT,Flags,I", "---------#", "----------" }, \
{ LOG_RISJ_MSG, RLOG_SIZE(RISJ), \
"RISJ", "ffB", "GBL,GBI,I", "--#", "---" }, \
{ LOG_RASH_MSG, RLOG_SIZE(RASH), \
"RASH", "BB", "Primary,NumInst", "--", "--" }, \
{ LOG_RASI_MSG, RLOG_SIZE(RASI), \