Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_InertialSensor: tidy IMU killing #27498

Merged
merged 1 commit into from
Jul 10, 2024
Merged

AP_InertialSensor: tidy IMU killing #27498

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions libraries/AP_InertialSensor/AP_InertialSensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -741,7 +741,9 @@ class AP_InertialSensor : AP_AccelCal_Client
bool _startup_error_counts_set;
uint32_t _startup_ms;

#if AP_INERTIALSENSOR_KILL_IMU_ENABLED
uint8_t imu_kill_mask;
#endif

#if HAL_INS_TEMPERATURE_CAL_ENABLE
public:
Expand Down
18 changes: 9 additions & 9 deletions libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -164,7 +164,7 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto
*/
void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro) /* front end */
{
if ((1U<<instance) & _imu.imu_kill_mask) {
if (has_been_killed(instance)) {
return;
}
_imu._gyro[instance] = gyro;
Expand Down Expand Up @@ -263,7 +263,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
const Vector3f &gyro,
uint64_t sample_us)
{
if ((1U<<instance) & _imu.imu_kill_mask) {
if (has_been_killed(instance)) {
return;
}
float dt;
Expand Down Expand Up @@ -360,7 +360,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
*/
void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const Vector3f &dangle)
{
if ((1U<<instance) & _imu.imu_kill_mask) {
if (has_been_killed(instance)) {
return;
}
float dt;
Expand Down Expand Up @@ -487,7 +487,7 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa
*/
void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel) /* front end */
{
if ((1U<<instance) & _imu.imu_kill_mask) {
if (has_been_killed(instance)) {
return;
}
_imu._accel[instance] = accel;
Expand Down Expand Up @@ -517,7 +517,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
uint64_t sample_us,
bool fsync_set)
{
if ((1U<<instance) & _imu.imu_kill_mask) {
if (has_been_killed(instance)) {
return;
}
float dt;
Expand Down Expand Up @@ -604,7 +604,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
*/
void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, const Vector3f &dvel)
{
if ((1U<<instance) & _imu.imu_kill_mask) {
if (has_been_killed(instance)) {
return;
}
float dt;
Expand Down Expand Up @@ -746,7 +746,7 @@ void AP_InertialSensor_Backend::_inc_gyro_error_count(uint8_t instance)
*/
void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float temperature) /* front end */
{
if ((1U<<instance) & _imu.imu_kill_mask) {
if (has_been_killed(instance)) {
return;
}
_imu._temperature[instance] = temperature;
Expand All @@ -769,7 +769,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
{
WITH_SEMAPHORE(_sem);

if ((1U<<instance) & _imu.imu_kill_mask) {
if (has_been_killed(instance)) {
return;
}
if (_imu._new_gyro_data[instance]) {
Expand Down Expand Up @@ -808,7 +808,7 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */
{
WITH_SEMAPHORE(_sem);

if ((1U<<instance) & _imu.imu_kill_mask) {
if (has_been_killed(instance)) {
return;
}
if (_imu._new_accel_data[instance]) {
Expand Down
7 changes: 7 additions & 0 deletions libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,13 @@ class AP_InertialSensor_Backend
virtual void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) {}
#endif

#if AP_INERTIALSENSOR_KILL_IMU_ENABLED
bool has_been_killed(uint8_t instance) const { return ((1U<<instance) & _imu.imu_kill_mask); }
#else
bool has_been_killed(uint8_t instance) const { return false; }
#endif


/*
device driver IDs. These are used to fill in the devtype field
of the device ID, which shows up as INS*ID* parameters to
Expand Down
Loading