From 6ba91369ffe8c9b62496354b7ccd31815a400b4b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 24 Jan 2014 16:23:33 +0900 Subject: [PATCH] Copter: split control_land into gps and non-gps --- ArduCopter/control_land.pde | 162 ++++++++++++++++++++++++------------ 1 file changed, 111 insertions(+), 51 deletions(-) diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index f41f9f1fd7..30784a9e71 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -13,6 +13,8 @@ static bool land_init(bool ignore_checks) // set target to current position wp_nav.init_loiter_target(); } + // initialise altitude target to stopping point + pos_control.set_target_to_stopping_point_z(); return true; } @@ -20,62 +22,120 @@ static bool land_init(bool ignore_checks) // should be called at 100hz or more static void land_run() { - int16_t target_roll, target_pitch; - float target_yaw_rate; - float target_climb_rate; - - // if not auto armed set throttle to zero and exit immediately - if(!ap.auto_armed) { - // To-Do: reset altitude target if we're somehow not landed? - attitude_control.init_targets(); - attitude_control.set_throttle_out(0, false); - return; - } - - // disarming detector -#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED - // disarm when the landing detector says we've landed and throttle is at minimum - if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) { - init_disarm_motors(); - return; - } -#else - // disarm when the landing detector says we've landed - if (ap.land_complete) { - init_disarm_motors(); - return; - } -#endif - - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); - - // get pilot desired lean angles - // To-Do: convert get_pilot_desired_lean_angles to return angles as floats - get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch); - - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); - - // reset target lean angles and heading while landed - if (ap.land_complete) { - attitude_control.init_targets(); - // move throttle to minimum to keep us on the ground - attitude_control.set_throttle_out(0, false); + if (land_with_gps) { + land_gps_run(); }else{ - // call attitude controller - attitude_control.angleef_rp_rateef_y(target_roll, target_pitch, target_yaw_rate); - // body-frame rate controller is run directly from 100hz loop + land_nogps_run(); + } +} - // call landing throttle controller - target_climb_rate = get_throttle_land(); +// land_run - runs the land controller +// horizontal position controlled with loiter controller +// should be called at 100hz or more +static void land_gps_run() +{ + int16_t roll_control = 0, pitch_control = 0; + float target_yaw_rate = 0; - // call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); - pos_control.update_z_controller(); + // if not auto armed or landed set throttle to zero and exit immediately + if(!ap.auto_armed || ap.land_complete) { + attitude_control.init_targets(); + attitude_control.set_throttle_out(0, false); + wp_nav.init_loiter_target(); + +#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED + // disarm when the landing detector says we've landed and throttle is at minimum + if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) { + init_disarm_motors(); + } +#else + // disarm when the landing detector says we've landed + if (ap.land_complete) { + init_disarm_motors(); + } +#endif + return; } - // refetch angle targets for reporting + // process pilot inputs + if (!failsafe.radio) { + // apply SIMPLE mode transform to pilot inputs + update_simple_mode(); + + // process pilot's roll and pitch input + roll_control = g.rc_1.control_in; + pitch_control = g.rc_2.control_in; + + // get pilot's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + } + + // process roll, pitch inputs + wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); + + // run loiter controller + wp_nav.update_loiter(); + + // call attitude controller + attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + + // update altitude target and call position controller + pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt); + pos_control.update_z_controller(); + + // re-fetch angle targets for reporting + const Vector3f angle_target = attitude_control.angle_ef_targets(); + control_roll = angle_target.x; + control_pitch = angle_target.y; + control_yaw = angle_target.z; +} + +// land_nogps_run - runs the land controller +// pilot controls roll and pitch angles +// should be called at 100hz or more +static void land_nogps_run() +{ + int16_t target_roll = 0, target_pitch = 0; + float target_yaw_rate = 0; + + // if not auto armed or landed set throttle to zero and exit immediately + if(!ap.auto_armed || ap.land_complete) { + attitude_control.init_targets(); + attitude_control.set_throttle_out(0, false); +#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED + // disarm when the landing detector says we've landed and throttle is at minimum + if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) { + init_disarm_motors(); + } +#else + // disarm when the landing detector says we've landed + if (ap.land_complete) { + init_disarm_motors(); + } +#endif + return; + } + + // process pilot inputs + if (!failsafe.radio) { + // apply SIMPLE mode transform to pilot inputs + update_simple_mode(); + + // get pilot desired lean angles + get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch); + + // get pilot's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); + } + + // call attitude controller + attitude_control.angleef_rp_rateef_y(target_roll, target_pitch, target_yaw_rate); + + // call position controller + pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt); + pos_control.update_z_controller(); + + // re-fetch angle targets for reporting const Vector3f angle_target = attitude_control.angle_ef_targets(); control_roll = angle_target.x; control_pitch = angle_target.y;