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

Copter: get_alt_above_ground to get_alt_above_ground_cm

Browse Source
This commit is contained in:
Peter Barker 2019-02-26 14:07:55 +11:00 committed by Randy Mackay
parent a7589a4aed
commit 2c814e9de2
3 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/landing_gear.cpp
View File

@ -30,7 +30,7 @@ void Copter::landinggear_update()
last_deploy_status = landinggear.deployed(); last_deploy_status = landinggear.deployed();
// support height based triggering using rangefinder or altitude above ground // support height based triggering using rangefinder or altitude above ground
int32_t height_cm = flightmode->get_alt_above_ground(); int32_t height_cm = flightmode->get_alt_above_ground_cm();
// use rangefinder if available // use rangefinder if available
switch (rangefinder.status_orient(ROTATION_PITCH_270)) { switch (rangefinder.status_orient(ROTATION_PITCH_270)) {

8
ArduCopter/mode.cpp
View File

@ -393,7 +393,7 @@ void Copter::Mode::zero_throttle_and_relax_ac(bool spool_up)
/* /*
get a height above ground estimate for landing get a height above ground estimate for landing
*/ */
int32_t Copter::Mode::get_alt_above_ground(void) int32_t Copter::Mode::get_alt_above_ground_cm(void)
{ {
int32_t alt_above_ground; int32_t alt_above_ground;
if (copter.rangefinder_alt_ok()) { if (copter.rangefinder_alt_ok()) {
@ -419,7 +419,6 @@ void Copter::Mode::land_run_vertical_control(bool pause_descent)
// compute desired velocity // compute desired velocity
const float precland_acceptable_error = 15.0f; const float precland_acceptable_error = 15.0f;
const float precland_min_descent_speed = 10.0f; const float precland_min_descent_speed = 10.0f;
const int32_t alt_above_ground = get_alt_above_ground();
float cmb_rate = 0; float cmb_rate = 0;
if (!pause_descent) { if (!pause_descent) {
@ -434,7 +433,7 @@ void Copter::Mode::land_run_vertical_control(bool pause_descent)
max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed)); max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
// Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low. // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low.
cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-alt_above_ground, pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt); cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt);
// Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed)); cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));
@ -526,8 +525,7 @@ void Copter::Mode::land_run_horizontal_control()
// there is any position estimate drift after touchdown. We // there is any position estimate drift after touchdown. We
// limit attitude to 7 degrees below this limit and linearly // limit attitude to 7 degrees below this limit and linearly
// interpolate for 1m above that // interpolate for 1m above that
const int alt_above_ground = get_alt_above_ground(); float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(),
float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, alt_above_ground,
g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U);
float total_angle_cd = norm(nav_roll, nav_pitch); float total_angle_cd = norm(nav_roll, nav_pitch);
if (total_angle_cd > attitude_limit_cd) { if (total_angle_cd > attitude_limit_cd) {

2
ArduCopter/mode.h
View File

@ -116,7 +116,7 @@ protected:
// functions to control landing // functions to control landing
// in modes that support landing // in modes that support landing
int32_t get_alt_above_ground(void); int32_t get_alt_above_ground_cm(void);
void land_run_horizontal_control(); void land_run_horizontal_control();
void land_run_vertical_control(bool pause_descent = false); void land_run_vertical_control(bool pause_descent = false);