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: move land_detector to separate file

Browse Source
This commit is contained in:
Randy Mackay 2015-01-14 12:49:39 +09:00
parent 36410a5131
commit 1c25c00f5b
2 changed files with 43 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
ArduCopter/control_land.pde
View File

@ -1,7 +1,5 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// counter to verify landings
static uint16_t land_detector = LAND_DETECTOR_TRIGGER; // we assume we are landed
static bool land_with_gps;
static uint32_t land_start_time;
@ -198,45 +196,6 @@ static float get_land_descent_speed()
}
}
// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
static bool land_complete_maybe()
{
return (ap.land_complete || ap.land_complete_maybe);
}
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at 50hz
static void update_land_detector()
{
bool climb_rate_small = abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX;
bool target_climb_rate_low = !pos_control.is_active_z() || pos_control.get_desired_velocity().z < LAND_SPEED;
bool motor_at_lower_limit = motors.limit.throttle_lower;
bool throttle_low = FRAME_CONFIG == HELI_FRAME || motors.get_throttle_out() < get_non_takeoff_throttle();
bool not_rotating_fast = ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX;
if (climb_rate_small && target_climb_rate_low && motor_at_lower_limit && throttle_low && not_rotating_fast) {
if (!ap.land_complete) {
// increase counter until we hit the trigger then set land complete flag
if( land_detector < LAND_DETECTOR_TRIGGER) {
land_detector++;
}else{
set_land_complete(true);
land_detector = LAND_DETECTOR_TRIGGER;
}
}
} else {
// we've sensed movement up or down so reset land_detector
land_detector = 0;
// if throttle output is high then clear landing flag
if (motors.get_throttle_out() > get_non_takeoff_throttle()) {
set_land_complete(false);
}
}
// set land maybe flag
set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER);
}
// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
// called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
// has no effect if we are not already in LAND mode

43
ArduCopter/land_detector.pde Normal file
View File

@ -0,0 +1,43 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// counter to verify landings
static uint16_t land_detector = LAND_DETECTOR_TRIGGER; // we assume we are landed
// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
static bool land_complete_maybe()
{
return (ap.land_complete || ap.land_complete_maybe);
}
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at 50hz
static void update_land_detector()
{
bool climb_rate_small = abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX;
bool target_climb_rate_low = !pos_control.is_active_z() || pos_control.get_desired_velocity().z < LAND_SPEED;
bool motor_at_lower_limit = motors.limit.throttle_lower;
bool throttle_low = FRAME_CONFIG == HELI_FRAME || motors.get_throttle_out() < get_non_takeoff_throttle();
bool not_rotating_fast = ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX;
if (climb_rate_small && target_climb_rate_low && motor_at_lower_limit && throttle_low && not_rotating_fast) {
if (!ap.land_complete) {
// increase counter until we hit the trigger then set land complete flag
if( land_detector < LAND_DETECTOR_TRIGGER) {
land_detector++;
}else{
set_land_complete(true);
land_detector = LAND_DETECTOR_TRIGGER;
}
}
} else {
// we've sensed movement up or down so reset land_detector
land_detector = 0;
// if throttle output is high then clear landing flag
if (motors.get_throttle_out() > get_non_takeoff_throttle()) {
set_land_complete(false);
}
}
// set land maybe flag
set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER);
}