fix(drivers/gnss): fill ins with raw imu data if no corrected imu data received #113
Conversation
There was a problem hiding this comment.
Pull request overview
This PR addresses a gap in the GNSS driver where INS (Inertial Navigation System) data would not be populated when corrected IMU messages (CORRIMUDATA) are unavailable. The fix adds logic to use raw IMU data as a fallback to populate INS acceleration and velocity fields.
Key changes:
- Added a flag
has_corr_imu_message_to track whether corrected IMU messages have been received - Modified
HandleRawImuto conditionally populateins_fields with raw IMU data when corrected IMU data is not available - Added an unused flag
has_ins_stat_message_(not currently utilized)
Reviewed changes
Copilot reviewed 2 out of 2 changed files in this pull request and generated 5 comments.
| File | Description |
|---|---|
| modules/drivers/gnss/parser/novatel/novatel_parser.h | Added two boolean member variables to track message availability |
| modules/drivers/gnss/parser/novatel/novatel_parser.cc | Set has_corr_imu_message_ flag when corrected IMU received; added fallback logic in HandleRawImu to populate ins_ with raw IMU data |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| // RTKLIB raw observation data structure | ||
| raw_t raw_; | ||
|
|
||
| bool has_ins_stat_message_ = false; |
There was a problem hiding this comment.
The has_ins_stat_message_ flag is declared but never used anywhere in the implementation. Consider removing this unused member variable, or if it's intended for future use, document its purpose.
| bool has_ins_stat_message_ = false; |
| break; | ||
| } | ||
| // HandleCorrImuData updates internal ins_. | ||
| has_corr_imu_message_ = true; |
There was a problem hiding this comment.
The has_corr_imu_message_ flag is set to true when a corrected IMU message is received, but it's never reset. This means that once a corrected IMU message is received, the raw IMU will never update the INS data again for the lifetime of the parser object, even if corrected IMU messages stop arriving. Consider resetting this flag periodically or using a timestamp-based approach to detect when corrected IMU messages are no longer being received.
| if (!has_corr_imu_message_) { | ||
| z_rot_90_ccw(imu->x_velocity_change * accel_scale, | ||
| -imu->y_velocity_change_neg * accel_scale, | ||
| imu->z_velocity_change * accel_scale, | ||
| ins_.mutable_linear_acceleration()); | ||
| z_rot_90_ccw(imu->x_angle_change * gyro_scale, | ||
| -imu->y_angle_change_neg * gyro_scale, | ||
| imu->z_angle_change * gyro_scale, | ||
| ins_.mutable_angular_velocity()); | ||
| } |
There was a problem hiding this comment.
When raw IMU data fills the INS fields in the absence of corrected IMU messages, the INS data is updated but no corresponding INS message is generated in PrepareMessage (lines 339-351 only create IMU messages). This means the updated ins_ member won't be published as a message. Consider adding logic to PrepareMessage to also generate an INS message when HandleRawImu updates the ins_ data and has_corr_imu_message_ is false, similar to how CORRIMUDATA generates both updates and publishes (lines 289-295).
| if (!has_corr_imu_message_) { | ||
| z_rot_90_ccw(imu->x_velocity_change * accel_scale, | ||
| -imu->y_velocity_change_neg * accel_scale, | ||
| imu->z_velocity_change * accel_scale, | ||
| ins_.mutable_linear_acceleration()); | ||
| z_rot_90_ccw(imu->x_angle_change * gyro_scale, | ||
| -imu->y_angle_change_neg * gyro_scale, | ||
| imu->z_angle_change * gyro_scale, | ||
| ins_.mutable_angular_velocity()); | ||
| } |
There was a problem hiding this comment.
When raw IMU data fills the INS acceleration and velocity fields, the measurement time and header timestamp are not set on the ins_ object. Compare this to HandleCorrImuData (lines 578-581) which sets both ins_.set_measurement_time() and ins_.mutable_header()->set_timestamp_sec(). This could lead to stale or incorrect timestamps in the INS data. Consider adding these timestamp updates after setting the acceleration and velocity values.
| z_rot_90_ccw(-imu->y_angle_change_neg * gyro_scale, | ||
| imu->x_angle_change * gyro_scale, | ||
| -imu->z_angle_change * gyro_scale, | ||
| ins_.mutable_angular_velocity()); |
There was a problem hiding this comment.
When raw IMU data fills the INS acceleration and velocity fields, the measurement time and header timestamp are not set on the ins_ object. Compare this to HandleCorrImuData (lines 578-581) which sets both ins_.set_measurement_time() and ins_.mutable_header()->set_timestamp_sec(). This could lead to stale or incorrect timestamps in the INS data. Consider adding these timestamp updates after setting the acceleration and velocity values.
| ins_.mutable_angular_velocity()); | |
| ins_.mutable_angular_velocity()); | |
| ins_.set_measurement_time(time); | |
| ins_.mutable_header()->set_timestamp_sec(time); |
…a received Signed-off-by: Pride Leong <lykling.lyk@gmail.com>
lykling
force-pushed
the
112-bug-linear_acceleration-and-angular_velocity-of-correctedimu-is-nan-for-m2-device
branch
from
6be6ba4 to
db25bc0
Compare
lykling
deleted the
112-bug-linear_acceleration-and-angular_velocity-of-correctedimu-is-nan-for-m2-device
branch
No description provided.