From e26e8b3b677046b486abdc5094bd41e081cccda9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Aug 2012 11:19:12 +1000 Subject: [PATCH] APM: added RDRSTEER_ PID for steering on ground this allows for rolling takeoff with steering, and use of rudder in landing --- ArduPlane/Attitude.pde | 21 ++++++++++++--------- ArduPlane/Parameters.h | 5 ++++- ArduPlane/Parameters.pde | 1 + ArduPlane/commands_logic.pde | 12 +++++------- ArduPlane/navigation.pde | 19 +------------------ 5 files changed, 23 insertions(+), 35 deletions(-) diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 6d50b2c2fa..76fe1d2fc4 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -172,16 +172,19 @@ static void calc_throttle() // ---------------------------------------------------------------------------------------- static void calc_nav_yaw(float speed_scaler) { -#if HIL_MODE != HIL_MODE_ATTITUDE - Vector3f temp = imu.get_accel(); - long error = (long)(-temp.y*100.0); + // always do rudder mixing from roll + g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out; - // Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero) - g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, speed_scaler); -#else - g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out; - // XXX probably need something here based on heading -#endif + if (hold_course != -1) { + // steering on or close to ground + g.channel_rudder.servo_out += g.pidRdrSteer.get_pid(bearing_error_cd); + } else { + // a PID to coordinate the turn (drive y axis accel to zero) + Vector3f temp = imu.get_accel(); + int32_t error = -temp.y*100.0; + + g.channel_rudder.servo_out += g.pidServoRudder.get_pid(error, speed_scaler); + } } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index c86bed0704..b3f14e9927 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -198,6 +198,7 @@ public: k_param_pidServoRudder, k_param_pidTeThrottle, k_param_pidNavPitchAltitude, + k_param_pidRdrSteer, // 254,255: reserved }; @@ -343,6 +344,7 @@ public: PID pidServoRudder; PID pidTeThrottle; PID pidNavPitchAltitude; + PID pidRdrSteer; Parameters() : // variable default @@ -369,7 +371,8 @@ public: pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC), pidServoRudder (SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX), pidTeThrottle (THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX), - pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM) + pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM), + pidRdrSteer (0, 0, 0, 0) {} }; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 3f90ccfb27..cd80adb1ca 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -515,6 +515,7 @@ const AP_Param::Info var_info[] PROGMEM = { GGROUP(pidServoRudder, "YW2SRV_", PID), GGROUP(pidTeThrottle, "ENRGY2THR_", PID), GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID), + GGROUP(pidRdrSteer, "RDRSTEER_", PID), // variables not in the g class which contain EEPROM saved variables diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index cb6e82cd1e..0757f297b0 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -283,14 +283,11 @@ static void do_loiter_time() /********************************************************************************/ static bool verify_takeoff() { - if (g_gps->ground_speed > 300){ + if (ahrs.yaw_initialised()) { if (hold_course == -1) { // save our current course to take off - if(g.compass_enabled) { - hold_course = ahrs.yaw_sensor; - } else { - hold_course = g_gps->ground_course; - } + hold_course = ahrs.yaw_sensor; + gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course); } } @@ -335,10 +332,11 @@ static bool verify_land() // sudden large roll correction which is very nasty at // this point in the landing. hold_course = ahrs.yaw_sensor; + gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course); } } - if (hold_course != -1){ + if (hold_course != -1) { // recalc bearing error with hold_course; nav_bearing_cd = hold_course; // recalc bearing error diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index d8581ef3fc..d4daa289d2 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -105,24 +105,7 @@ static void calc_gndspeed_undershoot() static void calc_bearing_error() { - if(takeoff_complete == true || g.compass_enabled == true) { - /* - most of the time we use the yaw sensor for heading, even if - we don't have a compass. The yaw sensor is drift corrected - in the DCM library. We only use the gps ground course - directly if we haven't completed takeoff, as the yaw drift - correction won't have had a chance to kick in. Drift - correction using the GPS typically takes 10 seconds or so - for a 180 degree correction. - */ - bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor; - } else { - - // TODO: we need to use the Yaw gyro for in between GPS reads, - // maybe as an offset from a saved gryo value. - bearing_error_cd = nav_bearing_cd - g_gps->ground_course; - } - + bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor; bearing_error_cd = wrap_180_cd(bearing_error_cd); }