Merge remote-tracking branch 'upstream/master' into new_state_machine

Conflicts:
	apps/commander/commander.c
	apps/uORB/topics/vehicle_status.h

1 files changed, 119 insertions, 6 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index a3e8fb745..aaabe7f4b 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -76,6 +76,8 @@
 #include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/differential_pressure.h>
+#include <mavlink/mavlink_log.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -799,6 +801,72 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
 	close(sub_sensor_combined);
 }
 
+void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
+{
+	/* announce change */
+
+	mavlink_log_info(mavlink_fd, "keep it still");
+
+	const int calibration_count = 2500;
+
+	int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s differential_pressure;
+
+	int calibration_counter = 0;
+	float airspeed_offset = 0.0f;
+
+	while (calibration_counter < calibration_count) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } };
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+			orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure);
+			airspeed_offset += differential_pressure.voltage;
+			calibration_counter++;
+
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
+			return;
+		}
+	}
+
+	airspeed_offset = airspeed_offset / calibration_count;
+
+	if (isfinite(airspeed_offset)) {
+
+		if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) {
+			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
+		}
+
+		/* auto-save to EEPROM */
+		int save_ret = param_save_default();
+
+		if (save_ret != 0) {
+			warn("WARNING: auto-save of params to storage failed");
+		}
+
+		//char buf[50];
+		//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
+		//mavlink_log_info(mavlink_fd, buf);
+		mavlink_log_info(mavlink_fd, "airspeed calibration done");
+
+		tune_confirm();
+		sleep(2);
+		tune_confirm();
+		sleep(2);
+		/* third beep by cal end routine */
+
+	} else {
+		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+	}
+
+	close(sub_differential_pressure);
+}
+
 
 
 void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
@@ -1018,6 +1086,28 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 					handled = true;
 				}
 
+				/* airspeed calibration */
+				if ((int)(cmd->param6) == 1) {
+
+					if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+
+						result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+						mavlink_log_info(mavlink_fd, "starting airspeed cal");
+						tune_confirm();
+						do_airspeed_calibration(status_pub, &current_status);
+						mavlink_log_info(mavlink_fd, "finished airspeed cal");
+						tune_confirm();
+						// XXX if this fails, go to ERROR
+						arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
+
+					} else {
+						result = VEHICLE_CMD_RESULT_DENIED;
+					}
+
+					handled = true;
+				}
+
 				/* none found */
 				if (!handled) {
 					//warnx("refusing unsupported calibration request\n");
@@ -1438,6 +1528,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct sensor_combined_s sensors;
 	memset(&sensors, 0, sizeof(sensors));
 
+	int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s differential_pressure;
+	memset(&differential_pressure, 0, sizeof(differential_pressure));
+	uint64_t last_differential_pressure_time = 0;
+
 	/* Subscribe to command topic */
 	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
 	struct vehicle_command_s cmd;
@@ -1491,6 +1586,13 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
 		}
 
+		orb_check(differential_pressure_sub, &new_data);
+
+		if (new_data) {
+			orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
+			last_differential_pressure_time = differential_pressure.timestamp;
+		}
+
 		orb_check(cmd_sub, &new_data);
 
 		if (new_data) {
@@ -1691,6 +1793,8 @@ int commander_thread_main(int argc, char *argv[])
 		// bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
 		bool global_pos_valid = current_status.condition_global_position_valid;
 		bool local_pos_valid = current_status.condition_local_position_valid;
+		bool airspeed_valid = current_status.condition_airspeed_valid;
+
 
 		/* check for global or local position updates, set a timeout of 2s */
 		if (hrt_absolute_time() - last_global_position_time < 2000000) {
@@ -1709,6 +1813,14 @@ int commander_thread_main(int argc, char *argv[])
 			current_status.condition_local_position_valid = false;
 		}
 
+		/* Check for valid airspeed/differential pressure measurements */
+		if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
+			current_status.condition_airspeed_valid = true;
+
+		} else {
+			current_status.condition_airspeed_valid = false;
+		}
+
 		/*
 		 * Consolidate global position and local position valid flags
 		 * for vector flight mode.
@@ -1721,12 +1833,13 @@ int commander_thread_main(int argc, char *argv[])
 		// 	current_status.flag_vector_flight_mode_ok = false;
 		// }
 
-		// /* consolidate state change, flag as changed if required */
-		// if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
-		//     global_pos_valid != current_status.flag_global_position_valid ||
-		//     local_pos_valid != current_status.flag_local_position_valid) {
-		// 	state_changed = true;
-		// }
+		// XXX why is this needed?
+		/* consolidate state change, flag as changed if required */
+		if (global_pos_valid != current_status.condition_global_position_valid ||
+		    local_pos_valid != current_status.condition_local_position_valid ||
+		    airspeed_valid != current_status.condition_airspeed_valid) {
+			state_changed = true;
+		}
 
 		/*
 		 * Mark the position of the first position lock as return to launch (RTL)