fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 10:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into linux

Browse Source 
Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Conflicts:
	src/drivers/rgbled/rgbled.cpp
	src/modules/commander/PreflightCheck.cpp
	src/modules/commander/airspeed_calibration.cpp
	src/modules/commander/calibration_routines.cpp
	src/modules/commander/gyro_calibration.cpp
	src/modules/commander/mag_calibration.cpp
	src/modules/mc_att_control/mc_att_control_main.cpp
This commit is contained in:
Mark Charlebois
2015-04-28 11:48:26 -07:00
parent c832f4c55c 2a46e0f0b6
commit 190814bc97
47 changed files with 935 additions and 656 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/PreflightCheck.cpp
+6 -6
View File
@@ -94,7 +94,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance);
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
success = false;
goto out;
}
@@ -138,7 +138,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance);
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
success = false;
goto out;
}
@@ -162,7 +162,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still");
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
/* this is frickin' fatal */
success = false;
goto out;
@@ -205,7 +205,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance);
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
success = false;
goto out;
}
@@ -255,13 +255,13 @@ static bool airspeedCheck(int mavlink_fd, bool optional)
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
success = false;
goto out;
}
if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
// XXX do not make this fatal yet
}
src/modules/commander/accelerometer_calibration.cpp
+52 -35
View File
@@ -153,11 +153,10 @@ static const int ERROR = -1;
static const char *sensor_name = "accel";
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
int mat_invert3(float src[3][3], float dst[3][3]);
int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
int accel_calibration_worker(detect_orientation_return orientation, void* worker_data);
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
/// Data passed to calibration worker routine
typedef struct {
@@ -172,7 +171,7 @@ int do_accel_calibration(int mavlink_fd)
int fd;
int32_t device_id[max_accel_sens];
mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
struct accel_scale accel_scale = {
0.0f,
@@ -203,7 +202,7 @@ int do_accel_calibration(int mavlink_fd)
px4_close(fd);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
}
}
@@ -211,13 +210,23 @@ int do_accel_calibration(int mavlink_fd)
float accel_T[max_accel_sens][3][3];
unsigned active_sensors;
/* measure and calculate offsets & scales */
if (res == OK) {
/* measure and calculate offsets & scales */
res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return ERROR;
} else if (cal_return == calibrate_return_ok) {
res = OK;
} else {
res = ERROR;
}
}
if (res != OK || active_sensors == 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
if (res != OK) {
if (active_sensors == 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
}
return ERROR;
}
@@ -264,7 +273,7 @@ int do_accel_calibration(int mavlink_fd)
failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, i);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
return ERROR;
}
@@ -272,7 +281,7 @@ int do_accel_calibration(int mavlink_fd)
fd = px4_open(str, 0);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist");
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = ERROR;
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
@@ -280,7 +289,7 @@ int do_accel_calibration(int mavlink_fd)
}
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i);
}
}
@@ -289,41 +298,47 @@ int do_accel_calibration(int mavlink_fd)
res = param_save_default();
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
}
mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return res;
}
int accel_calibration_worker(detect_orientation_return orientation, void* data)
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
const unsigned samples_num = 3000;
accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation));
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num);
mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side result: [ %8.4f %8.4f %8.4f ]", detect_orientation_str(orientation),
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]);
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * worker_data->done_count);
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
return OK;
return calibrate_return_ok;
}
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
int result = OK;
calibrate_return result = calibrate_return_ok;
*active_sensors = 0;
@@ -344,7 +359,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
for (unsigned i = 0; i < max_accel_sens; i++) {
worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
if (worker_data.subs[i] < 0) {
result = ERROR;
result = calibrate_return_error;
break;
}
@@ -354,8 +369,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
timestamps[i] = arp.timestamp;
}
if (result == OK) {
result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker, &worker_data);
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_fd, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
calibrate_cancel_unsubscribe(cancel_sub);
}
/* close all subscriptions */
@@ -371,13 +388,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
}
}
if (result == OK) {
if (result == calibrate_return_ok) {
/* calculate offsets and transform matrix */
for (unsigned i = 0; i < (*active_sensors); i++) {
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (result != OK) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error");
if (result != calibrate_return_ok) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: calibration calculation error");
break;
}
}
@@ -389,7 +406,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
/*
* Read specified number of accelerometer samples, calculate average and dispersion.
*/
int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
{
px4_pollfd_struct_t fds[max_accel_sens];
@@ -433,7 +450,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
}
if (errcount > samples_num / 10) {
return ERROR;
return calibrate_return_error;
}
}
@@ -443,7 +460,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
}
}
return OK;
return calibrate_return_ok;
}
int mat_invert3(float src[3][3], float dst[3][3])
@@ -469,7 +486,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
return OK;
}
int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
{
/* calculate offsets */
for (unsigned i = 0; i < 3; i++) {
@@ -491,7 +508,7 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
float mat_A_inv[3][3];
if (mat_invert3(mat_A, mat_A_inv) != OK) {
return ERROR;
return calibrate_return_error;
}
/* copy results to accel_T */
@@ -502,5 +519,5 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
}
}
return OK;
return calibrate_return_ok;
}
src/modules/commander/airspeed_calibration.cpp
+57 -49
View File
@@ -38,6 +38,7 @@
#include "airspeed_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include <px4_posix.h>
@@ -64,19 +65,20 @@ static const int ERROR = -1;
static const char *sensor_name = "dpress";
#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd"
static void feedback_calibration_failed(int mavlink_fd)
{
sleep(5);
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG);
mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
}
int do_airspeed_calibration(int mavlink_fd)
{
int result = OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 3000;
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = 2000;
@@ -99,11 +101,13 @@ int do_airspeed_calibration(int mavlink_fd)
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
}
px4_close(fd);
}
int cancel_sub = calibrate_cancel_subscribe();
if (!paramreset_successful) {
@@ -111,26 +115,26 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd");
px4_close(diff_pres_sub);
return ERROR;
mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
px4_close(diff_pres_sub);
return ERROR;
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
}
unsigned calibration_counter = 0;
mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
@@ -145,14 +149,13 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
px4_close(diff_pres_sub);
return ERROR;
goto error_return;
}
}
@@ -164,16 +167,15 @@ int do_airspeed_calibration(int mavlink_fd)
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
px4_close(diff_pres_sub);
return ERROR;
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
/* auto-save to EEPROM */
@@ -181,30 +183,31 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
px4_close(diff_pres_sub);
return ERROR;
mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
goto error_return;
}
} else {
feedback_calibration_failed(mavlink_fd);
px4_close(diff_pres_sub);
return ERROR;
goto error_return;
}
mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "Create airflow now");
mavlink_log_critical(mavlink_fd, "[cal] Create airflow now");
calibration_counter = 0;
const unsigned maxcount = 3000;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
@@ -219,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)",
mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
@@ -227,30 +230,26 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
px4_close(diff_pres_sub);
mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
px4_close(diff_pres_sub);
return ERROR;
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
/* save */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
(void)param_save_default();
px4_close(diff_pres_sub);
feedback_calibration_failed(mavlink_fd);
return ERROR;
goto error_return;
} else {
mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
@@ -258,21 +257,30 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
px4_close(diff_pres_sub);
return ERROR;
goto error_return;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_fd);
px4_close(diff_pres_sub);
return ERROR;
goto error_return;
}
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
normal_return:
calibrate_cancel_unsubscribe(cancel_sub);
px4_close(diff_pres_sub);
return OK;
// This give a chance for the log messages to go out of the queue before someone else stomps on then
sleep(1);
return result;
error_return:
result = ERROR;
goto normal_return;
}
src/modules/commander/calibration_messages.h
+19 -11
View File
@@ -42,17 +42,25 @@
#ifndef CALIBRATION_MESSAGES_H_
#define CALIBRATION_MESSAGES_H_
#define CAL_STARTED_MSG "%s calibration: started"
#define CAL_DONE_MSG "%s calibration: done"
#define CAL_FAILED_MSG "%s calibration: failed"
#define CAL_PROGRESS_MSG "%s calibration: progress <%u>"
// The calibration message defines which begin with CAL_QGC_ are used by QGroundControl to run a state
// machine to provide visual feedback for calibration. As such, the text for them or semantics of when
// they are displayed cannot be modified without causing QGC to break. If modifications are made, make
// sure to bump the calibration version number which will cause QGC to perform log based calibration
// instead of visual calibration until such a time as QGC is update to the new version.
#define CAL_FAILED_UNKNOWN_ERROR "ERROR: unknown error"
#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor"
#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration, sensor %u"
#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration, sensor %u"
#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters, sensor %u"
#define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters"
#define CAL_FAILED_ORIENTATION_TIMEOUT "ERROR: timed out waiting for correct orientation"
// The number in the cal started message is used to indicate the version stamp for the current calibration code.
#define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s"
#define CAL_QGC_DONE_MSG "[cal] calibration done: %s"
#define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s"
#define CAL_QGC_CANCELLED_MSG "[cal] calibration cancelled"
#define CAL_QGC_PROGRESS_MSG "[cal] progress <%u>"
#define CAL_QGC_ORIENTATION_DETECTED_MSG "[cal] %s orientation detected"
#define CAL_QGC_SIDE_DONE_MSG "[cal] %s side done, rotate to a different side"
#define CAL_ERROR_SENSOR_MSG "[cal] calibration failed: reading sensor"
#define CAL_ERROR_RESET_CAL_MSG "[cal] calibration failed: to reset, sensor %u"
#define CAL_ERROR_APPLY_CAL_MSG "[cal] calibration failed: to apply calibration, sensor %u"
#define CAL_ERROR_SET_PARAMS_MSG "[cal] calibration failed: to set parameters, sensor %u"
#define CAL_ERROR_SAVE_PARAMS_MSG "[cal] calibration failed: failed to save parameters"
#endif /* CALIBRATION_MESSAGES_H_ */
src/modules/commander/calibration_routines.cpp
+104 -41
View File
@@ -52,6 +52,8 @@
#include <geo/geo.h>
#include <string.h>
#include <uORB/topics/vehicle_command.h>
#include "calibration_routines.h"
#include "calibration_messages.h"
#include "commander_helper.h"
@@ -231,23 +233,19 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
return 0;
}
enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub, int accel_sub, bool lenient_still_position)
{
const unsigned ndim = 3;
struct accel_report sensor;
/* exponential moving average of accel */
float accel_ema[ndim] = { 0.0f };
/* max-hold dispersion of accel */
float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
/* EMA time constant in seconds*/
float ema_len = 0.5f;
/* set "still" threshold to 0.25 m/s^2 */
float still_thr2 = powf(0.25f, 2);
/* set accel error threshold to 5m/s^2 */
float accel_err_thr = 5.0f;
/* still time required in us */
hrt_abstime still_time = 2000000;
float accel_ema[ndim] = { 0.0f }; // exponential moving average of accel
float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; // max-hold dispersion of accel
float ema_len = 0.5f; // EMA time constant in seconds
const float normal_still_thr = 0.25; // normal still threshold
float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2
hrt_abstime still_time = lenient_still_position ? 1000000 : 1500000; // still time required in us
px4_pollfd_struct_t fds[1];
fds[0].fd = accel_sub;
fds[0].events = POLLIN;
@@ -309,7 +307,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still...");
mavlink_and_console_log_info(mavlink_fd, "[cal] detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
@@ -326,8 +324,8 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
accel_disp[2] > still_thr2 * 4.0f) {
/* not still, reset still start time */
if (t_still != 0) {
mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
sleep(3);
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
usleep(500000);
t_still = 0;
}
}
@@ -341,7 +339,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
}
if (poll_errcount > 1000) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
return DETECT_ORIENTATION_ERROR;
}
}
@@ -382,7 +380,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ]
}
mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation");
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: invalid orientation");
return DETECT_ORIENTATION_ERROR; // Can't detect orientation
}
@@ -402,28 +400,35 @@ const char* detect_orientation_str(enum detect_orientation_return orientation)
return rgOrientationStrs[orientation];
}
int calibrate_from_orientation(int mavlink_fd,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void* worker_data)
calibrate_return calibrate_from_orientation(int mavlink_fd,
int cancel_sub,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void* worker_data,
bool lenient_still_position)
{
int result = OK;
calibrate_return result = calibrate_return_ok;
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
if (sub_accel < 0) {
mavlink_and_console_log_critical(mavlink_fd, "No onboard accel found, abort");
return PX4_ERROR;
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
unsigned orientation_failures = 0;
// Rotate through all three main positions
// Rotate through all requested orientation
while (true) {
if (orientation_failures > 10) {
result = PX4_ERROR;
mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_ORIENTATION_TIMEOUT);
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
if (orientation_failures > 4) {
result = calibrate_return_error;
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
@@ -451,44 +456,102 @@ int calibrate_from_orientation(int mavlink_fd,
strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
}
}
mavlink_and_console_log_info(mavlink_fd, "pending:%s", pendingStr);
mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr);
mavlink_and_console_log_info(mavlink_fd, "hold the vehicle still on one of the pending sides");
enum detect_orientation_return orient = detect_orientation(mavlink_fd, sub_accel);
mavlink_and_console_log_info(mavlink_fd, "[cal] hold vehicle still on a pending side");
enum detect_orientation_return orient = detect_orientation(mavlink_fd, cancel_sub, sub_accel, lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
mavlink_and_console_log_info(mavlink_fd, "%s side already completed or not needed", detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_fd, "rotate to a pending side");
mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side");
continue;
}
mavlink_and_console_log_info(mavlink_fd, "%s orientation detected", detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
orientation_failures = 0;
// Call worker routine
calibration_worker(orient, worker_data);
result = calibration_worker(orient, cancel_sub, worker_data);
if (result != calibrate_return_ok ) {
break;
}
mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
// Note that this side is complete
side_data_collected[orient] = true;
tune_neutral(true);
sleep(1);
usleep(500000);
}
if (sub_accel >= 0) {
close(sub_accel);
}
// FIXME: Do we need an orientation complete routine?
return result;
}
int calibrate_cancel_subscribe(void)
{
return orb_subscribe(ORB_ID(vehicle_command));
}
void calibrate_cancel_unsubscribe(int cmd_sub)
{
orb_unsubscribe(cmd_sub);
}
static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive(true);
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "command denied during calibration: %u", cmd.command);
tune_negative(true);
break;
default:
break;
}
}
bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
{
struct pollfd fds[1];
fds[0].fd = cancel_sub;
fds[0].events = POLLIN;
if (poll(&fds[0], 1, 0) > 0) {
struct vehicle_command_s cmd;
memset(&cmd, 0, sizeof(cmd));
orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd);
if (cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
(int)cmd.param1 == 0 &&
(int)cmd.param2 == 0 &&
(int)cmd.param3 == 0 &&
(int)cmd.param4 == 0 &&
(int)cmd.param5 == 0 &&
(int)cmd.param6 == 0) {
calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_ACCEPTED);
mavlink_log_critical(mavlink_fd, CAL_QGC_CANCELLED_MSG);
return true;
} else {
calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_DENIED);
}
}
return false;
}
src/modules/commander/calibration_routines.h
+41 -30
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,12 +31,8 @@
*
****************************************************************************/
/**
* @file calibration_routines.h
* Calibration routines definitions.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
/// @file calibration_routines.h
/// @authot Don Gagne <don@thegagnes.com>
/**
* Least-squares fit of a sphere to a set of points.
@@ -75,30 +71,45 @@ enum detect_orientation_return {
};
static const unsigned detect_orientation_side_count = 6;
/**
* Wait for vehicle to become still and detect it's orientation.
*
* @param mavlink_fd the MAVLink file descriptor to print output to
* @param accel_sub Subscription to onboard accel
*
* @return detect_orientation)_return according to orientation when vehicle is still and ready for measurements,
* DETECT_ORIENTATION_ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
*/
enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub);
/// Wait for vehicle to become still and detect it's orientation
/// @return Returns detect_orientation_return according to orientation when vehicle
/// and ready for measurements
enum detect_orientation_return detect_orientation(int mavlink_fd, ///< Mavlink fd to write output to
int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
int accel_sub, ///< Orb subcription to accel sensor
bool lenient_still_detection); ///< true: Use more lenient still position detection
/**
* Returns the human readable string representation of the orientation
*
* @param orientation Orientation to return string for, "error" if buffer is too small
*
* @return str Returned orientation string
*/
/// Returns the human readable string representation of the orientation
/// @param orientation Orientation to return string for, "error" if buffer is too small
const char* detect_orientation_str(enum detect_orientation_return orientation);
typedef int (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, void* worker_data);
enum calibrate_return {
calibrate_return_ok,
calibrate_return_error,
calibrate_return_cancelled
};
int calibrate_from_orientation(int mavlink_fd,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void* worker_data);
typedef calibrate_return (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, ///< Orientation which was detected
int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
void* worker_data); ///< Opaque worker data
/// Perform calibration sequence which require a rest orientation detection prior to calibration.
/// @return OK: Calibration succeeded, ERROR: Calibration failed
calibrate_return calibrate_from_orientation(int mavlink_fd, ///< Mavlink fd to write output to
int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration
calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration
void* worker_data, ///< Opaque data passed to worker routine
bool lenient_still_detection); ///< true: Use more lenient still position detection
/// Called at the beginning of calibration in order to subscribe to the cancel command
/// @return Handle to vehicle_command subscription
int calibrate_cancel_subscribe(void);
/// Called to cancel the subscription to the cancel command
/// @param cancel_sub Cancel subcription from calibration_cancel_subscribe
void calibrate_cancel_unsubscribe(int cancel_sub);
/// Used to periodically check for a cancel command
bool calibrate_cancel_check(int mavlink_fd, ///< Mavlink fd for output
int cancel_sub); ///< Cancel subcription fromcalibration_cancel_subscribe
src/modules/commander/commander.cpp
+72 -77
View File
@@ -186,21 +186,6 @@ static struct safety_s safety;
static struct vehicle_control_mode_s control_mode;
static struct offboard_control_mode_s offboard_control_mode;
/* tasks waiting for low prio thread */
typedef enum {
LOW_PRIO_TASK_NONE = 0,
LOW_PRIO_TASK_PARAM_SAVE,
LOW_PRIO_TASK_PARAM_LOAD,
LOW_PRIO_TASK_GYRO_CALIBRATION,
LOW_PRIO_TASK_MAG_CALIBRATION,
LOW_PRIO_TASK_ALTITUDE_CALIBRATION,
LOW_PRIO_TASK_RC_CALIBRATION,
LOW_PRIO_TASK_ACCEL_CALIBRATION,
LOW_PRIO_TASK_AIRSPEED_CALIBRATION
} low_prio_task_t;
static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE;
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
@@ -232,6 +217,8 @@ int commander_thread_main(int argc, char *argv[]);
void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed);
void get_circuit_breaker_params();
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
@@ -558,8 +545,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
else {
//Refuse to arm if preflight checks have failed
if(!status.condition_system_sensors_initialized) {
// Refuse to arm if preflight checks have failed
if (!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed.");
cmd_result = VEHICLE_CMD_RESULT_DENIED;
break;
@@ -966,10 +953,10 @@ int commander_thread_main(int argc, char *argv[])
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count,
mission.current_seq);
mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
mission.dataman_id, mission.count, mission.current_seq);
if (mission.count > 0) {
mavlink_log_info(mavlink_fd, "[cmd] Mission #%d loaded, %u WPs, curr: %d",
mission.dataman_id, mission.count, mission.current_seq);
}
} else {
const char *missionfail = "reading mission state failed";
@@ -1038,7 +1025,7 @@ int commander_thread_main(int argc, char *argv[])
bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
telemetry_subs[i] = -1;
telemetry_last_heartbeat[i] = 0;
telemetry_last_dl_loss[i] = 0;
telemetry_lost[i] = true;
@@ -1135,6 +1122,8 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_sys_type, &(status.system_type)); // get system type
status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status);
get_circuit_breaker_params();
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
@@ -1142,9 +1131,9 @@ int commander_thread_main(int argc, char *argv[])
checkAirspeed = true;
}
//Run preflight check
// Run preflight check
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
if(!status.condition_system_sensors_initialized) {
if (!status.condition_system_sensors_initialized) {
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
}
else {
@@ -1176,7 +1165,7 @@ int commander_thread_main(int argc, char *argv[])
/* initialize low priority thread */
pthread_attr_t commander_low_prio_attr;
pthread_attr_init(&commander_low_prio_attr);
pthread_attr_setstacksize(&commander_low_prio_attr, 2100);
pthread_attr_setstacksize(&commander_low_prio_attr, 2000);
struct sched_param param;
(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -1228,14 +1217,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_system_id, &(status.system_id));
param_get(_param_component_id, &(status.component_id));
status.circuit_breaker_engaged_power_check =
circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status.circuit_breaker_engaged_airspd_check =
circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status.circuit_breaker_engaged_enginefailure_check =
circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
status.circuit_breaker_engaged_gpsfailure_check =
circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
get_circuit_breaker_params();
status_changed = true;
@@ -1296,6 +1278,11 @@ int commander_thread_main(int argc, char *argv[])
}
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (telemetry_subs[i] < 0 && (OK == orb_exists(telemetry_status_orb_id[i], 0))) {
telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
}
orb_check(telemetry_subs[i], &updated);
if (updated) {
@@ -1585,7 +1572,9 @@ int commander_thread_main(int argc, char *argv[])
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
if (armed.armed) {
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
}
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
@@ -1593,7 +1582,6 @@ int commander_thread_main(int argc, char *argv[])
&& !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL;
if (!armed.armed) {
@@ -1616,8 +1604,7 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
@@ -1625,8 +1612,6 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = true;
}
} else {
/* TODO: Add emergency stuff if sensors are lost */
}
@@ -1833,13 +1818,17 @@ int commander_thread_main(int argc, char *argv[])
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (telemetry_last_heartbeat[i] != 0 &&
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
/* handle the case where data link was regained,
/* handle the case where data link was gained first time or regained,
* accept datalink as healthy only after datalink_regain_timeout seconds
* */
if (telemetry_lost[i] &&
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
mavlink_log_info(mavlink_fd, "data link %i regained", i);
/* only report a regain */
if (telemetry_last_dl_loss[i] > 0) {
mavlink_and_console_log_critical(mavlink_fd, "data link #%i regained", i);
}
telemetry_lost[i] = false;
have_link = true;
@@ -1850,10 +1839,12 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
telemetry_last_dl_loss[i] = hrt_absolute_time();
if (!telemetry_lost[i]) {
mavlink_log_info(mavlink_fd, "data link %i lost", i);
/* only reset the timestamp to a different time on state change */
telemetry_last_dl_loss[i] = hrt_absolute_time();
mavlink_and_console_log_critical(mavlink_fd, "data link #%i lost", i);
telemetry_lost[i] = true;
}
}
@@ -1868,7 +1859,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (!status.data_link_lost) {
mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST");
mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
status.data_link_lost = true;
status.data_link_lost_counter++;
status_changed = true;
@@ -2119,6 +2110,19 @@ int commander_thread_main(int argc, char *argv[])
return 0;
}
void
get_circuit_breaker_params()
{
status.circuit_breaker_engaged_power_check =
circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status.circuit_breaker_engaged_airspd_check =
circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status.circuit_breaker_engaged_enginefailure_check =
circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
status.circuit_breaker_engaged_gpsfailure_check =
circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
}
void
check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
{
@@ -2670,7 +2674,7 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
false /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@@ -2719,53 +2723,38 @@ void *commander_low_prio_loop(void *arg)
/* enable RC control input */
status.rc_input_blocked = false;
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
calib_ret = OK;
}
/* this always succeeds */
calib_ret = OK;
}
if (calib_ret == OK) {
tune_positive(true);
// Update preflight check status
// we do not set the calibration return value based on it because the calibration
// might have worked just fine, but the preflight check fails for a different reason,
// so this would be prone to false negatives.
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
checkAirspeed = true;
}
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
} else {
tune_negative(true);
}
// Update preflight check status
// we do not set the calibration return value based on it because the calibration
// might have worked just fine, but the preflight check fails for a different reason,
// so this would be prone to false negatives.
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
checkAirspeed = true;
}
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
break;
}
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
checkAirspeed = true;
}
// Update preflight check status
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (((int)(cmd.param1)) == 0) {
int ret = param_load_default();
@@ -2789,9 +2778,15 @@ void *commander_low_prio_loop(void *arg)
}
} else if (((int)(cmd.param1)) == 1) {
int ret = param_save_default();
if (ret == OK) {
if (need_param_autosave) {
need_param_autosave = false;
need_param_autosave_timeout = 0;
}
mavlink_log_info(mavlink_fd, "settings saved");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
src/modules/commander/commander_helper.cpp
+4 -2
View File
@@ -87,8 +87,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
}
bool is_vtol(const struct vehicle_status_s * current_status) {
return current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR ||
current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR;
return (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR ||
current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR ||
current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_HEXAROTOR ||
current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_OCTOROTOR);
}
static int buzzer = -1;
src/modules/commander/gyro_calibration.cpp
+194 -109
View File
@@ -39,6 +39,7 @@
#include "gyro_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include <px4_posix.h>
@@ -65,142 +66,195 @@ static const int ERROR = -1;
static const char *sensor_name = "gyro";
int do_gyro_calibration(int mavlink_fd)
static const unsigned max_gyros = 3;
/// Data passed to calibration worker routine
typedef struct {
int mavlink_fd;
int32_t device_id[max_gyros];
int gyro_sensor_sub[max_gyros];
struct gyro_scale gyro_scale[max_gyros];
struct gyro_report gyro_report_0;
} gyro_worker_data_t;
static calibrate_return gyro_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
const unsigned max_gyros = 3;
int32_t device_id[3];
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "HOLD STILL");
/* wait for the user to respond */
sleep(2);
struct gyro_scale gyro_scale_zero = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
struct gyro_scale gyro_scale[max_gyros] = {};
int res = OK;
char str[30];
gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data);
unsigned calibration_counter[max_gyros] = { 0 };
const unsigned calibration_count = 5000;
struct gyro_report gyro_report;
unsigned poll_errcount = 0;
struct pollfd fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) {
/* ensure all scale fields are initialized tha same as the first struct */
(void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0]));
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
int fd = px4_open(str, 0);
if (fd < 0) {
continue;
fds[s].fd = worker_data->gyro_sensor_sub[s];
fds[s].events = POLLIN;
}
memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));
/* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) {
if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
return calibrate_return_cancelled;
}
device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
px4_close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
if (poll_ret > 0) {
for (unsigned s = 0; s < max_gyros; s++) {
bool changed;
orb_check(worker_data->gyro_sensor_sub[s], &changed);
if (changed) {
orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report);
if (s == 0) {
orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0);
}
worker_data->gyro_scale[s].x_offset += gyro_report.x;
worker_data->gyro_scale[s].y_offset += gyro_report.y;
worker_data->gyro_scale[s].z_offset += gyro_report.z;
calibration_counter[s]++;
}
if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
}
}
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
return calibrate_return_error;
}
}
unsigned calibration_counter[max_gyros] = { 0 };
const unsigned calibration_count = 5000;
struct gyro_report gyro_report_0 = {};
if (res == OK) {
/* determine gyro mean values */
unsigned poll_errcount = 0;
/* subscribe to gyro sensor topic */
int sub_sensor_gyro[max_gyros];
px4_pollfd_struct_t fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) {
sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
fds[s].fd = sub_sensor_gyro[s];
fds[s].events = POLLIN;
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s)
return calibrate_return_error;
}
struct gyro_report gyro_report;
worker_data->gyro_scale[s].x_offset /= calibration_counter[s];
worker_data->gyro_scale[s].y_offset /= calibration_counter[s];
worker_data->gyro_scale[s].z_offset /= calibration_counter[s];
}
/* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) {
/* wait blocking for new data */
return calibrate_return_ok;
}
int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
int do_gyro_calibration(int mavlink_fd)
{
int res = OK;
gyro_worker_data_t worker_data;
if (poll_ret > 0) {
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
for (unsigned s = 0; s < max_gyros; s++) {
bool changed;
orb_check(sub_sensor_gyro[s], &changed);
if (changed) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report);
if (s == 0) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0);
}
gyro_scale[s].x_offset += gyro_report.x;
gyro_scale[s].y_offset += gyro_report.y;
gyro_scale[s].z_offset += gyro_report.z;
calibration_counter[s]++;
}
if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count);
}
}
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
res = ERROR;
break;
}
worker_data.mavlink_fd = mavlink_fd;
struct gyro_scale gyro_scale_zero = {
0.0f, // x offset
1.0f, // x scale
0.0f, // y offset
1.0f, // y scale
0.0f, // z offset
1.0f, // z scale
};
for (unsigned s = 0; s < max_gyros; s++) {
char str[30];
// Reset gyro ids to unavailable
worker_data.device_id[s] = 0;
(void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
if (res != OK) {
mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
return ERROR;
}
// Reset all offsets to 0 and scales to 1
(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale));
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = open(str, 0);
if (fd >= 0) {
worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
close(fd);
<<<<<<< HEAD
for (unsigned s = 0; s < max_gyros; s++) {
px4_close(sub_sensor_gyro[s]);
gyro_scale[s].x_offset /= calibration_counter[s];
gyro_scale[s].y_offset /= calibration_counter[s];
gyro_scale[s].z_offset /= calibration_counter[s];
=======
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
return ERROR;
}
>>>>>>> upstream/master
}
}
for (unsigned s = 0; s < max_gyros; s++) {
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
}
// Calibrate right-side up
bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false };
int cancel_sub = calibrate_cancel_subscribe();
calibrate_return cal_return = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output
cancel_sub, // Subscription to vehicle_command for cancel support
side_collected, // Sides to calibrate
gyro_calibration_worker, // Calibration worker
&worker_data, // Opaque data for calibration worked
true); // true: lenient still detection
calibrate_cancel_unsubscribe(cancel_sub);
for (unsigned s = 0; s < max_gyros; s++) {
close(worker_data.gyro_sensor_sub[s]);
}
if (cal_return == calibrate_return_cancelled) {
// Cancel message already sent, we are done here
return ERROR;
} else if (cal_return == calibrate_return_error) {
res = ERROR;
}
if (res == OK) {
/* check offsets */
float xdiff = gyro_report_0.x - gyro_scale[0].x_offset;
float ydiff = gyro_report_0.y - gyro_scale[0].y_offset;
float zdiff = gyro_report_0.z - gyro_scale[0].z_offset;
float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
/* maximum allowable calibration error in radians */
const float maxoff = 0.0055f;
<<<<<<< HEAD
if (!PX4_ISFINITE(gyro_scale[0].x_offset) ||
!PX4_ISFINITE(gyro_scale[0].y_offset) ||
!PX4_ISFINITE(gyro_scale[0].z_offset) ||
=======
if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
!isfinite(worker_data.gyro_scale[0].y_offset) ||
!isfinite(worker_data.gyro_scale[0].z_offset) ||
>>>>>>> upstream/master
fabsf(xdiff) > maxoff ||
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: Motion during calibration");
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
res = ERROR;
}
}
@@ -210,7 +264,28 @@ int do_gyro_calibration(int mavlink_fd)
bool failed = false;
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data.device_id[s] != 0) {
char str[30];
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].x_offset)));
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset)));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset)));
(void)sprintf(str, "CAL_GYRO%u_ID", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
/* apply new scaling and offsets */
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = open(str, 0);
if (fd < 0) {
failed = true;
continue;
}
<<<<<<< HEAD
/* if any reasonable amount of data is missing, skip */
if (calibration_counter[s] < calibration_count / 2) {
continue;
@@ -236,14 +311,19 @@ int do_gyro_calibration(int mavlink_fd)
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]);
px4_close(fd);
=======
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]);
close(fd);
>>>>>>> upstream/master
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG);
}
}
}
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params");
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to set offset params");
res = ERROR;
}
}
@@ -260,16 +340,21 @@ int do_gyro_calibration(int mavlink_fd)
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
}
}
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return res;
}
src/modules/commander/mag_calibration.cpp
+97 -95
View File
@@ -68,8 +68,7 @@ static const int ERROR = -1;
static const char *sensor_name = "mag";
static const unsigned max_mags = 3;
int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
int mag_calibration_worker(detect_orientation_return orientation, void* worker_data);
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
/// Data passed to calibration worker routine
typedef struct {
@@ -89,7 +88,7 @@ typedef struct {
int do_mag_calibration(int mavlink_fd)
{
mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
struct mag_scale mscale_null = {
0.0f,
@@ -116,7 +115,7 @@ int do_mag_calibration(int mavlink_fd)
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "Unabled to reset CAL_MAG%u_ID", cur_mag);
mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
@@ -134,15 +133,15 @@ int do_mag_calibration(int mavlink_fd)
result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (result != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag);
}
/* calibrate range */
if (result == OK) {
/* calibrate range */
result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag);
mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
result = OK;
}
@@ -151,39 +150,52 @@ int do_mag_calibration(int mavlink_fd)
px4_close(fd);
}
// Calibrate all mags at the same time
if (result == OK) {
// Calibrate all mags at the same time
result = mag_calibrate_all(mavlink_fd, device_ids);
}
if (result == OK) {
/* auto-save to EEPROM */
result = param_save_default();
if (result != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
switch (mag_calibrate_all(mavlink_fd, device_ids)) {
case calibrate_return_cancelled:
// Cancel message already displayed, we're done here
result = ERROR;
break;
case calibrate_return_ok:
/* auto-save to EEPROM */
result = param_save_default();
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
if (result == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
break;
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
}
// Fall through
default:
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
break;
}
}
if (result == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return result;
}
int mag_calibration_worker(detect_orientation_return orientation, void* data)
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
int result = OK;
calibrate_return result = calibrate_return_ok;
unsigned int calibration_counter_side;
mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "Rotate vehicle around the detected orientation");
mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
sleep(2);
uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
@@ -194,6 +206,11 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter_side < worker_data->calibration_points_perside) {
if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
// Wait clocking for new data on all mags
px4_pollfd_struct_t fds[max_mags];
size_t fd_count = 0;
@@ -225,8 +242,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
// Progress indicator for side
mavlink_and_console_log_info(worker_data->mavlink_fd,
"%s %s side calibration: progress <%u>",
sensor_name,
"[cal] %s side calibration: progress <%u>",
detect_orientation_str(orientation),
(unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
} else {
@@ -234,50 +250,25 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
}
if (poll_errcount > worker_data->calibration_points_perside * 3) {
result = ERROR;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG);
result = calibrate_return_error;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
break;
}
}
// Mark the opposite side as collected as well. No need to collect opposite side since it
// would generate similar points.
detect_orientation_return alternateOrientation = orientation;
switch (orientation) {
case DETECT_ORIENTATION_TAIL_DOWN:
alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN;
break;
case DETECT_ORIENTATION_NOSE_DOWN:
alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN;
break;
case DETECT_ORIENTATION_LEFT:
alternateOrientation = DETECT_ORIENTATION_RIGHT;
break;
case DETECT_ORIENTATION_RIGHT:
alternateOrientation = DETECT_ORIENTATION_LEFT;
break;
case DETECT_ORIENTATION_UPSIDE_DOWN:
alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP;
break;
case DETECT_ORIENTATION_RIGHTSIDE_UP:
alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN;
break;
case DETECT_ORIENTATION_ERROR:
warnx("Invalid orientation in mag_calibration_worker");
break;
if (result == calibrate_return_ok) {
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count);
}
worker_data->side_data_collected[alternateOrientation] = true;
mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation));
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count);
return result;
}
int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
{
int result = OK;
calibrate_return result = calibrate_return_ok;
mag_worker_data_t worker_data;
@@ -288,10 +279,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
worker_data.calibration_interval_perside_seconds = 20;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
// Initialize to collect all sides
for (size_t cur_side=0; cur_side<6; cur_side++) {
worker_data.side_data_collected[cur_side] = false;
}
// Collect: Right-side up, Left Side, Nose down
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
// Initialize to no subscription
@@ -313,21 +307,21 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");
result = ERROR;
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory");
result = calibrate_return_error;
}
}
// Setup subscriptions to mag sensors
if (result == OK) {
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
if (worker_data.sub_mag[cur_mag] < 0) {
mavlink_and_console_log_critical(mavlink_fd, "Mag #%u not found, abort", cur_mag);
result = ERROR;
mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
result = calibrate_return_error;
break;
}
}
@@ -335,7 +329,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
}
// Limit update rate to get equally spaced measurements over time (in ms)
if (result == OK) {
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
@@ -347,8 +341,18 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected, mag_calibration_worker, &worker_data);
result = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output
cancel_sub, // Subscription to vehicle_command for cancel support
worker_data.side_data_collected, // Sides to calibrate
mag_calibration_worker, // Calibration worker
&worker_data, // Opaque data for calibration worked
true); // true: lenient still detection
calibrate_cancel_unsubscribe(cancel_sub);
}
// Close subscriptions
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
@@ -366,7 +370,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
float sphere_radius[max_mags];
// Sphere fit the data to get calibration values
if (result == OK) {
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available and we should have values for it to calibrate
@@ -378,8 +382,8 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
&sphere_radius[cur_mag]);
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
mavlink_and_console_log_info(mavlink_fd, "ERROR: NaN in sphere fit for mag #%u", cur_mag);
result = ERROR;
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
result = calibrate_return_error;
}
}
}
@@ -392,7 +396,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
free(worker_data.z[cur_mag]);
}
if (result == OK) {
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
int fd_mag = -1;
@@ -403,27 +407,25 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
mavlink_and_console_log_info(mavlink_fd, "ERROR: unable to open mag device #%u", cur_mag);
result = ERROR;
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
result = calibrate_return_error;
}
if (result == OK) {
result = px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "ERROR: failed to get current calibration #%u", cur_mag);
result = ERROR;
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
result = calibrate_return_error;
}
}
if (result == OK) {
if (result == calibrate_return_ok) {
mscale.x_offset = sphere_x[cur_mag];
mscale.y_offset = sphere_y[cur_mag];
mscale.z_offset = sphere_z[cur_mag];
result = px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, cur_mag);
result = ERROR;
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
result = calibrate_return_error;
}
}
@@ -432,7 +434,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
px4_close(fd_mag);
}
if (result == OK) {
if (result == calibrate_return_ok) {
bool failed = false;
/* set parameters */
@@ -452,13 +454,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
if (failed) {
mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag);
result = ERROR;
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
result = calibrate_return_error;
} else {
mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f",
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
}
src/modules/commander/state_machine_helper.cpp
+31 -12
View File
@@ -140,6 +140,13 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
/* enforce lockdown in HIL */
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
armed->lockdown = true;
prearm_ret = OK;
status->condition_system_sensors_initialized = true;
/* recover from a prearm fail */
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
}
} else {
armed->lockdown = false;
@@ -177,7 +184,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Fail transition if power is not good
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
@@ -187,7 +194,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
// Check avionics rail voltages
if (status->avionics_power_rail_voltage < 4.75f) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
@@ -212,23 +219,35 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational.");
// Sensors need to be initialized for STANDBY state, except for HIL
if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(!status->condition_system_sensors_initialized)) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
feedback_provided = true;
valid_transition = false;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
/* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
// Check if we are trying to arm, checks look good but we are in STANDBY_ERROR
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
} else {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
}
feedback_provided = true;
} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete");
feedback_provided = true;
} else {
mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
}
feedback_provided = true;
}
// Finish up the state transition