diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index 97892ba253..a50f5a0e94 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -839,6 +839,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate accel quick; sleep 2; param show CAL_ACC*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate gyro; sleep 2; param show CAL_GYRO*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate level; sleep 2; param show SENS_BOARD*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' @@ -904,6 +905,7 @@ void statusSEGGER() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "adc test"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander check"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate accel quick; sleep 2; param show CAL_ACC*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate gyro; sleep 2; param show CAL_GYRO*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate level; sleep 2; param show SENS_BOARD*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dataman status"' diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 04b2e83d03..47ff93b30a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -261,11 +261,18 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f); } else if (!strcmp(argv[2], "accel")) { - // accelerometer calibration: param5 = 1 - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f); + if (argc > 3 && (strcmp(argv[3], "quick") == 0)) { + // accelerometer quick calibration: param5 = 3 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 4.f, 0.f, 0.f); + + } else { + // accelerometer calibration: param5 = 1 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f); + } + } else if (!strcmp(argv[2], "level")) { - // board level calibration: param5 = 1 + // board level calibration: param5 = 2 send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.f, 0.f, 0.f); } else if (!strcmp(argv[2], "airspeed")) { @@ -3521,6 +3528,11 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); calib_ret = do_level_calibration(&mavlink_log_pub); + } else if ((int)(cmd.param5) == 4) { + // accelerometer quick calibration + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + calib_ret = do_accel_calibration_quick(&mavlink_log_pub); + } else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) { // TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017) /* airspeed calibration */ @@ -4243,6 +4255,7 @@ The commander module contains the state machine for mode switching and failsafe #ifndef CONSTRAINED_FLASH PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run sensor calibration"); PRINT_MODULE_USAGE_ARG("mag|accel|gyro|level|esc|airspeed", "Calibration type", false); + PRINT_MODULE_USAGE_ARG("quick", "Quick calibration (accel only, not recommended)", false); PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks"); PRINT_MODULE_USAGE_COMMAND("arm"); PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true); diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 68a3619408..f399cbb631 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -139,9 +139,10 @@ #include #include #include -#include #include #include +#include +#include using namespace matrix; using namespace time_literals; @@ -377,3 +378,139 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); return PX4_ERROR; } + +int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) +{ +#if !defined(CONSTRAINED_FLASH) + PX4_INFO("Accelerometer quick calibration"); + + int32_t device_id_primary = 0; + + // sensor thermal corrections (optional) + uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; + sensor_correction_s sensor_correction{}; + sensor_correction_sub.copy(&sensor_correction); + + uORB::Subscription accel_sub[MAX_ACCEL_SENS] { + {ORB_ID(sensor_accel), 0}, + {ORB_ID(sensor_accel), 1}, + {ORB_ID(sensor_accel), 2}, + }; + + /* use the first sensor to pace the readout, but do per-sensor counts */ + for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { + sensor_accel_s arp{}; + Vector3f accel_sum{}; + unsigned count = 0; + + while (accel_sub[accel_index].update(&arp)) { + // fetch optional thermal offset corrections in sensor/board frame + if ((arp.timestamp > 0) && (arp.device_id != 0)) { + Vector3f offset{0, 0, 0}; + + if (sensor_correction.timestamp > 0) { + for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) { + if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) { + switch (correction_index) { + case 0: + offset = Vector3f{sensor_correction.accel_offset_0}; + break; + case 1: + offset = Vector3f{sensor_correction.accel_offset_1}; + break; + case 2: + offset = Vector3f{sensor_correction.accel_offset_2}; + break; + } + } + } + } + + const Vector3f accel{Vector3f{arp.x, arp.y, arp.z} - offset}; + + if (count > 0) { + const Vector3f diff{accel - (accel_sum / count)}; + + if (diff.norm() < 1.f) { + accel_sum += Vector3f{arp.x, arp.y, arp.z} - offset; + count++; + } + + } else { + accel_sum = accel; + count = 1; + } + } + } + + if ((count > 0) && (arp.device_id != 0)) { + + bool calibrated = false; + const Vector3f accel_avg = accel_sum / count; + + Vector3f offset{0.f, 0.f, 0.f}; + + uORB::SubscriptionData attitude_sub{ORB_ID(vehicle_attitude)}; + attitude_sub.update(); + + if (attitude_sub.advertised() && attitude_sub.get().timestamp != 0) { + // use vehicle_attitude if available + const vehicle_attitude_s &att = attitude_sub.get(); + const matrix::Quatf q{att.q}; + const Vector3f accel_ref = q.conjugate_inversed(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G}); + + // sanity check angle between acceleration vectors + const float angle = AxisAnglef(Quatf(accel_avg, accel_ref)).angle(); + + if (angle <= math::radians(10.f)) { + offset = accel_avg - accel_ref; + calibrated = true; + } + } + + if (!calibrated) { + // otherwise simply normalize to gravity and remove offset + Vector3f accel{accel_avg}; + accel.normalize(); + accel = accel * CONSTANTS_ONE_G; + + offset = accel_avg - accel; + calibrated = true; + } + + calibration::Accelerometer calibration{arp.device_id}; + + // reset cal index to uORB + calibration.set_calibration_index(accel_index); + + if (!calibrated || (offset.norm() > CONSTANTS_ONE_G) + || !PX4_ISFINITE(offset(0)) + || !PX4_ISFINITE(offset(1)) + || !PX4_ISFINITE(offset(2))) { + + PX4_ERR("accel %d quick calibrate failed", accel_index); + + } else { + calibration.set_offset(offset); + } + + calibration.ParametersSave(); + + if (device_id_primary == 0) { + device_id_primary = arp.device_id; + } + + calibration.PrintStatus(); + } + } + + if (device_id_primary != 0) { + param_set_no_notification(param_find("CAL_ACC_PRIME"), &device_id_primary); + param_notify_changes(); + return PX4_OK; + } + +#endif // !CONSTRAINED_FLASH + + return PX4_ERROR; +} diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index fe3e03aedc..3eeb9610a8 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -46,5 +46,6 @@ #include int do_accel_calibration(orb_advert_t *mavlink_log_pub); +int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub); #endif /* ACCELEROMETER_CALIBRATION_H_ */