Airspeed: change APM to use new AP_Airspeed library

the next step is AHRS dead reckoning

ArduPlane/ArduPlane.pde

@@ -49,6 +49,7 @@ version 2.1 of the License, or (at your option) any later version.
 #include <LowPassFilter.h>	// LowPassFilter class (inherits from Filter class)
 #include <AP_Relay.h>       // APM relay
 #include <AP_Camera.h>		// Photo or video camera
+#include <AP_Airspeed.h>
 #include <memcheck.h>
 
 // Configuration
@@ -406,8 +407,6 @@ static byte	non_nav_command_ID	= NO_COMMAND;
 ////////////////////////////////////////////////////////////////////////////////
 // Airspeed
 ////////////////////////////////////////////////////////////////////////////////
-// The current airspeed estimate/measurement in centimeters per second
-static int16_t		airspeed;
 // The calculated airspeed to use in FBW-B.  Also used in higher modes for insuring min ground speed is met.
 // Also used for flap deployment criteria.  Centimeters per second.static int32_t		target_airspeed;
 static int32_t		target_airspeed;
@@ -459,10 +458,7 @@ static float	current_total1;
 ////////////////////////////////////////////////////////////////////////////////
 // Airspeed Sensors
 ////////////////////////////////////////////////////////////////////////////////
-// Raw differential pressure measurement (filtered).  ADC units
-static float   airspeed_raw;
-// Raw differential pressure less the zero pressure offset.  ADC units
-static float   airspeed_pressure;
+AP_Airspeed airspeed(&pitot_analog_source, AIRSPEED_RATIO, AIRSPEED_SENSOR);
 
 ////////////////////////////////////////////////////////////////////////////////
 // Altitude Sensor variables
@@ -710,12 +706,8 @@ static void fast_loop()
 
 	// Read Airspeed
 	// -------------
-	if (g.airspeed_enabled == true) {
-#if HIL_MODE != HIL_MODE_ATTITUDE
+	if (airspeed.enabled()) {
 		read_airspeed();
-#else
-		calc_airspeed_errors();
-#endif
 	}
 
 	#if HIL_MODE == HIL_MODE_SENSORS
@@ -994,7 +986,7 @@ static void update_current_flight_mode(void)
 					nav_roll = 0;
 				}
 
-				if(g.airspeed_enabled == true && g.airspeed_use == true){
+				if(airspeed.use()) {
 					calc_nav_pitch();
 					if(nav_pitch < (long)takeoff_pitch)
 						nav_pitch = (long)takeoff_pitch;
@@ -1012,7 +1004,7 @@ static void update_current_flight_mode(void)
 			case MAV_CMD_NAV_LAND:
 				calc_nav_roll();
 
-				if (g.airspeed_enabled == true && g.airspeed_use == true) {
+				if (airspeed.use()) {
 					calc_nav_pitch();
 					calc_throttle();
 				}else{

ArduPlane/Attitude.pde

@@ -11,12 +11,14 @@ static void stabilize()
 	float ch4_inf = 1.0;
 	float speed_scaler;
 
-	if (g.airspeed_enabled == true && g.airspeed_use == true){
-		if(airspeed > 0)
-			speed_scaler = (STANDARD_SPEED * 100) / airspeed;
-		else
+	if (airspeed.use()) {
+        float aspeed = airspeed.get_airspeed();
+		if (aspeed > 0) {
+			speed_scaler = STANDARD_SPEED / aspeed;
+        } else {
 			speed_scaler = 2.0;
-			speed_scaler = constrain(speed_scaler, 0.5, 2.0);
+        }
+        speed_scaler = constrain(speed_scaler, 0.5, 2.0);
 	} else {
 		if (g.channel_throttle.servo_out > 0){
 			speed_scaler = 0.5 + ((float)THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0);	// First order taylor expansion of square root
@@ -130,13 +132,12 @@ static void crash_checker()
 
 static void calc_throttle()
 {
-  if (g.airspeed_enabled == false || g.airspeed_use == false) {
-	int throttle_target = g.throttle_cruise + throttle_nudge;
-
-    // TODO: think up an elegant way to bump throttle when
-    // groundspeed_undershoot > 0 in the no airspeed sensor case; PID
-    // control?
+    if (!airspeed.use()) {
+        int throttle_target = g.throttle_cruise + throttle_nudge;
 
+        // TODO: think up an elegant way to bump throttle when
+        // groundspeed_undershoot > 0 in the no airspeed sensor case; PID
+        // control?
 
 		// no airspeed sensor, we use nav pitch to determine the proper throttle output
 		// AUTO, RTL, etc
@@ -188,7 +189,7 @@ static void calc_nav_pitch()
 {
 	// Calculate the Pitch of the plane
 	// --------------------------------
-	if (g.airspeed_enabled == true && g.airspeed_use == true) {
+	if (airspeed.use()) {
 		nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error);
 	} else {
 		nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error);
@@ -349,7 +350,7 @@ static void set_servos(void)
 			if (
 					(control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) &&
 					(abs(home.alt - current_loc.alt) < 1000) &&
-					(((g.airspeed_enabled && g.airspeed_use) ? airspeed : g_gps->ground_speed) < 500 ) &&
+					((airspeed.use()? airspeed.get_airspeed_cm() : g_gps->ground_speed) < 500 ) &&
 					!(control_mode==AUTO && takeoff_complete == false)
 				) {
 				g.channel_throttle.servo_out = 0;
@@ -386,7 +387,7 @@ static void set_servos(void)
             // FIXME: use target_airspeed in both FBW_B and g.airspeed_enabled cases - Doug?
 			if (control_mode == FLY_BY_WIRE_B) {
 				flapSpeedSource = ((float)target_airspeed)/100;
-			} else if (g.airspeed_enabled == true && g.airspeed_use == true) {
+			} else if (airspeed.use()) {
 				flapSpeedSource = g.airspeed_cruise/100;
 			} else {
 				flapSpeedSource = g.throttle_cruise;

ArduPlane/GCS_Mavlink.pde

@@ -344,8 +344,8 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
 {
     mavlink_msg_vfr_hud_send(
         chan,
-        (float)airspeed / 100.0,
-        (float)g_gps->ground_speed / 100.0,
+        airspeed.get_airspeed(),
+        (float)g_gps->ground_speed * 0.01,
         (ahrs.yaw_sensor / 100) % 360,
         (uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100
         current_loc.alt / 100.0,
@@ -1654,7 +1654,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             mavlink_msg_vfr_hud_decode(msg, &packet);
 
             // set airspeed
-            airspeed = 100*packet.airspeed;
+            airspeed.set_HIL(100*packet.airspeed);
             break;
         }
 

ArduPlane/Log.pde

@@ -310,12 +310,12 @@ static void Log_Write_Nav_Tuning()
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
 	DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor);
-	DataFlash.WriteInt((int)wp_distance);
+	DataFlash.WriteInt((int16_t)wp_distance);
 	DataFlash.WriteInt((uint16_t)target_bearing);
 	DataFlash.WriteInt((uint16_t)nav_bearing);
 	DataFlash.WriteInt(altitude_error);
-	DataFlash.WriteInt((int)airspeed);
-	DataFlash.WriteInt((int)(nav_gain_scaler*1000));
+	DataFlash.WriteInt((int16_t)airspeed.get_airspeed_cm());
+	DataFlash.WriteInt((int16_t)(nav_gain_scaler*1000));
 	DataFlash.WriteByte(END_BYTE);
 }
 

ArduPlane/Parameters.h

@@ -89,11 +89,9 @@ public:
         //
         k_param_imu = 130,  // sensor calibration
         k_param_altitude_mix,
-        k_param_airspeed_ratio,
-
-        // ground_pressure and ground_temperature removed
-        // do not re-use 133 and 134 unless format_version
-        // is changed
+        k_param_airspeed_ratio, // UNUSED
+        k_param_ground_pressure, // UNUSED
+        k_param_ground_temperature, // UNUSED
 
 		k_param_compass_enabled = 135,
 		k_param_compass,
@@ -102,12 +100,13 @@ public:
 		k_param_curr_amp_per_volt,
 		k_param_input_voltage,
 		k_param_pack_capacity,
-        k_param_airspeed_offset,
+        k_param_airspeed_offset, // UNUSED
 		k_param_sonar_enabled,
-		k_param_airspeed_enabled,
+		k_param_airspeed_enabled, // UNUSED
         k_param_ahrs,  // AHRS group
-		k_param_airspeed_use,
+		k_param_airspeed_use, // UNUSED
         k_param_barometer,   // barometer ground calibration
+        k_param_airspeed,  // AP_Airspeed parameters
 
         //
         // 150: Navigation parameters
@@ -269,8 +268,6 @@ public:
     // Estimation
     //
     AP_Float    altitude_mix;
-    AP_Float    airspeed_ratio;
-	AP_Int16	airspeed_offset;
 
     // Waypoints
     //
@@ -349,8 +346,6 @@ public:
 	AP_Int16	pack_capacity;		// Battery pack capacity less reserve
     AP_Int8		inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
     AP_Int8		sonar_enabled;
-    AP_Int8		airspeed_enabled;
-    AP_Int8		airspeed_use;
     AP_Int8		flap_1_percent;
     AP_Int8		flap_1_speed;
     AP_Int8		flap_2_percent;
@@ -400,8 +395,6 @@ public:
         crosstrack_entry_angle  (XTRACK_ENTRY_ANGLE_CENTIDEGREE),
 
         altitude_mix            (ALTITUDE_MIX),
-        airspeed_ratio          (AIRSPEED_RATIO),
-        airspeed_offset			(0),
 
         /* XXX waypoint_mode missing here */
         command_total           (0),
@@ -473,8 +466,6 @@ public:
         pack_capacity	 		(HIGH_DISCHARGE),
         inverted_flight_ch      (0),
         sonar_enabled			(SONAR_ENABLED),
-        airspeed_enabled		(AIRSPEED_SENSOR),
-        airspeed_use			(1),
 
         // PID controller    initial P        initial I        initial D        initial imax
         //-----------------------------------------------------------------------------------

ArduPlane/Parameters.pde

@@ -91,17 +91,6 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	// @User: Advanced
 	GSCALAR(altitude_mix,           "ALT_MIX"),
 
-    // @Param: ARSPD_RATIO
-	// @DisplayName: Airspeed Ratio
-	// @Description: Used to scale raw adc airspeed sensor to a SI Unit (m/s)
-	// @Units: Scale
-	// @Range: 0 5
-	// @Increment: 0.001
-	// @User: Advanced
-	GSCALAR(airspeed_ratio,         "ARSPD_RATIO"),
-
-	GSCALAR(airspeed_offset,        "ARSPD_OFFSET"),
-
 	GSCALAR(command_total,          "CMD_TOTAL"),
 	GSCALAR(command_index,          "CMD_INDEX"),
 
@@ -446,20 +435,6 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	// @User: Standard
 	GSCALAR(sonar_enabled,          "SONAR_ENABLE"),
 
-    // @Param: ARSPD_ENABLE
-	// @DisplayName: Enable Airspeed
-	// @Description: Setting this to Enabled(1) will enable the Airspeed sensor. Setting this to Disabled(0) will disable the Airspeed sensor
-	// @Values: 0:Disabled,1:Enabled
-	// @User: Standard
-	GSCALAR(airspeed_enabled,       "ARSPD_ENABLE"),
-
-    // @Param: ARSPD_USE
-	// @DisplayName: Use Airspeed if enabled
-	// @Description: Setting this to Enabled(1) will enable use of the Airspeed sensor for flight control when ARSPD_ENABLE is also true. This is separate from ARSPD_ENABLE to allow for the airspeed value to be logged without it being used for flight control
-	// @Values: 0:Disabled,1:Enabled
-	// @User: Advanced
-	GSCALAR(airspeed_use,       	"ARSPD_USE"),
-
 	// barometer ground calibration. The GND_ prefix is chosen for
 	// compatibility with previous releases of ArduPlane
 	GOBJECT(barometer, "GND_", AP_Baro),
@@ -517,6 +492,10 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	// @Path: ../libraries/AP_AHRS/AP_AHRS_DCM.cpp, ../libraries/AP_AHRS/AP_AHRS_Quaternion.cpp
 	GOBJECT(ahrs,					"AHRS_",    AP_AHRS),
 
+    // @Group: ARSPD_
+	// @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp
+	GOBJECT(airspeed,				"ARSPD_",   AP_Airspeed),
+
 #if MOUNT == ENABLED
 	// @Group: MNT_
 	// @Path: ../libraries/AP_Mount/AP_Mount.cpp

ArduPlane/navigation.pde

@@ -64,6 +64,8 @@ void calc_distance_error()
 
 static void calc_airspeed_errors()
 {
+    float aspeed_cm = airspeed.get_airspeed_cm();
+
     // Normal airspeed target
     target_airspeed = g.airspeed_cruise;
 
@@ -79,7 +81,7 @@ static void calc_airspeed_errors()
     // but only when this is faster than the target airspeed commanded
     // above.
     if (control_mode >= FLY_BY_WIRE_B && (g.min_gndspeed > 0)) {
-        long min_gnd_target_airspeed = airspeed + groundspeed_undershoot;
+        long min_gnd_target_airspeed = aspeed_cm + groundspeed_undershoot;
         if (min_gnd_target_airspeed > target_airspeed)
             target_airspeed = min_gnd_target_airspeed;
     }
@@ -93,8 +95,8 @@ static void calc_airspeed_errors()
     if (target_airspeed > (g.flybywire_airspeed_max * 100))
         target_airspeed = (g.flybywire_airspeed_max * 100);
 
-    airspeed_error = target_airspeed - airspeed;
-    airspeed_energy_error = ((target_airspeed * target_airspeed) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
+    airspeed_error = target_airspeed - aspeed_cm;
+    airspeed_energy_error = ((target_airspeed * target_airspeed) - (aspeed_cm*aspeed_cm))*0.00005;
 }
 
 static void calc_gndspeed_undershoot()
@@ -142,25 +144,6 @@ static void calc_altitude_error()
 		target_altitude = next_WP.alt;
 	}
 
-	/*
-	// Disabled for now
-	#if AIRSPEED_SENSOR == 1
-		long    altitude_estimate;                  // for smoothing GPS output
-
-		// special thanks to Ryan Beall for this one
-		float pitch_angle 	= pitch_sensor - g.pitch_trim; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°)
-		pitch_angle			= constrain(pitch_angle, -2000, 2000);
-		float scale			= sin(radians(pitch_angle * .01));
-		altitude_estimate 	+= (float)airspeed * .0002 * scale;
-		altitude_estimate 	-= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt);
-
-		// compute altitude error for throttle control
-		altitude_error  = target_altitude - altitude_estimate;
-	#else
-		altitude_error 	= target_altitude - current_loc.alt;
-	#endif
-	*/
-
 	altitude_error 	= target_altitude - current_loc.alt;
 }
 

ArduPlane/radio.pde

@@ -85,7 +85,7 @@ static void read_radio()
 	g.channel_throttle.servo_out = g.channel_throttle.control_in;
 
 	if (g.channel_throttle.servo_out > 50) {
-		if(g.airspeed_enabled == true && g.airspeed_use == true) {
+		if (airspeed.use()) {
 			airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
         } else {
 			throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);

ArduPlane/sensors.pde

@@ -27,34 +27,13 @@ static int32_t read_barometer(void)
 // in M/S * 100
 static void read_airspeed(void)
 {
-	#if GPS_PROTOCOL != GPS_PROTOCOL_IMU	// Xplane will supply the airspeed
-		if (g.airspeed_offset == 0) {
-            // runtime enabling of airspeed, we need to do instant
-            // calibration before we can use it. This isn't as
-            // accurate as the 50 point average in zero_airspeed(),
-            // but it is better than using it uncalibrated
-            airspeed_raw = pitot_analog_source.read();
-            g.airspeed_offset.set_and_save(airspeed_raw);
-		}
-		airspeed_raw 		= (pitot_analog_source.read() * 0.1) + (airspeed_raw * 0.9);
-		airspeed_pressure 	= max((airspeed_raw - g.airspeed_offset), 0);
-		airspeed 			= sqrt(airspeed_pressure * g.airspeed_ratio) * 100;
-	#endif
-
+    airspeed.read();
 	calc_airspeed_errors();
 }
 
 static void zero_airspeed(void)
 {
-    float sum = 0;
-    int c;
-	airspeed_raw = pitot_analog_source.read();
-	for(c = 0; c < 250; c++) {
-		delay(2);
-		sum += pitot_analog_source.read();
-	}
-    sum /= c;
-	g.airspeed_offset.set_and_save((int16_t)sum);
+    airspeed.calibrate(mavlink_delay);
 }
 
 #endif // HIL_MODE != HIL_MODE_ATTITUDE

ArduPlane/system.pde

@@ -470,13 +470,13 @@ static void startup_IMU_ground(bool force_accel_level)
 	//-----------------------------
 	init_barometer();
 
-    if (g.airspeed_enabled == true) {
+    if (airspeed.enabled()) {
         // initialize airspeed sensor
         // --------------------------
         zero_airspeed();
-        gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> zero airspeed calibrated"));
+        gcs_send_text_P(SEVERITY_LOW,PSTR("zero airspeed calibrated"));
     } else {
-        gcs_send_text_P(SEVERITY_LOW,PSTR("<startup_ground> NO airspeed"));
+        gcs_send_text_P(SEVERITY_LOW,PSTR("NO airspeed"));
     }
 
 #endif // HIL_MODE_ATTITUDE

ArduPlane/test.pde

@@ -650,7 +650,7 @@ test_airspeed(uint8_t argc, const Menu::arg *argv)
 	// Serial.println(pitot_analog_source.read());
     Serial.printf_P(PSTR("airspeed_ch: %.1f\n"), airspeed_ch);
 
-	if (g.airspeed_enabled == false){
+	if (!airspeed.enabled()) {
 		Serial.printf_P(PSTR("airspeed: "));
 		print_enabled(false);
 		return (0);
@@ -664,7 +664,7 @@ test_airspeed(uint8_t argc, const Menu::arg *argv)
 		while(1){
 			delay(20);
 			read_airspeed();
-			Serial.printf_P(PSTR("%.1f m/s\n"), airspeed / 100.0);
+			Serial.printf_P(PSTR("%.1f m/s\n"), airspeed.get_airspeed());
 
 			if(Serial.available() > 0){
 				return (0);