diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-27 22:34:08 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-27 22:34:08 +0200 |
commit | 3dd45b9d3467ee62ac0d604a5607baaa10089ac3 (patch) | |
tree | 7bf56e57c71bcfedc47a7af9be4bab290beb21da /src/modules | |
parent | 7b06d781d8ade137a1837a3ecf002579bddfbb14 (diff) | |
parent | bd1c3363df44170523c68fd87ee19f2582a5e8fc (diff) | |
download | px4-firmware-beta.tar.gz px4-firmware-beta.tar.bz2 px4-firmware-beta.zip |
Merge branch 'master' into betabeta
Diffstat (limited to 'src/modules')
28 files changed, 849 insertions, 620 deletions
diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index b0a587762..4d9bd8ae0 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -254,13 +254,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 } diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 409c2ea00..f83640d28 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -152,11 +152,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 { @@ -171,7 +170,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, @@ -202,7 +201,7 @@ int do_accel_calibration(int mavlink_fd) 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); } } @@ -210,13 +209,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; } @@ -263,7 +272,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; } @@ -271,7 +280,7 @@ int do_accel_calibration(int mavlink_fd) fd = 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 = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); @@ -279,7 +288,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); } } @@ -288,41 +297,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; @@ -343,7 +358,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; } @@ -353,8 +368,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 */ @@ -370,13 +387,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; } } @@ -388,7 +405,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) { struct pollfd fds[max_accel_sens]; @@ -432,7 +449,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; } } @@ -442,7 +459,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]) @@ -468,7 +485,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++) { @@ -490,7 +507,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 */ @@ -501,5 +518,5 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s } } - return OK; + return calibrate_return_ok; } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 747d915ff..837a3f1e8 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -38,6 +38,7 @@ #include "airspeed_calibration.h" #include "calibration_messages.h" +#include "calibration_routines.h" #include "commander_helper.h" #include <stdio.h> @@ -61,19 +62,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; @@ -96,11 +98,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"); } close(fd); } + + int cancel_sub = calibrate_cancel_subscribe(); if (!paramreset_successful) { @@ -108,26 +112,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"); - 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); - 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 */ struct pollfd fds[1]; fds[0].fd = diff_pres_sub; @@ -142,14 +146,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); - close(diff_pres_sub); - return ERROR; + goto error_return; } } @@ -161,16 +164,15 @@ int do_airspeed_calibration(int mavlink_fd) airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { if (OK != 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"); } close(fd_scale); } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); - return ERROR; + mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + goto error_return; } /* auto-save to EEPROM */ @@ -178,30 +180,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); - 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); - 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 */ struct pollfd fds[1]; fds[0].fd = diff_pres_sub; @@ -216,7 +219,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; @@ -224,30 +227,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!"); - 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); - 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(); - 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; } @@ -255,21 +254,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); - close(diff_pres_sub); - return ERROR; + goto error_return; } } if (calibration_counter == maxcount) { feedback_calibration_failed(mavlink_fd); - 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); 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; } diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 29f44dc36..7b4baae62 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -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_ */ diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 192b8c727..7e8c0fa52 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -48,6 +48,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" @@ -230,23 +232,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 + struct pollfd fds[1]; fds[0].fd = accel_sub; fds[0].events = POLLIN; @@ -308,7 +306,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; @@ -325,8 +323,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; } } @@ -340,7 +338,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; } } @@ -381,7 +379,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 } @@ -401,28 +399,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 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 = 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; } @@ -450,44 +455,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; +}
\ No newline at end of file diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 8f34f0204..b8232730a 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -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 +}; + +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); -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); +/// 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 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5e0346155..7aaf5e5cd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -182,21 +182,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 @@ -228,6 +213,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); @@ -552,8 +539,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.hil_state != vehicle_status_s::HIL_STATE_ON && !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; @@ -960,10 +947,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"; @@ -1032,7 +1019,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; @@ -1129,6 +1116,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 */ @@ -1136,9 +1125,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 { @@ -1170,7 +1159,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, ¶m); @@ -1222,14 +1211,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; @@ -1290,6 +1272,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) { @@ -1611,8 +1598,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); @@ -1620,8 +1606,6 @@ int commander_thread_main(int argc, char *argv[]) arming_state_changed = true; } - } else { - /* TODO: Add emergency stuff if sensors are lost */ } @@ -2121,6 +2105,19 @@ int commander_thread_main(int argc, char *argv[]) } 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) { hrt_abstime t = hrt_absolute_time(); @@ -2669,7 +2666,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; } @@ -2718,51 +2715,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); - } 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. - // 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; + } - 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); - 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); - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + } else { + tune_negative(true); + } 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); - if (((int)(cmd.param1)) == 0) { int ret = param_load_default(); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 5f735b7b7..99b926be4 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -85,8 +85,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; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 291ccfe80..bdef8771e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -39,6 +39,7 @@ #include "gyro_calibration.h" #include "calibration_messages.h" +#include "calibration_routines.h" #include "commander_helper.h" #include <stdio.h> @@ -62,142 +63,180 @@ static const int ERROR = -1; static const char *sensor_name = "gyro"; -int do_gyro_calibration(int mavlink_fd) -{ - 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] = {}; +static const unsigned max_gyros = 3; - int res = OK; - - char str[30]; +/// 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) +{ + 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 = 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] = ioctl(fd, DEVIOCGDEVICEID, 0); - - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); - close(fd); - - if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); + + int poll_ret = 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]; - struct pollfd 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; - - /* use first gyro to pace, but count correctly per-gyro for statistics */ - while (calibration_counter[0] < calibration_count) { - /* wait blocking for new data */ - - int poll_ret = poll(&fds[0], max_gyros, 1000); - - if (poll_ret > 0) { - - 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); - } + 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]; + } - 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]++; - } + return calibrate_return_ok; +} - 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); - } - } +int do_gyro_calibration(int mavlink_fd) +{ + int res = OK; + gyro_worker_data_t worker_data; - } else { - poll_errcount++; - } + mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); - 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); - for (unsigned s = 0; s < max_gyros; s++) { - 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; + } } + + } + + 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; - if (!isfinite(gyro_scale[0].x_offset) || - !isfinite(gyro_scale[0].y_offset) || - !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) || 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; } } @@ -207,40 +246,38 @@ 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; + } - /* if any reasonable amount of data is missing, skip */ - if (calibration_counter[s] < calibration_count / 2) { - continue; - } - - (void)sprintf(str, "CAL_GYRO%u_XOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].x_offset))); - (void)sprintf(str, "CAL_GYRO%u_YOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].y_offset))); - (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].z_offset))); - (void)sprintf(str, "CAL_GYRO%u_ID", s); - failed |= (OK != param_set_no_notification(param_find(str), &(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; - } - - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]); - close(fd); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]); + close(fd); - 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; } } @@ -257,16 +294,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; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 198acb027..8a8d85818 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -65,8 +65,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 { @@ -86,7 +85,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, @@ -113,7 +112,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; } @@ -131,15 +130,15 @@ int do_mag_calibration(int mavlink_fd) result = 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 = 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; } @@ -148,39 +147,52 @@ int do_mag_calibration(int mavlink_fd) 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; @@ -191,6 +203,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 struct pollfd fds[max_mags]; size_t fd_count = 0; @@ -222,8 +239,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 { @@ -231,50 +247,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; @@ -285,10 +276,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 @@ -310,21 +304,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; } } @@ -332,7 +326,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 @@ -344,8 +338,18 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) } } - - result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected, mag_calibration_worker, &worker_data); + + if (result == calibrate_return_ok) { + int cancel_sub = calibrate_cancel_subscribe(); + + 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++) { @@ -363,7 +367,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 @@ -375,8 +379,8 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) &sphere_radius[cur_mag]); if (!isfinite(sphere_x[cur_mag]) || !isfinite(sphere_y[cur_mag]) || !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; } } } @@ -389,7 +393,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; @@ -400,27 +404,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 = 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 = 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 (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 = 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 (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; } } @@ -429,7 +431,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) close(fd_mag); } - if (result == OK) { + if (result == calibrate_return_ok) { bool failed = false; /* set parameters */ @@ -449,13 +451,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); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9aba61e14..73fdb0940 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -140,6 +140,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s 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; } @@ -211,23 +216,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_and_console_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_and_console_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_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); + } - feedback_provided = true; } // Finish up the state transition diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 68bf12024..b442b7430 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -685,20 +685,21 @@ task_main(int argc, char *argv[]) fsync(g_task_fd); + printf("dataman: "); /* see if we need to erase any items based on restart type */ int sys_restart_val; if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) { if (sys_restart_val == DM_INIT_REASON_POWER_ON) { - warnx("Power on restart"); + printf("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { - warnx("In flight restart"); + printf("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); } else { - warnx("Unknown restart"); + printf("Unknown restart"); } } else { - warnx("Unknown restart"); + printf("Unknown restart"); } /* We use two file descriptors, one for the caller context and one for the worker thread */ @@ -706,7 +707,7 @@ task_main(int argc, char *argv[]) /* worker thread is shutting down but still processing requests */ g_fd = g_task_fd; - warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset); + printf(", data manager file '%s' size is %d bytes\n", k_data_manager_device_path, max_offset); /* Tell startup that the worker thread has completed its initialization */ sem_post(&g_init_sema); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ba049bac4..22ff3edf6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 20000 ///< max data rate in bytes/s +#define MAX_DATA_RATE 60000 ///< max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate #define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it. diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 276035aa2..7ddc52fd1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2231,7 +2231,7 @@ protected: }; -StreamListItem *streams_list[] = { +const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 7e4416609..5b6b9d633 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -56,6 +56,6 @@ public: ~StreamListItem() {}; }; -extern StreamListItem *streams_list[]; +extern const StreamListItem *streams_list[]; #endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 20d7cfdbb..9c8794017 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -130,7 +130,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) } else { /* when index is >= 0, send this parameter again */ - send_param(param_for_index(req_read.param_index)); + send_param(param_for_used_index(req_read.param_index)); } } break; @@ -192,6 +192,7 @@ MavlinkParametersManager::send(const hrt_abstime t) /* look for the first parameter which is used */ param_t p; do { + /* walk through all parameters, including unused ones */ p = param_for_index(_send_all_index); _send_all_index++; } while (p != PARAM_INVALID && !param_used(p)); @@ -200,7 +201,7 @@ MavlinkParametersManager::send(const hrt_abstime t) send_param(p); } - if (_send_all_index >= (int) param_count()) { + if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) { _send_all_index = -1; } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index faede15cb..c4e332bf1 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -822,7 +822,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ if (!(_offboard_control_mode.ignore_attitude)) { - static struct vehicle_attitude_setpoint_s att_sp = {}; + struct vehicle_attitude_setpoint_s att_sp = {}; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 3a0dfe4c3..ec512ab56 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -908,7 +908,7 @@ MulticopterAttitudeControl::start() _control_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 1600, + 1500, (main_t)&MulticopterAttitudeControl::task_main_trampoline, nullptr); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 5191a4de3..6ffb37d97 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1426,7 +1426,7 @@ MulticopterPositionControl::start() _control_task = task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 1600, + 1500, (main_t)&MulticopterPositionControl::task_main_trampoline, nullptr); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 904f44238..949231b15 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -134,7 +134,7 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, { /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ for (size_t i = 0; i < nMissionItems; i++) { - static struct mission_item_s missionitem; + struct mission_item_s missionitem; const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9691e26a8..042f926d3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -518,7 +518,7 @@ Navigator::start() _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, - 1800, + 1700, (main_t)&Navigator::task_main_trampoline, nullptr); diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 7800a6f03..3d8c78cf1 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -48,18 +48,6 @@ */ /** - * Loiter radius after RTL (FW only) - * - * Default value of loiter radius after RTL (fixedwing only). - * - * @unit meters - * @min 20 - * @max 200 - * @group Return To Land - */ -PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); - -/** * RTL altitude * * Altitude to fly back in RTL in meters diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e2274342e..ae4913559 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -51,7 +51,6 @@ #include <errno.h> #include <unistd.h> #include <stdio.h> -#include <poll.h> #include <stdlib.h> #include <string.h> #include <ctype.h> @@ -151,11 +150,6 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1); log_msgs_skipped++; \ } -#define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ - fds[fdsc_count].fd = subs.##_var##_sub; \ - fds[fdsc_count].events = POLLIN; \ - fdsc_count++; - #define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) static bool main_thread_should_exit = false; /**< Deamon exit flag */ @@ -218,7 +212,7 @@ static void *logwriter_thread(void *arg); */ __EXPORT int sdlog2_main(int argc, char *argv[]); -static bool copy_if_updated(orb_id_t topic, int handle, void *buffer); +static bool copy_if_updated(orb_id_t topic, int *handle, void *buffer); /** * Mainloop of sd log deamon. @@ -811,14 +805,25 @@ int write_parameters(int fd) return written; } -bool copy_if_updated(orb_id_t topic, int handle, void *buffer) +bool copy_if_updated(orb_id_t topic, int *handle, void *buffer) { - bool updated; - - orb_check(handle, &updated); + bool updated = false; + + if (*handle < 0) { + if (OK == orb_exists(topic, 0)) { + *handle = orb_subscribe(topic); + /* copy first data */ + if (*handle >= 0) { + orb_copy(topic, *handle, buffer); + updated = true; + } + } + } else { + orb_check(*handle, &updated); - if (updated) { - orb_copy(topic, handle, buffer); + if (updated) { + orb_copy(topic, *handle, buffer); + } } return updated; @@ -1121,54 +1126,47 @@ int sdlog2_thread_main(int argc, char *argv[]) int mc_att_ctrl_status_sub; } subs; - subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); - subs.vtol_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); - subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); - subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1)); - subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate)); - subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); - subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); - subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); - subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); - subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); - subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); - subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); - subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); - subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); - subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); - subs.tsync_sub = orb_subscribe(ORB_ID(time_offset)); - subs.mc_att_ctrl_status_sub = orb_subscribe(ORB_ID(mc_att_ctrl_status)); - - /* we need to rate-limit wind, as we do not need the full update rate */ - orb_set_interval(subs.wind_sub, 90); - subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); + subs.cmd_sub = -1; + subs.status_sub = -1; + subs.vtol_status_sub = -1; + subs.gps_pos_sub = -1; + subs.sensor_sub = -1; + subs.att_sub = -1; + subs.att_sp_sub = -1; + subs.rates_sp_sub = -1; + subs.act_outputs_sub = -1; + subs.act_controls_sub = -1; + subs.act_controls_1_sub = -1; + subs.local_pos_sub = -1; + subs.local_pos_sp_sub = -1; + subs.global_pos_sub = -1; + subs.triplet_sub = -1; + subs.vicon_pos_sub = -1; + subs.vision_pos_sub = -1; + subs.flow_sub = -1; + subs.rc_sub = -1; + subs.airspeed_sub = -1; + subs.esc_sub = -1; + subs.global_vel_sp_sub = -1; + subs.battery_sub = -1; + subs.range_finder_sub = -1; + subs.estimator_status_sub = -1; + subs.tecs_status_sub = -1; + subs.system_power_sub = -1; + subs.servorail_status_sub = -1; + subs.wind_sub = -1; + subs.tsync_sub = -1; + subs.mc_att_ctrl_status_sub = -1; + subs.encoders_sub = -1; /* add new topics HERE */ - for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); - } - - if (_extended_logging) { - subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); - } else { - subs.sat_info_sub = 0; + for (unsigned i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = -1; } + + subs.sat_info_sub = -1; /* close non-needed fd's */ @@ -1216,12 +1214,12 @@ int sdlog2_thread_main(int argc, char *argv[]) usleep(sleep_delay); /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ - if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) { + if (copy_if_updated(ORB_ID(vehicle_command), &subs.cmd_sub, &buf.cmd)) { handle_command(&buf.cmd); } /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ - bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + bool status_updated = copy_if_updated(ORB_ID(vehicle_status), &subs.status_sub, &buf_status); if (status_updated) { if (log_when_armed) { @@ -1230,7 +1228,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GPS POSITION - LOG MANAGEMENT --- */ - bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos); + bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { gps_time = buf_gps_pos.time_utc_usec; @@ -1261,7 +1259,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- VTOL VEHICLE STATUS --- */ - if(copy_if_updated(ORB_ID(vtol_vehicle_status), subs.vtol_status_sub, &buf.vtol_status)) { + if(copy_if_updated(ORB_ID(vtol_vehicle_status), &subs.vtol_status_sub, &buf.vtol_status)) { log_msg.msg_type = LOG_VTOL_MSG; log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot; LOGBUFFER_WRITE_AND_COUNT(VTOL); @@ -1292,7 +1290,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SATELLITE INFO - UNIT #1 --- */ if (_extended_logging) { - if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) { + if (copy_if_updated(ORB_ID(satellite_info), &subs.sat_info_sub, &buf.sat_info)) { /* log the SNR of each satellite for a detailed view of signal quality */ unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0])); @@ -1338,7 +1336,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- SENSOR COMBINED --- */ - if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) { + if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) { bool write_IMU = false; bool write_IMU1 = false; bool write_IMU2 = false; @@ -1480,7 +1478,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ATTITUDE --- */ - if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) { + if (copy_if_updated(ORB_ID(vehicle_attitude), &subs.att_sub, &buf.att)) { log_msg.msg_type = LOG_ATT_MSG; log_msg.body.log_ATT.q_w = buf.att.q[0]; log_msg.body.log_ATT.q_x = buf.att.q[1]; @@ -1499,7 +1497,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ATTITUDE SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp)) { + if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), &subs.att_sp_sub, &buf.att_sp)) { log_msg.msg_type = LOG_ATSP_MSG; log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; @@ -1513,7 +1511,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- RATES SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp)) { + if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), &subs.rates_sp_sub, &buf.rates_sp)) { log_msg.msg_type = LOG_ARSP_MSG; log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll; log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch; @@ -1522,14 +1520,14 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ACTUATOR OUTPUTS --- */ - if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) { + if (copy_if_updated(ORB_ID(actuator_outputs), &subs.act_outputs_sub, &buf.act_outputs)) { log_msg.msg_type = LOG_OUT0_MSG; memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); LOGBUFFER_WRITE_AND_COUNT(OUT0); } /* --- ACTUATOR CONTROL --- */ - if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls)) { + if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &subs.act_controls_sub, &buf.act_controls)) { log_msg.msg_type = LOG_ATTC_MSG; log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; @@ -1539,7 +1537,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ACTUATOR CONTROL FW VTOL --- */ - if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) { + if(copy_if_updated(ORB_ID(actuator_controls_1), &subs.act_controls_1_sub,&buf.act_controls)) { log_msg.msg_type = LOG_ATC1_MSG; log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; @@ -1549,7 +1547,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- LOCAL POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) { + if (copy_if_updated(ORB_ID(vehicle_local_position), &subs.local_pos_sub, &buf.local_pos)) { log_msg.msg_type = LOG_LPOS_MSG; log_msg.body.log_LPOS.x = buf.local_pos.x; log_msg.body.log_LPOS.y = buf.local_pos.y; @@ -1575,7 +1573,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- LOCAL POSITION SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) { + if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), &subs.local_pos_sp_sub, &buf.local_pos_sp)) { log_msg.msg_type = LOG_LPSP_MSG; log_msg.body.log_LPSP.x = buf.local_pos_sp.x; log_msg.body.log_LPSP.y = buf.local_pos_sp.y; @@ -1591,7 +1589,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GLOBAL POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos)) { + if (copy_if_updated(ORB_ID(vehicle_global_position), &subs.global_pos_sub, &buf.global_pos)) { log_msg.msg_type = LOG_GPOS_MSG; log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; @@ -1610,7 +1608,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GLOBAL POSITION SETPOINT --- */ - if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { + if (copy_if_updated(ORB_ID(position_setpoint_triplet), &subs.triplet_sub, &buf.triplet)) { if (buf.triplet.current.valid) { log_msg.msg_type = LOG_GPSP_MSG; @@ -1628,7 +1626,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- VICON POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { + if (copy_if_updated(ORB_ID(vehicle_vicon_position), &subs.vicon_pos_sub, &buf.vicon_pos)) { log_msg.msg_type = LOG_VICN_MSG; log_msg.body.log_VICN.x = buf.vicon_pos.x; log_msg.body.log_VICN.y = buf.vicon_pos.y; @@ -1640,7 +1638,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- VISION POSITION --- */ - if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) { + if (copy_if_updated(ORB_ID(vision_position_estimate), &subs.vision_pos_sub, &buf.vision_pos)) { log_msg.msg_type = LOG_VISN_MSG; log_msg.body.log_VISN.x = buf.vision_pos.x; log_msg.body.log_VISN.y = buf.vision_pos.y; @@ -1656,7 +1654,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- FLOW --- */ - if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { + if (copy_if_updated(ORB_ID(optical_flow), &subs.flow_sub, &buf.flow)) { log_msg.msg_type = LOG_FLOW_MSG; log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature; @@ -1672,7 +1670,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- RC CHANNELS --- */ - if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) { + if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); @@ -1682,7 +1680,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- AIRSPEED --- */ - if (copy_if_updated(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed)) { + if (copy_if_updated(ORB_ID(airspeed), &subs.airspeed_sub, &buf.airspeed)) { log_msg.msg_type = LOG_AIRS_MSG; log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; @@ -1691,7 +1689,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ESCs --- */ - if (copy_if_updated(ORB_ID(esc_status), subs.esc_sub, &buf.esc)) { + if (copy_if_updated(ORB_ID(esc_status), &subs.esc_sub, &buf.esc)) { for (uint8_t i = 0; i < buf.esc.esc_count; i++) { log_msg.msg_type = LOG_ESC_MSG; log_msg.body.log_ESC.counter = buf.esc.counter; @@ -1711,7 +1709,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- GLOBAL VELOCITY SETPOINT --- */ - if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp)) { + if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), &subs.global_vel_sp_sub, &buf.global_vel_sp)) { log_msg.msg_type = LOG_GVSP_MSG; log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; @@ -1720,7 +1718,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- BATTERY --- */ - if (copy_if_updated(ORB_ID(battery_status), subs.battery_sub, &buf.battery)) { + if (copy_if_updated(ORB_ID(battery_status), &subs.battery_sub, &buf.battery)) { log_msg.msg_type = LOG_BATT_MSG; log_msg.body.log_BATT.voltage = buf.battery.voltage_v; log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v; @@ -1730,7 +1728,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- SYSTEM POWER RAILS --- */ - if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) { + if (copy_if_updated(ORB_ID(system_power), &subs.system_power_sub, &buf.system_power)) { log_msg.msg_type = LOG_PWR_MSG; log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v; log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; @@ -1748,8 +1746,8 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TELEMETRY --- */ - for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) { + for (unsigned i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (copy_if_updated(telemetry_status_orb_id[i], &subs.telemetry_subs[i], &buf.telemetry)) { log_msg.msg_type = LOG_TEL0_MSG + i; log_msg.body.log_TEL.rssi = buf.telemetry.rssi; log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi; @@ -1764,7 +1762,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- BOTTOM DISTANCE --- */ - if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { + if (copy_if_updated(ORB_ID(sensor_range_finder), &subs.range_finder_sub, &buf.range_finder)) { log_msg.msg_type = LOG_DIST_MSG; log_msg.body.log_DIST.bottom = buf.range_finder.distance; log_msg.body.log_DIST.bottom_rate = 0.0f; @@ -1773,7 +1771,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ESTIMATOR STATUS --- */ - if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { + if (copy_if_updated(ORB_ID(estimator_status), &subs.estimator_status_sub, &buf.estimator_status)) { log_msg.msg_type = LOG_EST0_MSG; unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s); memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s)); @@ -1792,7 +1790,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TECS STATUS --- */ - if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { + if (copy_if_updated(ORB_ID(tecs_status), &subs.tecs_status_sub, &buf.tecs_status)) { log_msg.msg_type = LOG_TECS_MSG; log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; @@ -1812,7 +1810,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- WIND ESTIMATE --- */ - if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) { + if (copy_if_updated(ORB_ID(wind_estimate), &subs.wind_sub, &buf.wind_estimate)) { log_msg.msg_type = LOG_WIND_MSG; log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north; log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east; @@ -1822,7 +1820,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ENCODERS --- */ - if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) { + if (copy_if_updated(ORB_ID(encoders), &subs.encoders_sub, &buf.encoders)) { log_msg.msg_type = LOG_ENCD_MSG; log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0]; log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0]; @@ -1832,14 +1830,14 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TIMESYNC OFFSET --- */ - if (copy_if_updated(ORB_ID(time_offset), subs.tsync_sub, &buf.time_offset)) { + if (copy_if_updated(ORB_ID(time_offset), &subs.tsync_sub, &buf.time_offset)) { log_msg.msg_type = LOG_TSYN_MSG; log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns; LOGBUFFER_WRITE_AND_COUNT(TSYN); } /* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */ - if (copy_if_updated(ORB_ID(mc_att_ctrl_status), subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) { + if (copy_if_updated(ORB_ID(mc_att_ctrl_status), &subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) { log_msg.msg_type = LOG_MACS_MSG; log_msg.body.log_MACS.roll_rate_integ = buf.mc_att_ctrl_status.roll_rate_integ; log_msg.body.log_MACS.pitch_rate_integ = buf.mc_att_ctrl_status.pitch_rate_integ; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fc8627c1..4fbc0416e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -145,7 +145,7 @@ #endif static const int ERROR = -1; -#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u" +#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u" /** * Sensor app start / stop handling function @@ -246,6 +246,7 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; struct rc_parameter_map_s _rc_parameter_map; + float _param_rc_values[RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */ math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ @@ -523,6 +524,7 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), + _param_rc_values{}, _board_rotation{}, _mag_rotation{}, @@ -620,6 +622,18 @@ Sensors::Sensors() : /* Barometer QNH */ _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); + // These are parameters for which QGroundControl always expects to be returned in a list request. + // We do a param_find here to force them into the list. + (void)param_find("RC_CHAN_CNT"); + (void)param_find("RC_TH_USER"); + (void)param_find("CAL_MAG0_ID"); + (void)param_find("CAL_MAG1_ID"); + (void)param_find("CAL_MAG2_ID"); + (void)param_find("CAL_MAG0_ROT"); + (void)param_find("CAL_MAG1_ROT"); + (void)param_find("CAL_MAG2_ROT"); + (void)param_find("SYS_PARAM_VER"); + /* fetch initial parameter values */ parameters_update(); } @@ -1382,12 +1396,12 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); if (failed) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); + warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); } else { /* apply new scaling and offsets */ res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); + warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); } else { config_ok = true; } @@ -1448,12 +1462,12 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); if (failed) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); + warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); } else { /* apply new scaling and offsets */ res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); + warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); } else { config_ok = true; } @@ -1571,12 +1585,12 @@ Sensors::parameter_update_poll(bool forced) } if (failed) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); + warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); } else { /* apply new scaling and offsets */ res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); + warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); } else { config_ok = true; } @@ -1609,7 +1623,8 @@ Sensors::parameter_update_poll(bool forced) close(fd); } - warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); + /* do not output this for now, as its covered in preflight checks */ + // warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); } } @@ -1633,7 +1648,7 @@ Sensors::rc_parameter_map_poll(bool forced) /* Set the handle by index if the index is set, otherwise use the id */ if (_rc_parameter_map.param_index[i] >= 0) { - _parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]); + _parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]); } else { _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); @@ -1838,8 +1853,6 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) void Sensors::set_params_from_rc() { - static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) @@ -1851,8 +1864,8 @@ Sensors::set_params_from_rc() float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ - if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { - param_rc_values[i] = rc_val; + if (rc_val > _param_rc_values[i] + FLT_EPSILON || rc_val < _param_rc_values[i] - FLT_EPSILON) { + _param_rc_values[i] = rc_val; float param_val = math::constrain( _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 8dea6e6cc..4ec885ab3 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -92,7 +92,7 @@ struct param_wbuf_s { }; // XXX this should be param_info_count, but need to work out linking -uint8_t param_changed_storage[(600 / sizeof(uint8_t)) + 1] = {}; +uint8_t param_changed_storage[(700 / sizeof(uint8_t)) + 1] = {}; /** flexible array holding modified parameter values */ UT_array *param_values; @@ -265,8 +265,37 @@ param_count_used(void) param_t param_for_index(unsigned index) { - if (index < param_info_count) + if (index < param_info_count) { return (param_t)index; + } + + return PARAM_INVALID; +} + +param_t +param_for_used_index(unsigned index) +{ + if (index < param_info_count) { + + /* walk all params and count */ + int count = 0; + + for (unsigned i = 0; i < (unsigned)param_info_count + 1; i++) { + for (unsigned j = 0; j < 8; j++) { + if (param_changed_storage[i] & (1 << j)) { + + /* we found the right used count, + * return the param value + */ + if (index == count) { + return (param_t)i; + } + + count++; + } + } + } + } return PARAM_INVALID; } @@ -274,8 +303,9 @@ param_for_index(unsigned index) int param_get_index(param_t param) { - if (handle_in_range(param)) + if (handle_in_range(param)) { return (unsigned)param; + } return -1; } @@ -283,7 +313,9 @@ param_get_index(param_t param) int param_get_used_index(param_t param) { - if (!handle_in_range(param)) { + int param_storage_index = param_get_index(param); + + if (param_storage_index < 0) { return -1; } @@ -293,12 +325,17 @@ param_get_used_index(param_t param) for (unsigned i = 0; i < (unsigned)param + 1; i++) { for (unsigned j = 0; j < 8; j++) { if (param_changed_storage[i] & (1 << j)) { + + if (param_storage_index == i) { + return count; + } + count++; } } } - return count; + return -1; } const char * diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index b29a7e51d..9cbe3570b 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -130,6 +130,14 @@ __EXPORT bool param_used(param_t param); __EXPORT param_t param_for_index(unsigned index); /** + * Look up an used parameter by index. + * + * @param param The parameter to obtain the index for. + * @return The index of the parameter in use, or -1 if the parameter does not exist. + */ +__EXPORT param_t param_for_used_index(unsigned index); + +/** * Look up the index of a parameter. * * @param param The parameter to obtain the index for. diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index b4f81d429..d531c6b0b 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -41,6 +41,7 @@ #include <drivers/device/device.h> #include <sys/types.h> +#include <sys/stat.h> #include <stdint.h> #include <stdbool.h> #include <string.h> @@ -1196,6 +1197,25 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve } // namespace +int +orb_exists(const struct orb_metadata *meta, int instance) +{ + /* + * Generate the path to the node and try to open it. + */ + char path[orb_maxpath]; + int inst = instance; + int ret = node_mkpath(path, PUBSUB, meta, &inst); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + + struct stat buffer; + return stat(path, &buffer); +} + orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 44dc6614f..a8b19d91f 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -319,6 +319,15 @@ extern int orb_check(int handle, bool *updated) __EXPORT; extern int orb_stat(int handle, uint64_t *time) __EXPORT; /** + * Check if a topic has already been created. + * + * @param meta ORB topic metadata. + * @param instance ORB instance + * @return OK if the topic exists, ERROR otherwise with errno set accordingly. + */ +extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT; + +/** * Return the priority of the topic * * @param handle A handle returned from orb_subscribe. |