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

Copter: move sanity check for rangefinder alt to within protected func

Browse Source 
get_surface_tracking_climb_rate can do an additional check....
This commit is contained in:
Peter Barker 2018-06-05 20:58:21 +10:00 committed by Randy Mackay
parent 4c8c85f75b
commit 4e3bbe9311
7 changed files with 12 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/Attitude.cpp
View File

@ -163,6 +163,11 @@ float Copter::get_non_takeoff_throttle()
float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
{ {
#if RANGEFINDER_ENABLED == ENABLED #if RANGEFINDER_ENABLED == ENABLED
if (!copter.rangefinder_alt_ok()) {
// if rangefinder is not ok, do not use surface tracking
return target_rate;
}
static uint32_t last_call_ms = 0; static uint32_t last_call_ms = 0;
float distance_error; float distance_error;
float velocity_correction; float velocity_correction;

5
ArduCopter/mode_althold.cpp
View File

@ -142,10 +142,7 @@ void Copter::ModeAltHold::run()
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) { target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}
// get avoidance adjusted climb rate // get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

6
ArduCopter/mode_circle.cpp
View File

@ -86,10 +86,8 @@ void Copter::ModeCircle::run()
} }
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) { target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}
// update altitude target and call position controller // update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
pos_control->update_z_controller(); pos_control->update_z_controller();

5
ArduCopter/mode_flowhold.cpp
View File

@ -354,10 +354,7 @@ void Copter::ModeFlowHold::run()
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) { target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate, copter.pos_control->get_alt_target(), copter.G_Dt);
// if rangefinder is ok, use surface tracking
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate, copter.pos_control->get_alt_target(), copter.G_Dt);
}
// get avoidance adjusted climb rate // get avoidance adjusted climb rate
target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate); target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate);

5
ArduCopter/mode_loiter.cpp
View File

@ -197,10 +197,7 @@ void Copter::ModeLoiter::run()
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) { target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}
// get avoidance adjusted climb rate // get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

5
ArduCopter/mode_poshold.cpp
View File

@ -519,10 +519,7 @@ void Copter::ModePosHold::run()
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) { target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}
// get avoidance adjusted climb rate // get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

5
ArduCopter/mode_sport.cpp
View File

@ -150,10 +150,7 @@ void Copter::ModeSport::run()
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) { target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}
// get avoidance adjusted climb rate // get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);