fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 17:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: get PRIME parameter back

Browse Source 
The PRIME param got lost on the way of the refactoring.
This commit is contained in:
Julian Oes
2016-03-22 10:49:31 +01:00
parent a6af53bcb4
commit e949d6d18a
3 changed files with 19 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+1 -8
View File
@@ -157,6 +157,7 @@ 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);
@@ -425,18 +426,10 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
int32_t prio;
orb_priority(worker_data.subs[i], &prio);
#ifndef __PX4_QURT
int device_prio_max = 0;
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[i];
}
#else
PX4_INFO("found device id: %d", arp.device_id);
// TODO FIXME: this is hacky but should get the device ID for now
device_id[i] = arp.device_id;
#endif
}
if (result == calibrate_return_ok) {
src/modules/commander/gyro_calibration.cpp
+1 -10
View File
@@ -167,6 +167,7 @@ int do_gyro_calibration(int mavlink_fd)
gyro_scale_zero.z_offset = 0.0f;
gyro_scale_zero.z_scale = 1.0f;
int device_prio_max = 0;
int32_t device_id_primary = 0;
for (unsigned s = 0; s < max_gyros; s++) {
@@ -238,20 +239,10 @@ int do_gyro_calibration(int mavlink_fd)
int32_t prio;
orb_priority(worker_data.gyro_sensor_sub[s], &prio);
#ifndef __PX4_QURT
int device_prio_max = 0;
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = worker_data.device_id[s];
}
#else
gyro_report report = {};
orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &report);
//PX4_INFO("found device id: %d", report.device_id);
// TODO FIXME: this is hacky but should get the device ID for now
worker_data.device_id[s] = report.device_id;
#endif
}
int cancel_sub = calibrate_cancel_subscribe();
src/modules/commander/mag_calibration.cpp
+17 -24
View File
@@ -469,31 +469,24 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
// Setup subscriptions to mag sensors
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
// 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, "[cal] Mag #%u not found, abort", cur_mag);
result = calibrate_return_error;
break;
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, "[cal] Mag #%u not found, abort", cur_mag);
result = calibrate_return_error;
break;
}
// Get priority
int32_t prio;
orb_priority(worker_data.sub_mag[cur_mag], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_ids[cur_mag];
}
}
// Get priority
int32_t prio;
orb_priority(worker_data.sub_mag[cur_mag], &prio);
#ifndef __PX4_QURT
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_ids[cur_mag];
}
#else
mag_report report = {};
orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &report);
// TODO FIXME: this is hacky but should get the device ID for now
device_ids[cur_mag] = report.device_id;
PX4_INFO("found device id: %d", report.device_id);
#endif
}
}