Merged master

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");
@@ -1381,6 +1477,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;
@@ -1434,6 +1535,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) {
@@ -1626,6 +1734,7 @@ 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.flag_global_position_valid;
 		bool local_pos_valid = current_status.flag_local_position_valid;
+		bool airspeed_valid = current_status.flag_airspeed_valid;
 
 		/* check for global or local position updates, set a timeout of 2s */
 		if (hrt_absolute_time() - last_global_position_time < 2000000) {
@@ -1644,6 +1753,14 @@ int commander_thread_main(int argc, char *argv[])
 			current_status.flag_local_position_valid = false;
 		}
 
+		/* Check for valid airspeed/differential pressure measurements */
+		if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
+			current_status.flag_airspeed_valid = true;
+
+		} else {
+			current_status.flag_airspeed_valid = false;
+		}
+
 		/*
 		 * Consolidate global position and local position valid flags
 		 * for vector flight mode.
@@ -1659,7 +1776,8 @@ int commander_thread_main(int argc, char *argv[])
 		/* 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) {
+		    local_pos_valid != current_status.flag_local_position_valid ||
+		    airspeed_valid != current_status.flag_airspeed_valid) {
 			state_changed = true;
 		}
 

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);

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");
@@ -1025,8 +1033,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
 
 					/* calculate airspeed, raw is the difference from */
-
+					float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
-					float voltage = buf_adc[i].am_data / 4096.0f;
 
 					/**
 					 * The voltage divider pulls the signal down, only act on
@@ -1034,24 +1041,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					 */
 					if (voltage > 0.4f) {
 
-						float pres_raw = fabsf(voltage - (3.3f / 2.0f));
+						float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor
-						float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f;
 
-						float airspeed_true = calc_true_airspeed(pres_mbar + _barometer.pressure,
+						float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f,
-							_barometer.pressure, _barometer.temperature - 5.0f);
+							_barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa
 						// XXX HACK - true temperature is much less than indicated temperature in baro,
 						// subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
 
-						float airspeed_indicated = calc_indicated_airspeed(pres_mbar + _barometer.pressure,
+						float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
-							_barometer.pressure, _barometer.temperature - 5.0f);
+
-						// XXX HACK
+						//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;
-						_differential_pressure.differential_pressure_mbar = pres_mbar;
+						_differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f;
 						_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) {

apps/systemlib/airspeed.c

@@ -40,14 +40,31 @@
  *
  */
 
-#include "math.h"
+#include <stdio.h>
+#include <math.h>
 #include "conversions.h"
 #include "airspeed.h"
 
 
-float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature)
+/**
+ * Calculate indicated airspeed.
+ *
+ * Note that the indicated airspeed is not the true airspeed because it
+ * lacks the air density compensation. Use the calc_true_airspeed functions to get
+ * the true airspeed.
+ *
+ * @param differential_pressure total_ pressure - static pressure
+ * @return indicated airspeed in m/s
+ */
+float calc_indicated_airspeed(float differential_pressure)
 {
-	return sqrtf((2.0f*(pressure_front - pressure_ambient)) / 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);
+	}
+
 }
  
 /**
@@ -55,14 +72,14 @@ float calc_indicated_airspeed(float pressure_front, float pressure_ambient, floa
  *
  * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
  *
- * @param speed current indicated airspeed
+ * @param speed_indicated current indicated airspeed
  * @param pressure_ambient pressure at the side of the tube/airplane
- * @param temperature air temperature in degrees celcius
+ * @param temperature_celsius air temperature in degrees celcius
  * @return true airspeed in m/s
  */
-float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature)
+float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius)
 {
-	return speed * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature));
+	return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius));
 }
  
 /**
@@ -70,12 +87,25 @@ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, flo
  *
  * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
  *
- * @param pressure_front pressure inside the pitot/prandl tube
+ * @param total_pressure pressure inside the pitot/prandtl tube
- * @param pressure_ambient pressure at the side of the tube/airplane
+ * @param static_pressure pressure at the side of the tube/airplane
- * @param temperature air temperature in degrees celcius
+ * @param temperature_celsius air temperature in degrees celcius
  * @return true airspeed in m/s
  */
-float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature)
+float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
 {
-	return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature));
+	float density = get_air_density(static_pressure, temperature_celsius);
+	if (density < 0.0001f || !isfinite(density)) {
+	 density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
+	 printf("[airspeed] Invalid air density, using density at sea level\n");
+	}
+
+	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);
+	}
 }

apps/systemlib/airspeed.h

@@ -48,43 +48,42 @@
 
  __BEGIN_DECLS
  
-/**
+ /**
   * Calculate indicated airspeed.
   *
   * Note that the indicated airspeed is not the true airspeed because it
   * lacks the air density compensation. Use the calc_true_airspeed functions to get
   * the true airspeed.
   *
- * @param pressure_front pressure inside the pitot/prandl tube
+  * @param total_pressure pressure inside the pitot/prandtl tube
- * @param pressure_ambient pressure at the side of the tube/airplane
+  * @param static_pressure pressure at the side of the tube/airplane
- * @param temperature air temperature in degrees celcius
   * @return indicated airspeed in m/s
   */
-__EXPORT float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature);
+ __EXPORT float calc_indicated_airspeed(float differential_pressure);
  
-/**
+ /**
   * Calculate true airspeed from indicated airspeed.
   *
   * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
   *
- * @param speed current indicated airspeed
+  * @param speed_indicated current indicated airspeed
   * @param pressure_ambient pressure at the side of the tube/airplane
- * @param temperature air temperature in degrees celcius
+  * @param temperature_celsius air temperature in degrees celcius
   * @return true airspeed in m/s
   */
-__EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature);
+ __EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius);
  
-/**
+ /**
   * Directly calculate true airspeed
   *
   * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
   *
- * @param pressure_front pressure inside the pitot/prandl tube
+  * @param total_pressure pressure inside the pitot/prandtl tube
- * @param pressure_ambient pressure at the side of the tube/airplane
+  * @param static_pressure pressure at the side of the tube/airplane
- * @param temperature air temperature in degrees celcius
+  * @param temperature_celsius air temperature in degrees celcius
   * @return true airspeed in m/s
   */
-__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature);
+ __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
 
 __END_DECLS
 

apps/systemlib/conversions.c

@@ -150,5 +150,5 @@ void quat2rot(const float Q[4], float R[9])
 
 float get_air_density(float static_pressure, float temperature_celsius)
 {
-	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius + CONSTANTS_ABSOLUTE_NULL_KELVIN));
+	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
 }

apps/systemlib/conversions.h

@@ -44,10 +44,10 @@
 #include <float.h>
 #include <stdint.h>
 
-#define CONSTANTS_ONE_G				9.80665f
+#define CONSTANTS_ONE_G						9.80665f		// m/s^2
-#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C	1.225f
+#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C	1.225f			// kg/m^3
-#define CONSTANTS_AIR_GAS_CONST 8.31432f
+#define CONSTANTS_AIR_GAS_CONST				287.1f 			// J/(kg * K)
-#define CONSTANTS_ABSOLUTE_NULL_KELVIN 273.15f
+#define CONSTANTS_ABSOLUTE_NULL_CELSIUS		-273.15f		// °C
 
 __BEGIN_DECLS
 

apps/uORB/topics/differential_pressure.h

@@ -49,7 +49,7 @@
  */
 
 /**
- * Battery voltages and status
+ * Differential pressure and airspeed
  */
 struct differential_pressure_s {
 	uint64_t	timestamp;					/**< microseconds since system boot, needed to integrate */
@@ -58,6 +58,7 @@ struct differential_pressure_s {
 	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		voltage;					/**< Voltage from the airspeed sensor (voltage divider already compensated) */
 };
 
 /**

apps/uORB/topics/vehicle_status.h

@@ -175,6 +175,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 */
@@ -212,6 +213,7 @@ struct vehicle_status_s
 	bool flag_external_manual_override_ok;		/**< external override non-fatal for system. Only true for fixed wing */
 	bool flag_valid_launch_position;		/**< indicates a valid launch position */
 	bool flag_valid_home_position;			/**< indicates a valid home position (a valid home position is not always a valid launch) */
+	bool flag_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
 };
 
 /**