Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: split control_land into gps and non-gps

Browse Source
This commit is contained in:
Randy Mackay 2014-01-24 16:23:33 +09:00 committed by Andrew Tridgell
parent 011de6683e
commit 6ba91369ff
1 changed files with 111 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

162
ArduCopter/control_land.pde
View File

@ -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;