From 53ff04e0160a1602719d8051035eca1c48daa078 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 6 Oct 2015 19:44:17 +0200 Subject: [PATCH] Commander: Finish preflight update for prime sensor IDs --- src/modules/commander/PreflightCheck.cpp | 30 ++++++++++++++----- .../commander/accelerometer_calibration.cpp | 12 ++++---- src/modules/commander/gyro_calibration.cpp | 5 ++-- src/modules/commander/mag_calibration.cpp | 12 ++++---- 4 files changed, 40 insertions(+), 19 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 61aa461a91..cdddd3b637 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -79,13 +79,16 @@ int check_calibration(int fd, const char* param_template) bool calibration_found; /* new style: ask device for calibration state */ - ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); + int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); calibration_found = (ret == OK); /* old style transition: check param values */ int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + char s[20]; + int instance = 0; + while (!calibration_found) { sprintf(s, param_template, instance); param_t parm = param_find(s); @@ -105,6 +108,7 @@ int check_calibration(int fd, const char* param_template) break; } } + instance++; } return !calibration_found; @@ -150,7 +154,7 @@ out: return success; } -static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic) +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id) { bool success = true; @@ -215,7 +219,7 @@ out: return success; } -static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) { bool success = true; @@ -255,7 +259,7 @@ out: return success; } -static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) { bool success = true; @@ -272,6 +276,18 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) return false; } + // TODO: There is no baro calibration yet, since no external baros exist + // int ret = check_calibration(fd, "CAL_BARO%u_ID"); + + // if (ret) { + // mavlink_and_console_log_critical(mavlink_fd, + // "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance); + // success = false; + // goto out; + // } + +//out: + px4_close(fd); return success; } @@ -371,7 +387,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { bool required = (i < max_mandatory_accel_count); - int device_id + int device_id; if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) { failed = true; @@ -436,9 +452,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present - // if (!prime_found) { + if (!prime_found) { // failed = true; - // } + } } /* ---- AIRSPEED ---- */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 2c52489fdf..d7a58e6254 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -156,6 +156,10 @@ static const int ERROR = -1; static const char *sensor_name = "accel"; +static int32_t device_id[max_accel_sens]; +static int device_prio_max = 0; +static int32_t device_id_primary = 0; + 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]); @@ -172,9 +176,6 @@ typedef struct { int do_accel_calibration(int mavlink_fd) { int fd; - int32_t device_id[max_accel_sens]; - int device_prio_max = 0; - int32_t device_id_primary = 0; mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); @@ -261,7 +262,7 @@ int do_accel_calibration(int mavlink_fd) bool failed = false; - failed = failed || (OK != param_set_no_notification("CAL_ACC_PRIME", &(device_id_primary))); + failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); @@ -376,7 +377,8 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel timestamps[i] = arp.timestamp; // Get priority - int32_t prio = orb_priority(work_data.subs[i]); + int32_t prio; + orb_priority(worker_data.subs[i], &prio); if (prio > device_prio_max) { device_prio_max = prio; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 6c8c840713..54866d8cb3 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -204,7 +204,8 @@ int do_gyro_calibration(int mavlink_fd) worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); // Get priority - int32_t prio = orb_priority(work_data.gyro_sensor_subs[s]); + int32_t prio; + orb_priority(worker_data.gyro_sensor_sub[s], &prio); if (prio > device_prio_max) { device_prio_max = prio; @@ -273,7 +274,7 @@ int do_gyro_calibration(int mavlink_fd) /* set offset parameters to new values */ bool failed = false; - failed = failed || (OK != param_set_no_notification("CAL_GYRO_PRIME", &(device_id_primary))); + failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary))); for (unsigned s = 0; s < max_gyros; s++) { if (worker_data.device_id[s] != 0) { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index c9bd48a5cc..eba8feec99 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,6 +73,10 @@ static constexpr unsigned int calibration_sides = 6; ///< The total number of static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take +int32_t device_ids[max_mags]; +int device_prio_max = 0; +int32_t device_id_primary = 0; + calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); /// Data passed to calibration worker routine @@ -108,9 +112,6 @@ int do_mag_calibration(int mavlink_fd) // Determine which mags are available and reset each - int32_t device_ids[max_mags]; - int device_prio_max = 0; - int32_t device_id_primary = 0; char str[30]; for (size_t i=0; i device_prio_max) { device_prio_max = prio; @@ -561,7 +563,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag if (result == calibrate_return_ok) { - (void)param_set_no_notification("CAL_MAG_PRIME", &(device_id_primary)); + (void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary)); for (unsigned cur_mag=0; cur_mag