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

Commander: Finish preflight update for prime sensor IDs

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+7 -5
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;