From de3c9ce56d4fe0bbce24fcf96db53e4e6cb3a810 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 Jul 2012 11:21:50 +1000 Subject: [PATCH] Airspeed: change APM to use new AP_Airspeed library the next step is AHRS dead reckoning --- ArduPlane/ArduPlane.pde | 18 +++++------------- ArduPlane/Attitude.pde | 29 +++++++++++++++-------------- ArduPlane/GCS_Mavlink.pde | 6 +++--- ArduPlane/Log.pde | 6 +++--- ArduPlane/Parameters.h | 23 +++++++---------------- ArduPlane/Parameters.pde | 29 ++++------------------------- ArduPlane/navigation.pde | 27 +++++---------------------- ArduPlane/radio.pde | 2 +- ArduPlane/sensors.pde | 25 ++----------------------- ArduPlane/system.pde | 6 +++--- ArduPlane/test.pde | 4 ++-- 11 files changed, 50 insertions(+), 125 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 228af3c796..5e50d236e4 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -49,6 +49,7 @@ version 2.1 of the License, or (at your option) any later version. #include // LowPassFilter class (inherits from Filter class) #include // APM relay #include // Photo or video camera +#include #include // 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{ diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 4633366aea..67da793bc5 100644 --- a/ArduPlane/Attitude.pde +++ b/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; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 1cabf7191a..77e4f42069 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/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; } diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index d152dd03f8..99d6ec8813 100644 --- a/ArduPlane/Log.pde +++ b/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); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 2f17c3eec4..67f13be597 100644 --- a/ArduPlane/Parameters.h +++ b/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 //----------------------------------------------------------------------------------- diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 79734bcde2..b9d891e820 100644 --- a/ArduPlane/Parameters.pde +++ b/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 diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 91edcac7c0..ca56092f47 100644 --- a/ArduPlane/navigation.pde +++ b/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; } diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 7876f048d4..cfd96b5d5d 100644 --- a/ArduPlane/radio.pde +++ b/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); diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index deaa3e601e..1b128d2d2c 100644 --- a/ArduPlane/sensors.pde +++ b/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 diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 5518957398..9224e7e46f 100644 --- a/ArduPlane/system.pde +++ b/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(" zero airspeed calibrated")); + gcs_send_text_P(SEVERITY_LOW,PSTR("zero airspeed calibrated")); } else { - gcs_send_text_P(SEVERITY_LOW,PSTR(" NO airspeed")); + gcs_send_text_P(SEVERITY_LOW,PSTR("NO airspeed")); } #endif // HIL_MODE_ATTITUDE diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index d02f0deef6..d750b866f7 100644 --- a/ArduPlane/test.pde +++ b/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);