Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-28 10:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add rangefinder_alt_ok

Browse Source 
Reduces some duplicate code,no functional change.
This commit is contained in:
Randy Mackay 2016-04-27 21:18:35 +09:00
parent 7689315ba2
commit 949d5f7109
8 changed files with 18 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -947,6 +947,7 @@ private:
void read_barometer(void); void read_barometer(void);
void init_rangefinder(void); void init_rangefinder(void);
int16_t read_rangefinder(void); int16_t read_rangefinder(void);
bool rangefinder_alt_ok();
void init_compass(); void init_compass();
void init_optflow(); void init_optflow();
void update_optical_flow(void); void update_optical_flow(void);

4
ArduCopter/control_althold.cpp
View File

@ -159,8 +159,8 @@ void Copter::althold_run()
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// call throttle controller // adjust climb rate using rangefinder
if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking // 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); target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
} }

4
ArduCopter/control_circle.cpp
View File

@ -90,8 +90,8 @@ void Copter::circle_run()
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true); attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
} }
// run altitude controller // adjust climb rate using rangefinder
if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking // 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); target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
} }

4
ArduCopter/control_loiter.cpp
View File

@ -180,8 +180,8 @@ void Copter::loiter_run()
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// run altitude controller // adjust climb rate using rangefinder
if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking // 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); target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
} }

4
ArduCopter/control_poshold.cpp
View File

@ -519,8 +519,8 @@ void Copter::poshold_run()
// update attitude controller targets // update attitude controller targets
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);
// throttle control // adjust climb rate using rangefinder
if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking // 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); target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
} }

4
ArduCopter/control_sport.cpp
View File

@ -107,8 +107,8 @@ void Copter::sport_run()
// call attitude controller // call attitude controller
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);
// call throttle controller // adjust climb rate using rangefinder
if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking // 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); target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
} }

2
ArduCopter/precision_landing.cpp
View File

@ -18,7 +18,7 @@ void Copter::update_precland()
float final_alt = current_loc.alt; float final_alt = current_loc.alt;
// use range finder altitude if it is valid // use range finder altitude if it is valid
if (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) { if (rangefinder_alt_ok()) {
final_alt = rangefinder_alt; final_alt = rangefinder_alt;
} }

6
ArduCopter/sensors.cpp
View File

@ -69,6 +69,12 @@ int16_t Copter::read_rangefinder(void)
#endif #endif
} }
// return true if rangefinder_alt can be used
bool Copter::rangefinder_alt_ok()
{
return (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX));
}
/* /*
update RPM sensors update RPM sensors
*/ */