forked from Archive/PX4-Autopilot
Commander: Finish preflight update for prime sensor IDs
This commit is contained in:
parent
e5bb1cff91
commit
53ff04e016
|
@ -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 ---- */
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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) {
|
||||
|
|
Loading…
Reference in New Issue