From 71d786187e42333d991b60b86f20ea39cac9fc57 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Oct 2014 07:17:33 +1100 Subject: [PATCH] Plane: make auto takeoff independent of compass this solves a problem of poor initial yaw due to poor compass offsets causing a takeoff to not be in the direction the plane is pointing. A summed gyro is used until the GPS speed is above 5m/s for 2 seconds, then the GPS heading corrected by the summed gyro error is used for L1 based navigation for the rest of the takeoff --- ArduPlane/ArduPlane.pde | 14 ++++++++++++-- ArduPlane/Attitude.pde | 16 ++++++++-------- ArduPlane/commands_logic.pde | 32 +++++++++++++++++++++++++++----- ArduPlane/system.pde | 3 +++ 4 files changed, 50 insertions(+), 15 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 46d96d5e7d..a5da5ea509 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -514,7 +514,7 @@ static struct { int32_t hold_course_cd; // locked_course and locked_course_cd are used in stabilize mode - // when ground steering is active + // when ground steering is active, and for steering in auto-takeoff bool locked_course; float locked_course_err; } steer_state = { @@ -564,6 +564,9 @@ static struct { // filtered sink rate for landing float land_sink_rate; + + // time when we first pass min GPS speed on takeoff + uint32_t takeoff_speed_time_ms; } auto_state = { takeoff_complete : true, land_complete : false, @@ -576,7 +579,8 @@ static struct { highest_airspeed : 0, initial_pitch_cd : 0, next_turn_angle : 90.0f, - land_sink_rate : 0 + land_sink_rate : 0, + takeoff_speed_time_ms : 0 }; // true if we are in an auto-throttle mode, which means @@ -887,6 +891,12 @@ static void ahrs_update() // calculate a scaled roll limit based on current pitch roll_limit_cd = g.roll_limit_cd * cosf(ahrs.pitch); pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll)); + + // updated the summed gyro used for ground steering and + // auto-takeoff. Dot product of DCM.c with gyro vector gives earth + // frame yaw rate + steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt; + steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err); } /* diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 199d5ca601..0a8d24d923 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -446,7 +446,8 @@ static void calc_nav_yaw_course(void) static void calc_nav_yaw_ground(void) { if (gps.ground_speed() < 1 && - channel_throttle->control_in == 0) { + channel_throttle->control_in == 0 && + flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF) { // manual rudder control while still steer_state.locked_course = false; steer_state.locked_course_err = 0; @@ -455,20 +456,24 @@ static void calc_nav_yaw_ground(void) } float steer_rate = (channel_rudder->control_in/4500.0f) * g.ground_steer_dps; + if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) { + steer_rate = 0; + } if (steer_rate != 0) { // pilot is giving rudder input steer_state.locked_course = false; } else if (!steer_state.locked_course) { // pilot has released the rudder stick or we are still - lock the course steer_state.locked_course = true; - steer_state.locked_course_err = 0; + if (flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF) { + steer_state.locked_course_err = 0; + } } if (!steer_state.locked_course) { // use a rate controller at the pilot specified rate steering_control.steering = steerController.get_steering_out_rate(steer_rate); } else { // use a error controller on the summed error - steer_state.locked_course_err += ahrs.get_gyro().z * G_Dt; int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100; steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd); } @@ -588,11 +593,6 @@ static bool suppress_throttle(void) auto_takeoff_check()) { // we're in auto takeoff throttle_suppressed = false; - if (steer_state.hold_course_cd != -1) { - // update takeoff course hold, if already initialised - steer_state.hold_course_cd = ahrs.yaw_sensor; - gcs_send_text_fmt(PSTR("Holding course %ld"), steer_state.hold_course_cd); - } return false; } diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index a6bb764231..bcc946edb1 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -282,8 +282,13 @@ static void do_takeoff(const AP_Mission::Mission_Command& cmd) auto_state.takeoff_altitude_cm = next_WP_loc.alt; next_WP_loc.lat = home.lat + 10; next_WP_loc.lng = home.lng + 10; + auto_state.takeoff_speed_time_ms = 0; auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction // Flag also used to override "on the ground" throttle disable + + // zero locked course + steer_state.locked_course_err = 0; + } static void do_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -332,11 +337,28 @@ static void do_loiter_time(const AP_Mission::Mission_Command& cmd) /********************************************************************************/ static bool verify_takeoff() { - if (ahrs.yaw_initialised()) { - if (steer_state.hold_course_cd == -1) { - // save our current course to take off - steer_state.hold_course_cd = ahrs.yaw_sensor; - gcs_send_text_fmt(PSTR("Holding course %ld"), steer_state.hold_course_cd); + if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) { + const float min_gps_speed = 5; + if (auto_state.takeoff_speed_time_ms == 0 && + gps.status() >= AP_GPS::GPS_OK_FIX_3D && + gps.ground_speed() > min_gps_speed) { + auto_state.takeoff_speed_time_ms = hal.scheduler->millis(); + } + if (auto_state.takeoff_speed_time_ms != 0 && + hal.scheduler->millis() - auto_state.takeoff_speed_time_ms >= 2000) { + // once we reach sufficient speed for good GPS course + // estimation we save our current GPS ground course + // corrected for summed yaw to set the take off + // course. This keeps wings level until we are ready to + // rotate, and also allows us to cope with arbitary + // compass errors for auto takeoff + float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01)) - steer_state.locked_course_err; + takeoff_course = wrap_PI(takeoff_course); + steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100); + gcs_send_text_fmt(PSTR("Holding course %ld at %.1fm/s (%.1f)"), + steer_state.hold_course_cd, + gps.ground_speed(), + degrees(steer_state.locked_course_err)); } } diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index af97d4d04e..66252d2682 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -297,6 +297,9 @@ static void set_mode(enum FlightMode mode) // don't cross-track when starting a mission auto_state.next_wp_no_crosstrack = true; + // zero locked course + steer_state.locked_course_err = 0; + // set mode previous_mode = control_mode; control_mode = mode;