Commander: Finish preflight update for prime sensor IDs

This commit is contained in:
Lorenz Meier 2015-10-06 19:44:17 +02:00
parent e5bb1cff91
commit 53ff04e016
4 changed files with 40 additions and 19 deletions

View File

@ -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 ---- */

View File

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

View File

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

View File

@ -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<max_mags; i++) {
@ -438,7 +439,8 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
}
// Get priority
int32_t prio = orb_priority(work_data.sub_mag[cur_mag]);
int32_t prio;
orb_priority(worker_data.sub_mag[cur_mag], &prio);
if (prio > 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<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {