Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-10 06:39:25 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

commander fix and enforce code style

Browse Source
This commit is contained in:
Daniel Agar
2018-11-28 17:11:15 -05:00
parent 91721f2060
commit 48d9484ceb
16 changed files with 855 additions and 481 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/astyle/files_to_check_code_style.sh
-1
View File
@@ -13,7 +13,6 @@ exec find src platforms \
-path src/lib/DriverFramework -prune -o \
-path src/lib/ecl -prune -o \
-path src/lib/matrix -prune -o \
-path src/modules/commander -prune -o \
-path src/modules/micrortps_bridge/micro-CDR -prune -o \
-path src/modules/systemlib/uthash -prune -o \
-path src/modules/uavcan/libuavcan -prune -o \
src/modules/commander/Commander.cpp
+203 -114
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/PreflightCheck.cpp
+21 -3
View File
@@ -474,7 +474,8 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage);
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt",
(double)avionics_power_rail_voltage);
}
} else if (avionics_power_rail_voltage < 4.9f) {
@@ -610,44 +611,59 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s
if (report_fail) {
// Only report the first failure to avoid spamming
const char *message = nullptr;
if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
message = "Preflight%s: GPS fix too low";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
message = "Preflight%s: not enough GPS Satellites";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)) {
message = "Preflight%s: GPS GDoP too low";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
message = "Preflight%s: GPS Horizontal Pos Error too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
message = "Preflight%s: GPS Vertical Pos Error too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
message = "Preflight%s: GPS Speed Accuracy too low";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
message = "Preflight%s: GPS Horizontal Pos Drift too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
message = "Preflight%s: GPS Vertical Pos Drift too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
message = "Preflight%s: GPS Hor Speed Drift too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
message = "Preflight%s: GPS Vert Speed Drift too high";
} else {
if (!ekf_gps_fusion) {
// Likely cause unknown
message = "Preflight%s: Estimator not using GPS";
gps_present = false;
} else {
// if we land here there was a new flag added and the code not updated. Show a generic message.
message = "Preflight%s: Poor GPS Quality";
}
}
if (message) {
if (enforce_gps_required) {
mavlink_log_critical(mavlink_log_pub, message, " Fail");
} else {
mavlink_log_warning(mavlink_log_pub, message, "");
}
}
}
gps_success = false;
if (enforce_gps_required) {
@@ -690,7 +706,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
checkAirspeed = true;
}
reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout && !status_flags.condition_calibration_enabled);
reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout
&& !status_flags.condition_calibration_enabled);
bool failed = false;
@@ -902,7 +919,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
} else {
// The calibration is fine, but only set the overall health state to true if the signal is not currently lost
status_flags.rc_calibration_valid = true;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, !status.rc_signal_lost, status);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true,
!status.rc_signal_lost, status);
}
}
src/modules/commander/accelerometer_calibration.cpp
+97 -35
View File
@@ -155,10 +155,14 @@ 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(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub,
float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
calibrate_return read_accelerometer_avg(int sensor_correction_sub, 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]);
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
calibrate_return calculate_calibration_values(unsigned sensor,
float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3],
float (&accel_offs)[max_accel_sens][3], float g);
/// Data passed to calibration worker routine
typedef struct {
@@ -208,37 +212,50 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
}
#else
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
param_notify_changes();
#endif
}
@@ -250,11 +267,14 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
/* measure and calculate offsets & scales */
if (res == PX4_OK) {
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return PX4_ERROR;
} else if (cal_return == calibrate_return_ok) {
res = PX4_OK;
} else {
res = PX4_ERROR;
}
@@ -264,6 +284,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
}
return PX4_ERROR;
}
@@ -282,9 +303,9 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
/* handle individual sensors, one by one */
matrix::Vector3f accel_offs_vec(accel_offs[uorb_index]);
matrix::Vector3f accel_offs_rotated = board_rotation_t * accel_offs_vec;
matrix::Vector3f accel_offs_rotated = board_rotation_t *accel_offs_vec;
matrix::Matrix3f accel_T_mat(accel_T[uorb_index]);
matrix::Matrix3f accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
matrix::Matrix3f accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
@@ -299,17 +320,18 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_offset,
(double)accel_scale.y_offset,
(double)accel_scale.z_offset);
(double)accel_scale.x_offset,
(double)accel_scale.y_offset,
(double)accel_scale.z_offset);
PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_scale,
(double)accel_scale.y_scale,
(double)accel_scale.z_scale);
(double)accel_scale.x_scale,
(double)accel_scale.y_scale,
(double)accel_scale.z_scale);
/* check if thermal compensation is enabled */
int32_t tc_enabled_int;
param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int));
if (tc_enabled_int == 1) {
/* Get struct containing sensor thermal compensation data */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
@@ -325,18 +347,23 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
/* update the _X0_ terms to include the additional offset */
int32_t handle;
float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f;
(void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) {
val += accel_scale.x_offset;
} else if (axis_index == 1) {
val += accel_scale.y_offset;
} else if (axis_index == 2) {
val += accel_scale.z_offset;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
@@ -345,15 +372,20 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
val = 1.0f;
(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
if (axis_index == 0) {
val = accel_scale.x_scale;
} else if (axis_index == 1) {
val = accel_scale.y_scale;
} else if (axis_index == 2) {
val = accel_scale.z_scale;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
param_notify_changes();
}
@@ -394,6 +426,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = PX4_ERROR;
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
@@ -402,6 +435,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index);
}
#endif
}
@@ -421,19 +455,22 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
return res;
}
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void *data)
{
const unsigned samples_num = 750;
accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
accel_worker_data_t *worker_data = (accel_worker_data_t *)(data);
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
detect_orientation_str(orientation));
read_accelerometer_avg(worker_data->sensor_correction_sub, worker_data->subs, worker_data->accel_ref, orientation, samples_num);
read_accelerometer_avg(worker_data->sensor_correction_sub, worker_data->subs, worker_data->accel_ref, orientation,
samples_num);
calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]);
calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]",
detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]);
worker_data->done_count++;
calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
@@ -441,7 +478,8 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
return calibrate_return_ok;
}
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub,
float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
calibrate_return result = calibrate_return_ok;
@@ -458,7 +496,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
// Initialize subs to error condition so we know which ones are open and which are not
for (size_t i=0; i<max_accel_sens; i++) {
for (size_t i = 0; i < max_accel_sens; i++) {
worker_data.subs[i] = -1;
}
@@ -469,14 +507,16 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
// Warn that we will not calibrate more than max_accels accelerometers
if (orb_accel_count > max_accel_sens) {
calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count, max_accel_sens);
calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count,
max_accel_sens);
}
for (unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel < max_accel_sens; cur_accel++) {
// Lock in to correct ORB instance
bool found_cur_accel = false;
for(unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) {
for (unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) {
worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
sensor_accel_s report = {};
@@ -493,6 +533,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
found_cur_accel = true;
// store initial timestamp - used to infer which sensors are active
timestamps[cur_accel] = report.timestamp;
} else {
orb_unsubscribe(worker_data.subs[cur_accel]);
}
@@ -506,7 +547,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
#endif
}
if(!found_cur_accel) {
if (!found_cur_accel) {
calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]);
result = calibrate_return_error;
break;
@@ -521,6 +562,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
device_prio_max = prio;
device_id_primary = device_id[cur_accel];
}
} else {
calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
result = calibrate_return_error;
@@ -530,7 +572,8 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data,
false /* normal still */);
calibrate_cancel_unsubscribe(cancel_sub);
}
@@ -540,12 +583,15 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
/* figure out which sensors were active */
sensor_accel_s arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
(*active_sensors)++;
}
px4_close(worker_data.subs[i]);
}
}
orb_unsubscribe(worker_data.sensor_correction_sub);
if (result == calibrate_return_ok) {
@@ -566,7 +612,8 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
/*
* Read specified number of accelerometer samples, calculate average and dispersion.
*/
calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens],
float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
{
/* get total sensor board rotation matrix */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
@@ -607,6 +654,7 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m
if (orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction) != 0) {
/* use default values */
memset(&sensor_correction, 0, sizeof(sensor_correction));
for (unsigned i = 0; i < 3; i++) {
sensor_correction.accel_scale_0[i] = 1.0f;
sensor_correction.accel_scale_1[i] = 1.0f;
@@ -634,14 +682,17 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]);
} else if (s == 1) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]);
} else if (s == 2) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]);
} else {
accel_sum[s][0] += arp.x;
accel_sum[s][1] += arp.y;
@@ -701,7 +752,9 @@ int mat_invert3(float src[3][3], float dst[3][3])
return PX4_OK;
}
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
calibrate_return calculate_calibration_values(unsigned sensor,
float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3],
float (&accel_offs)[max_accel_sens][3], float g)
{
/* calculate offsets */
for (unsigned i = 0; i < 3; i++) {
@@ -737,7 +790,8 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
return calibrate_return_ok;
}
int do_level_calibration(orb_advert_t *mavlink_log_pub) {
int do_level_calibration(orb_advert_t *mavlink_log_pub)
{
const unsigned cal_time = 5;
const unsigned cal_hz = 100;
unsigned settle_time = 30;
@@ -762,7 +816,7 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
param_get(board_rot_handle, &board_rot_current);
// give attitude some time to settle if there have been changes to the board rotation parameters
if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON ) {
if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON) {
settle_time = 0;
}
@@ -782,14 +836,17 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
// sleep for some time
hrt_abstime start = hrt_absolute_time();
while(hrt_elapsed_time(&start) < settle_time * 1000000) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time));
while (hrt_elapsed_time(&start) < settle_time * 1000000) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG,
(int)(90 * hrt_elapsed_time(&start) / 1e6f / (float)settle_time));
sleep(settle_time / 10);
}
start = hrt_absolute_time();
// average attitude for 5 seconds
while(hrt_elapsed_time(&start) < cal_time * 1000000) {
while (hrt_elapsed_time(&start) < cal_time * 1000000) {
int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
if (pollret <= 0) {
@@ -808,19 +865,22 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
if (counter > (cal_time * cal_hz / 2 )) {
if (counter > (cal_time * cal_hz / 2)) {
roll_mean /= counter;
pitch_mean /= counter;
} else {
calibration_log_info(mavlink_log_pub, "not enough measurements taken");
success = false;
goto out;
}
if (fabsf(roll_mean) > 0.8f ) {
if (fabsf(roll_mean) > 0.8f) {
calibration_log_critical(mavlink_log_pub, "excess roll angle");
} else if (fabsf(pitch_mean) > 0.8f ) {
} else if (fabsf(pitch_mean) > 0.8f) {
calibration_log_critical(mavlink_log_pub, "excess pitch angle");
} else {
roll_mean *= (float)M_RAD_TO_DEG;
pitch_mean *= (float)M_RAD_TO_DEG;
@@ -831,9 +891,11 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
}
out:
if (success) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
return 0;
} else {
// set old parameters
param_set_no_notification(roll_offset_handle, &roll_offset_current);
src/modules/commander/airspeed_calibration.cpp
+13 -4
View File
@@ -107,6 +107,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
/* only warn if analog scaling is zero */
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
goto error_return;
@@ -143,7 +144,8 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
/* any differential pressure failure a reason to abort */
if (diff_pres.error_count != 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu64 ")", diff_pres.error_count);
calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu64 ")",
diff_pres.error_count);
calibration_log_critical(mavlink_log_pub, "[cal] Check your wiring before trying again");
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
@@ -166,6 +168,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
@@ -220,15 +223,19 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
if (diff_pres.differential_pressure_filtered_pa > 0) {
calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa);
calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_filtered_pa);
break;
} else {
/* do not allow negative values */
calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa);
calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_filtered_pa);
calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
@@ -244,9 +251,11 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
}
if (calibration_counter % 500 == 0) {
calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_filtered_pa);
calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_filtered_pa);
tune_neutral(true);
}
calibration_counter++;
} else if (poll_ret == 0) {
src/modules/commander/arm_auth.cpp
+26 -6
View File
@@ -85,6 +85,7 @@ static void arm_auth_request_msg_send()
if (handle_vehicle_command_pub == nullptr) {
handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &vcmd);
}
@@ -96,8 +97,10 @@ static uint8_t _auth_method_arm_req_check()
case ARM_AUTH_IDLE:
/* no authentication in process? handle bellow */
break;
case ARM_AUTH_MISSION_APPROVED:
return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
default:
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
}
@@ -127,12 +130,13 @@ static uint8_t _auth_method_arm_req_check()
state = ARM_AUTH_IDLE;
mavlink_log_critical(mavlink_log_pub, "Arm auth: No response");
break;
default:
break;
}
return state == ARM_AUTH_MISSION_APPROVED ?
vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
}
static uint8_t _auth_method_two_arm_check()
@@ -141,11 +145,14 @@ static uint8_t _auth_method_two_arm_check()
case ARM_AUTH_IDLE:
/* no authentication in process? handle bellow */
break;
case ARM_AUTH_MISSION_APPROVED:
return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
case ARM_AUTH_WAITING_AUTH:
case ARM_AUTH_WAITING_AUTH_WITH_ACK:
return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
default:
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
}
@@ -174,7 +181,7 @@ uint8_t arm_auth_check()
void arm_auth_update(hrt_abstime now, bool param_update)
{
if (param_update) {
param_get(param_arm_parameters, (int32_t*)&arm_parameters);
param_get(param_arm_parameters, (int32_t *)&arm_parameters);
}
switch (state) {
@@ -182,11 +189,14 @@ void arm_auth_update(hrt_abstime now, bool param_update)
case ARM_AUTH_WAITING_AUTH_WITH_ACK:
/* handle bellow */
break;
case ARM_AUTH_MISSION_APPROVED:
if (now > auth_timeout) {
state = ARM_AUTH_IDLE;
}
return;
case ARM_AUTH_IDLE:
default:
return;
@@ -199,50 +209,60 @@ void arm_auth_update(hrt_abstime now, bool param_update)
bool updated = false;
orb_check(command_ack_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_command_ack), command_ack_sub, &command_ack);
}
if (updated
&& command_ack.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST
&& command_ack.target_system == *system_id) {
&& command_ack.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST
&& command_ack.target_system == *system_id) {
switch (command_ack.result) {
case vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS:
state = ARM_AUTH_WAITING_AUTH_WITH_ACK;
break;
case vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED:
mavlink_log_critical(mavlink_log_pub,
"Arm auth: Authorized for the next %u seconds",
command_ack.result_param2);
"Arm auth: Authorized for the next %u seconds",
command_ack.result_param2);
state = ARM_AUTH_MISSION_APPROVED;
auth_timeout = now + (command_ack.result_param2 * 1000000);
return;
case vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Temporarily rejected");
state = ARM_AUTH_IDLE;
return;
case vehicle_command_ack_s::VEHICLE_RESULT_DENIED:
default:
switch (command_ack.result_param1) {
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_NONE:
/* Authorizer will send reason to ground station */
break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied, waypoint %i have a invalid value", command_ack.result_param2);
break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_TIMEOUT:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied by timeout in authorizer");
break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because airspace is in use");
break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_BAD_WEATHER:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because of bad weather");
break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_GENERIC:
default:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied");
}
state = ARM_AUTH_IDLE;
return;
}
src/modules/commander/calibration_routines.cpp
+9 -6
View File
@@ -800,14 +800,17 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
int calibrate_cancel_subscribe()
{
int vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
if (vehicle_command_sub >= 0) {
// make sure we won't read any old messages
struct vehicle_command_s cmd;
bool update;
while (orb_check(vehicle_command_sub, &update) == 0 && update) {
orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &cmd);
}
}
return vehicle_command_sub;
}
@@ -847,12 +850,12 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
// ignore internal commands, such as VEHICLE_CMD_DO_MOUNT_CONTROL from vmount
if (cmd.from_external) {
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
(int)cmd.param1 == 0 &&
(int)cmd.param2 == 0 &&
(int)cmd.param3 == 0 &&
(int)cmd.param4 == 0 &&
(int)cmd.param5 == 0 &&
(int)cmd.param6 == 0) {
(int)cmd.param1 == 0 &&
(int)cmd.param2 == 0 &&
(int)cmd.param3 == 0 &&
(int)cmd.param4 == 0 &&
(int)cmd.param5 == 0 &&
(int)cmd.param6 == 0) {
calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG);
return true;
src/modules/commander/calibration_routines.h
+4 -4
View File
@@ -66,11 +66,11 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
float *offdiag_z);
int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
unsigned int size, float *offset_x, float *offset_y, float *offset_z,
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
float *offdiag_z);
unsigned int size, float *offset_x, float *offset_y, float *offset_z,
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
float *offdiag_z);
bool inverse4x4(float m[], float invOut[]);
bool mat_inverse(float* A, float* inv, uint8_t n);
bool mat_inverse(float *A, float *inv, uint8_t n);
// FIXME: Change the name
static const unsigned max_accel_sens = 3;
src/modules/commander/commander_helper.cpp
+3 -2
View File
@@ -94,10 +94,11 @@ bool is_multirotor(const struct vehicle_status_s *current_status)
bool is_rotary_wing(const struct vehicle_status_s *current_status)
{
return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
}
bool is_vtol(const struct vehicle_status_s * current_status) {
bool is_vtol(const struct vehicle_status_s *current_status)
{
return (current_status->system_type == VEHICLE_TYPE_VTOL_DUOROTOR ||
current_status->system_type == VEHICLE_TYPE_VTOL_QUADROTOR ||
current_status->system_type == VEHICLE_TYPE_VTOL_TILTROTOR ||
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+325 -225
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/esc_calibration.cpp
+16 -5
View File
@@ -63,21 +63,22 @@
using namespace time_literals;
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed)
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed)
{
int return_code = PX4_OK;
#if defined(__PX4_POSIX_OCPOC) || defined(__PX4_POSIX_BBBLUE)
hrt_abstime timeout_start = 0;
hrt_abstime timeout_wait = 60_s;
armed->in_esc_calibration_mode = true;
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "begin esc");
timeout_start = hrt_absolute_time();
while (true) {
if (hrt_absolute_time() - timeout_start > timeout_wait) {
break;
}else{
} else {
usleep(50000);
}
}
@@ -88,7 +89,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
if (return_code == OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
}
return return_code;
#else
@@ -106,6 +107,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
batt_sub = orb_subscribe(ORB_ID(battery_status));
if (batt_sub < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery");
goto Error;
@@ -173,8 +175,10 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
if (!batt_connected) {
orb_check(batt_sub, &batt_updated);
if (batt_updated) {
orb_copy(ORB_ID(battery_status), batt_sub, &battery);
if (battery.connected) {
// Battery is connected, signal to user and start waiting again
batt_connected = true;
@@ -183,25 +187,32 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
}
}
}
usleep(50_ms);
}
Out:
if (batt_sub != -1) {
orb_unsubscribe(batt_sub);
}
if (fd != -1) {
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still off");
}
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed");
}
if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still deactivated");
}
px4_close(fd);
}
armed->in_esc_calibration_mode = false;
if (return_code == PX4_OK) {
src/modules/commander/esc_calibration.h
+1 -1
View File
@@ -44,6 +44,6 @@
#include <uORB/topics/actuator_armed.h>
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed);
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed);
#endif
src/modules/commander/failure_detector/FailureDetector.cpp
+1
View File
@@ -83,6 +83,7 @@ FailureDetector::update_attitude_status()
if (roll_status) {
_status |= FAILURE_ROLL;
}
if (pitch_status) {
_status |= FAILURE_PITCH;
}
src/modules/commander/gyro_calibration.cpp
+87 -47
View File
@@ -1,35 +1,35 @@
/****************************************************************************
*
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
*
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file gyro_calibration.cpp
@@ -74,18 +74,20 @@ typedef struct {
sensor_gyro_s gyro_report_0;
} gyro_worker_data_t;
static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
{
gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data);
gyro_worker_data_t *worker_data = (gyro_worker_data_t *)(data);
unsigned calibration_counter[max_gyros] = { 0 }, slow_count = 0;
const unsigned calibration_count = 5000;
sensor_gyro_s gyro_report;
unsigned poll_errcount = 0;
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) {
/* use default values */
memset(&sensor_correction, 0, sizeof(sensor_correction));
for (unsigned i = 0; i < 3; i++) {
sensor_correction.gyro_scale_0[i] = 1.0f;
sensor_correction.gyro_scale_1[i] = 1.0f;
@@ -94,6 +96,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
}
px4_pollfd_struct_t fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) {
fds[s].fd = worker_data->gyro_sensor_sub[s];
fds[s].events = POLLIN;
@@ -120,6 +123,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
if (poll_ret > 0) {
unsigned update_count = calibration_count;
for (unsigned s = 0; s < max_gyros; s++) {
if (calibration_counter[s] >= calibration_count) {
// Skip if instance has enough samples
@@ -134,9 +138,12 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
if (s == 0) {
// take a working copy
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2];
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_0[0]) *
sensor_correction.gyro_scale_0[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_0[1]) *
sensor_correction.gyro_scale_0[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_0[2]) *
sensor_correction.gyro_scale_0[2];
// take a reference copy of the primary sensor including correction for thermal drift
orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0);
@@ -145,14 +152,20 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
worker_data->gyro_report_0.z = (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2];
} else if (s == 1) {
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_1[0]) * sensor_correction.gyro_scale_1[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_1[1]) * sensor_correction.gyro_scale_1[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_1[2]) * sensor_correction.gyro_scale_1[2];
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_1[0]) *
sensor_correction.gyro_scale_1[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_1[1]) *
sensor_correction.gyro_scale_1[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_1[2]) *
sensor_correction.gyro_scale_1[2];
} else if (s == 2) {
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_2[0]) * sensor_correction.gyro_scale_2[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_2[1]) * sensor_correction.gyro_scale_2[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_2[2]) * sensor_correction.gyro_scale_2[2];
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_2[0]) *
sensor_correction.gyro_scale_2[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_2[1]) *
sensor_correction.gyro_scale_2[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_2[2]) *
sensor_correction.gyro_scale_2[2];
} else {
worker_data->gyro_scale[s].x_offset += gyro_report.x;
@@ -165,7 +178,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
}
// Maintain the sample count of the slowest sensor
// Maintain the sample count of the slowest sensor
if (calibration_counter[s] && calibration_counter[s] < update_count) {
update_count = calibration_counter[s];
}
@@ -236,6 +249,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
worker_data.gyro_sensor_sub[s] = -1;
(void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "Unable to reset CAL_GYRO%u_ID", s);
return PX4_ERROR;
@@ -246,6 +260,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
#ifdef __PX4_NUTTX
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0);
if (fd >= 0) {
worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
@@ -256,37 +271,50 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
return PX4_ERROR;
}
}
#else
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_XSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
param_notify_changes();
#endif
@@ -304,7 +332,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
// Lock in to correct ORB instance
bool found_cur_gyro = false;
for(unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) {
for (unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) {
worker_data.gyro_sensor_sub[cur_gyro] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
sensor_gyro_s report{};
@@ -319,6 +348,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (report.device_id == (uint32_t)worker_data.device_id[cur_gyro]) {
// Device IDs match, correct ORB instance for this gyro
found_cur_gyro = true;
} else {
orb_unsubscribe(worker_data.gyro_sensor_sub[cur_gyro]);
}
@@ -332,8 +362,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
#endif
}
if(!found_cur_gyro) {
calibration_log_critical(mavlink_log_pub, "Gyro #%u (ID %u) no matching uORB devid", cur_gyro, worker_data.device_id[cur_gyro]);
if (!found_cur_gyro) {
calibration_log_critical(mavlink_log_pub, "Gyro #%u (ID %u) no matching uORB devid", cur_gyro,
worker_data.device_id[cur_gyro]);
res = calibrate_return_error;
break;
}
@@ -347,6 +378,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
device_prio_max = prio;
device_id_primary = worker_data.device_id[cur_gyro];
}
} else {
calibration_log_critical(mavlink_log_pub, "Gyro #%u no device id, abort", cur_gyro);
}
@@ -393,6 +425,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
res = PX4_OK;
}
}
try_count++;
} while (res == PX4_ERROR && try_count <= max_tries);
@@ -424,6 +457,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
/* check if thermal compensation is enabled */
int32_t tc_enabled_int;
param_get(param_find("TC_G_ENABLE"), &(tc_enabled_int));
if (tc_enabled_int == 1) {
/* Get struct containing sensor thermal compensation data */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
@@ -437,11 +471,13 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
/* update the _X0_ terms to include the additional offset */
int32_t handle;
float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f;
(void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index);
handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) {
val += worker_data.gyro_scale[uorb_index].x_offset;
@@ -452,8 +488,10 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
val += worker_data.gyro_scale[uorb_index].z_offset;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
param_notify_changes();
}
@@ -489,6 +527,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, 1);
}
#endif
}
}
@@ -504,6 +543,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (res == PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
src/modules/commander/mag_calibration.cpp
+27 -16
View File
@@ -118,16 +118,21 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
// sensor calibration and will be invalidated by a new sensor calibration
(void)sprintf(str, "EKF2_MAGBIAS_X");
result = param_set_no_notification(param_find(str), &mscale_null.x_offset);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "EKF2_MAGBIAS_Y");
result = param_set_no_notification(param_find(str), &mscale_null.y_offset);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "EKF2_MAGBIAS_Z");
result = param_set_no_notification(param_find(str), &mscale_null.z_offset);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
@@ -289,45 +294,49 @@ static unsigned progress_percentage(mag_worker_data_t *worker_data)
// Returns calibrate_return_error if any parameter is not finite
// Logs if parameters are out of range
static calibrate_return check_calibration_result(float offset_x, float offset_y, float offset_z,
float sphere_radius,
float diag_x, float diag_y, float diag_z,
float offdiag_x, float offdiag_y, float offdiag_z,
orb_advert_t *mavlink_log_pub, size_t cur_mag)
float sphere_radius,
float diag_x, float diag_y, float diag_z,
float offdiag_x, float offdiag_y, float offdiag_z,
orb_advert_t *mavlink_log_pub, size_t cur_mag)
{
float must_be_finite[] = {offset_x, offset_y, offset_z,
sphere_radius,
diag_x, diag_y, diag_z,
offdiag_x, offdiag_y, offdiag_z};
sphere_radius,
diag_x, diag_y, diag_z,
offdiag_x, offdiag_y, offdiag_z
};
float should_be_not_huge[] = {offset_x, offset_y, offset_z};
float should_be_positive[] = {sphere_radius, diag_x, diag_y, diag_z};
// Make sure every parameter is finite
const int num_finite = sizeof(must_be_finite) / sizeof(*must_be_finite);
for (unsigned i = 0; i < num_finite; ++i) {
if (!PX4_ISFINITE(must_be_finite[i])) {
calibration_log_emergency(mavlink_log_pub,
"ERROR: Retry calibration (sphere NaN, #%zu)", cur_mag);
"ERROR: Retry calibration (sphere NaN, #%zu)", cur_mag);
return calibrate_return_error;
}
}
// Notify if offsets are too large
const int num_not_huge = sizeof(should_be_not_huge) / sizeof(*should_be_not_huge);
for (unsigned i = 0; i < num_not_huge; ++i) {
if (fabsf(should_be_not_huge[i]) > MAG_MAX_OFFSET_LEN) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets",
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
break;
}
}
// Notify if a parameter which should be positive is non-positive
const int num_positive = sizeof(should_be_positive) / sizeof(*should_be_positive);
for (unsigned i = 0; i < num_positive; ++i) {
if (should_be_positive[i] <= 0.0f) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with non-positive scale",
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
break;
}
}
@@ -597,7 +606,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
// Lock in to correct ORB instance
bool found_cur_mag = false;
for(unsigned i = 0; i < orb_mag_count && !found_cur_mag; i++) {
for (unsigned i = 0; i < orb_mag_count && !found_cur_mag; i++) {
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), i);
struct mag_report report;
@@ -612,6 +622,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
if (report.device_id == (uint32_t)device_ids[cur_mag]) {
// Device IDs match, correct ORB instance for this mag
found_cur_mag = true;
} else {
orb_unsubscribe(worker_data.sub_mag[cur_mag]);
}
@@ -625,7 +636,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
#endif
}
if(!found_cur_mag) {
if (!found_cur_mag) {
calibration_log_critical(mavlink_log_pub, "Mag #%u (ID %u) no matching uORB devid", cur_mag, device_ids[cur_mag]);
result = calibrate_return_error;
break;
@@ -724,10 +735,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
&offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]);
result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag],
sphere_radius[cur_mag],
diag_x[cur_mag], diag_y[cur_mag], diag_z[cur_mag],
offdiag_x[cur_mag], offdiag_y[cur_mag], offdiag_z[cur_mag],
mavlink_log_pub, cur_mag);
sphere_radius[cur_mag],
diag_x[cur_mag], diag_y[cur_mag], diag_z[cur_mag],
offdiag_x[cur_mag], offdiag_y[cur_mag], offdiag_z[cur_mag],
mavlink_log_pub, cur_mag);
if (result == calibrate_return_error) {
break;
src/modules/commander/state_machine_helper.cpp
+22 -12
View File
@@ -126,7 +126,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
&& !hil_enabled) {
preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true, time_since_boot);
preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true,
time_since_boot);
if (preflight_check_ret) {
status_flags->condition_system_sensors_initialized = true;
@@ -137,13 +138,15 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
/* re-run the pre-flight check as long as sensors are failing */
if (!status_flags->condition_system_sensors_initialized
&& fRunPreArmChecks
&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
&& !hil_enabled) {
&& fRunPreArmChecks
&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
&& !hil_enabled) {
if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) {
status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, false, false, time_since_boot);
status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags,
checkGNSS, false, false, time_since_boot);
last_preflight_check = hrt_absolute_time();
}
@@ -205,7 +208,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
// Finish up the state transition
if (valid_transition) {
armed->armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED);
armed->ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
armed->ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
@@ -972,11 +976,13 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break;
case LOW_BAT_ACTION::RETURN:
// FALLTHROUGH
// FALLTHROUGH
case LOW_BAT_ACTION::RETURN_OR_LAND:
// let us send the critical message even if already in RTL
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state)) {
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
internal_state)) {
mavlink_log_critical(mavlink_log_pub, "%s, RETURNING", battery_critical);
} else {
@@ -986,7 +992,8 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break;
case LOW_BAT_ACTION::LAND:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, internal_state)) {
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
internal_state)) {
mavlink_log_critical(mavlink_log_pub, "%s, LANDING AT CURRENT POSITION", battery_critical);
} else {
@@ -1008,7 +1015,8 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break;
case LOW_BAT_ACTION::RETURN:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state)) {
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
internal_state)) {
mavlink_log_emergency(mavlink_log_pub, "%s, RETURNING", battery_dangerous);
} else {
@@ -1018,9 +1026,11 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break;
case LOW_BAT_ACTION::RETURN_OR_LAND:
// FALLTHROUGH
// FALLTHROUGH
case LOW_BAT_ACTION::LAND:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, internal_state)) {
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
internal_state)) {
mavlink_log_emergency(mavlink_log_pub, "%s, LANDING IMMEDIATELY", battery_dangerous);
} else {