From 2396a248ed698a38f98a1aac0df3149a91a7b7fa Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 17 Mar 2018 23:03:13 +1100 Subject: [PATCH] AP_InertialSensor: use ahrs singleton for simple accelcal --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 8 ++++---- libraries/AP_InertialSensor/AP_InertialSensor.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index e3127e8c34..3d7ab3df4d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1846,7 +1846,7 @@ bool AP_InertialSensor::get_primary_accel_cal_sample_avg(uint8_t sample_num, Vec /* perform a simple 1D accel calibration, returning mavlink result code */ -MAV_RESULT AP_InertialSensor::simple_accel_cal(AP_AHRS &ahrs) +MAV_RESULT AP_InertialSensor::simple_accel_cal() { uint8_t num_accels = MIN(get_accel_count(), INS_MAX_INSTANCES); Vector3f last_average[INS_MAX_INSTANCES]; @@ -1974,7 +1974,7 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal(AP_AHRS &ahrs) } // force trim to zero - ahrs.set_trim(Vector3f(0, 0, 0)); + AP::ahrs().set_trim(Vector3f(0, 0, 0)); } else { hal.console->printf("\nFAILED\n"); // restore old values @@ -1996,8 +1996,8 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal(AP_AHRS &ahrs) } // and reset state estimators - ahrs.reset(); - + AP::ahrs().reset(); + // stop flashing leds AP_Notify::flags.initialising = false; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 65e3eb51c3..6775abd044 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -268,7 +268,7 @@ public: void acal_update(); // simple accel calibration - MAV_RESULT simple_accel_cal(AP_AHRS &ahrs); + MAV_RESULT simple_accel_cal(); bool accel_cal_requires_reboot() const { return _accel_cal_requires_reboot; }