airspeed (pitot) offset calibration

6 files changed, 130 insertions, 7 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 42ca10f55..41f4e5674 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -80,6 +80,7 @@
 #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 <systemlib/param/param.h>
@@ -785,6 +786,79 @@ 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");
+	/* set to accel calibration mode */
+	status->flag_preflight_airspeed_calibration = true;
+	state_machine_publish(status_pub, status, mavlink_fd);
+
+	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)");
+	}
+
+	/* exit airspeed calibration mode */
+	status->flag_preflight_airspeed_calibration = false;
+	state_machine_publish(status_pub, status, mavlink_fd);
+
+	close(sub_differential_pressure);
+}
+
 
 
 void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
@@ -980,6 +1054,28 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				handled = true;
 			}
 
+			/* airspeed calibration */
+			if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol
+				/* transition to calibration state */
+				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
+
+				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
+					mavlink_log_info(mavlink_fd, "CMD starting airspeed cal");
+					tune_confirm();
+					do_airspeed_calibration(status_pub, &current_status);
+					tune_confirm();
+					mavlink_log_info(mavlink_fd, "CMD finished airspeed cal");
+					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+				} else {
+					mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal");
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
+
+				handled = true;
+			}
+
 			/* none found */
 			if (!handled) {
 				//warnx("refusing unsupported calibration request\n");
diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c
index 9f23ebbba..a1ef9d136 100644
--- a/apps/sensors/sensor_params.c
+++ b/apps/sensors/sensor_params.c
@@ -64,6 +64,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
+PARAM_DEFINE_FLOAT(SENS_VAIR_OFF, 2.5f);
+
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
 PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index c29910dcc..f77bc9b8b 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -187,6 +187,7 @@ private:
 		float mag_scale[3];
 		float accel_offset[3];
 		float accel_scale[3];
+		float airspeed_offset;
 
 		int rc_type;
 
@@ -235,6 +236,7 @@ private:
 		param_t accel_scale[3];
 		param_t mag_offset[3];
 		param_t mag_scale[3];
+		param_t airspeed_offset;
 
 		param_t rc_map_roll;
 		param_t rc_map_pitch;
@@ -480,6 +482,9 @@ Sensors::Sensors() :
 	_parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
 	_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
 
+	/*Airspeed offset */
+	_parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF");
+
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
 	/* fetch initial parameter values */
@@ -687,6 +692,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1]));
 	param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2]));
 
+	/* Airspeed offset */
+	param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset));
+
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
 		warnx("Failed updating voltage scaling param");
@@ -1036,7 +1044,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 //						float pres_raw = fabsf(voltage - (3.3f / 2.0f));
 //						float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f;
 						//XXX depends on sensor used..., where are the above numbers from?
-						float diff_pres_pa = fabsf(voltage - 2.5f) * 1000.0f; //for MPXV7002DP //xxx: need an offset calibration
+						float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP
 
 						float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f,
 							_barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa
@@ -1045,7 +1053,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 
 						float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
 
-						//printf("voltage: %.4f, diff_pres_pa %.4f, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)airspeed_indicated, (double)airspeed_true);
+						//printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true);
 
 						_differential_pressure.timestamp = hrt_absolute_time();
 						_differential_pressure.static_pressure_mbar = _barometer.pressure;
@@ -1053,6 +1061,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 						_differential_pressure.temperature_celcius = _barometer.temperature;
 						_differential_pressure.indicated_airspeed_m_s = airspeed_indicated;
 						_differential_pressure.true_airspeed_m_s = airspeed_true;
+						_differential_pressure.voltage = voltage;
 
 						/* announce the airspeed if needed, just publish else */
 						if (_airspeed_pub > 0) {
diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c
index 32943b2f5..382df2ee4 100644
--- a/apps/systemlib/airspeed.c
+++ b/apps/systemlib/airspeed.c
@@ -58,7 +58,13 @@
  */
 float calc_indicated_airspeed(float differential_pressure)
 {
-	return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+
+	if (differential_pressure > 0) {
+		return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+	} else {
+		return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+	}
+
 }
  
 /**
@@ -89,9 +95,17 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am
 float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
 {
 	float density = get_air_density(static_pressure, temperature_celsius);
-	if (density < 0.0001f || isnan(density)) {
+	if (density < 0.0001f || !isfinite(density)) {
 	 density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
 	 printf("Invalid air density, using density at sea level\n");
 	}
-	return sqrtf((2.0f*(total_pressure - static_pressure)) / density);
+
+	float pressure_difference = total_pressure - static_pressure;
+
+	if(pressure_difference > 0) {
+		return sqrtf((2.0f*(pressure_difference)) / density);
+	} else
+	{
+		return -sqrtf((2.0f*fabs(pressure_difference)) / density);
+	}
 }
diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h
index 78a37fdf4..d5e4bf37e 100644
--- a/apps/uORB/topics/differential_pressure.h
+++ b/apps/uORB/topics/differential_pressure.h
@@ -52,12 +52,13 @@
  * Differential pressure and airspeed
  */
 struct differential_pressure_s {
-	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
+	uint64_t	timestamp;					/**< microseconds since system boot, needed to integrate */
 	float   	static_pressure_mbar;		/**< Static / environment pressure */
 	float		differential_pressure_mbar;	/**< Differential pressure reading */
 	float		temperature_celcius;		/**< ambient temperature in celcius, -1 if unknown */
 	float		indicated_airspeed_m_s;		/**< indicated airspeed in meters per second, -1 if unknown	 */
-	float		true_airspeed_m_s;		/**< true airspeed in meters per second, -1 if unknown */
+	float		true_airspeed_m_s;			/**< true airspeed in meters per second, -1 if unknown */
+	float		voltage;					/**< Voltage from the airspeed sensor (voltage divider already compensated) */
 };
 
 /**
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index 06b4c5ca5..ae18ba050 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -173,6 +173,7 @@ struct vehicle_status_s
 	bool flag_preflight_gyro_calibration;		/**< true if gyro calibration is requested */
 	bool flag_preflight_mag_calibration;			/**< true if mag calibration is requested */
 	bool flag_preflight_accel_calibration;
+	bool flag_preflight_airspeed_calibration;
 
 	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception is terminally lost */