dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: add simple accelerometer quick calibration

This commit is contained in:
Daniel Agar 2020-08-13 16:59:37 -04:00
parent e3e8c55e82
commit d92ddffa5d
4 changed files with 157 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.ci/Jenkinsfile-hardware
View File

@ -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"'

19
src/modules/commander/Commander.cpp
View File

@ -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);

139
src/modules/commander/accelerometer_calibration.cpp
View File

@ -139,9 +139,10 @@
#include <lib/parameters/param.h>
#include <lib/systemlib/err.h>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/vehicle_attitude.h>
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<vehicle_attitude_s> 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;
}

1
src/modules/commander/accelerometer_calibration.h
View File

@ -46,5 +46,6 @@
#include <uORB/uORB.h>
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_ */