commander fix and enforce code style

Tools/astyle/files_to_check_code_style.sh

@@ -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/PreflightCheck.cpp

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

@@ -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 do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub,
-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);
+		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.x_offset,
-				(double)accel_scale.y_offset,
+			 (double)accel_scale.y_offset,
-				(double)accel_scale.z_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.x_scale,
-				(double)accel_scale.y_scale,
+			 (double)accel_scale.y_scale,
-				(double)accel_scale.z_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),
+	calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]",
-				     (double)worker_data->accel_ref[0][orientation][0],
+			     detect_orientation_str(orientation),
-				     (double)worker_data->accel_ref[0][orientation][1],
+			     (double)worker_data->accel_ref[0][orientation][0],
-				     (double)worker_data->accel_ref[0][orientation][2]);
+			     (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

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

@@ -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.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST
-			&& command_ack.target_system == *system_id) {
+	    && 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",
+					     "Arm auth: Authorized for the next %u seconds",
-					command_ack.result_param2);
+					     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

@@ -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.param1 == 0 &&
-					(int)cmd.param2 == 0 &&
+			    (int)cmd.param2 == 0 &&
-					(int)cmd.param3 == 0 &&
+			    (int)cmd.param3 == 0 &&
-					(int)cmd.param4 == 0 &&
+			    (int)cmd.param4 == 0 &&
-					(int)cmd.param5 == 0 &&
+			    (int)cmd.param5 == 0 &&
-					(int)cmd.param6 == 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

@@ -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,
+			 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 *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
-		      float *offdiag_z);
+			 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

@@ -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/esc_calibration.cpp

@@ -63,7 +63,7 @@
 
 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;
 
@@ -77,7 +77,8 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
 	while (true) {
 		if (hrt_absolute_time() - timeout_start > timeout_wait) {
 			break;
-		}else{
+
+		} else {
 			usleep(50000);
 		}
 	}
@@ -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

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

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

@@ -1,35 +1,35 @@
- /****************************************************************************
+/****************************************************************************
- *
+*
- *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
- *
+*
- * Redistribution and use in source and binary forms, with or without
+* Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
+* modification, are permitted provided that the following conditions
- * are met:
+* are met:
- *
+*
- * 1. Redistributions of source code must retain the above copyright
+* 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
+*    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
+* 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
+*    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
+*    the documentation and/or other materials provided with the
- *    distribution.
+*    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
+* 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
+*    used to endorse or promote products derived from this software
- *    without specific prior written permission.
+*    without specific prior written permission.
- *
+*
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
+* 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].x_offset += (gyro_report.x - sensor_correction.gyro_offset_0[0]) *
-						worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1];
+										       sensor_correction.gyro_scale_0[0];
-						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].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].x_offset += (gyro_report.x - sensor_correction.gyro_offset_1[0]) *
-						worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_1[1]) * sensor_correction.gyro_scale_1[1];
+										       sensor_correction.gyro_scale_1[0];
-						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].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].x_offset += (gyro_report.x - sensor_correction.gyro_offset_2[0]) *
-						worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_2[1]) * sensor_correction.gyro_scale_2[1];
+										       sensor_correction.gyro_scale_2[0];
-						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].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;
@@ -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) {
+		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]);
+			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

@@ -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 sphere_radius,
-				float diag_x, float diag_y, float diag_z,
+		float diag_x, float diag_y, float diag_z,
-				float offdiag_x, float offdiag_y, float offdiag_z,
+		float offdiag_x, float offdiag_y, float offdiag_z,
-				orb_advert_t *mavlink_log_pub, size_t cur_mag)
+		orb_advert_t *mavlink_log_pub, size_t cur_mag)
 {
 	float must_be_finite[] = {offset_x, offset_y, offset_z,
-							  sphere_radius,
+				  sphere_radius,
-							  diag_x, diag_y, diag_z,
+				  diag_x, diag_y, diag_z,
-							  offdiag_x, offdiag_y, offdiag_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],
+								  sphere_radius[cur_mag],
-							    diag_x[cur_mag], diag_y[cur_mag], diag_z[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],
+								  offdiag_x[cur_mag], offdiag_y[cur_mag], offdiag_z[cur_mag],
-							    mavlink_log_pub, cur_mag);
+								  mavlink_log_pub, cur_mag);
 
 				if (result == calibrate_return_error) {
 					break;

src/modules/commander/state_machine_helper.cpp

@@ -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
+		    && fRunPreArmChecks
-			&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
+		    && ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
-			&& !hil_enabled) {
+			|| (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 {