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

Copter: reduce args passed to get_surface_tracking_climb_rate

Browse Source 
The same arguments are always passed in
This commit is contained in:
Randy Mackay 2019-04-11 11:36:13 +09:00
parent a732dfed6d
commit 33a57361bd
11 changed files with 15 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/Attitude.cpp
View File

@ -121,7 +121,7 @@ float Copter::get_non_takeoff_throttle()
// get_surface_tracking_climb_rate - hold copter at the desired distance above the ground // get_surface_tracking_climb_rate - hold copter at the desired distance above the ground
// returns climb rate (in cm/s) which should be passed to the position controller // returns climb rate (in cm/s) which should be passed to the position controller
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)
{ {
#if RANGEFINDER_ENABLED == ENABLED #if RANGEFINDER_ENABLED == ENABLED
if (!copter.rangefinder_alt_ok()) { if (!copter.rangefinder_alt_ok()) {
@ -130,9 +130,10 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
} }
static uint32_t last_call_ms = 0; static uint32_t last_call_ms = 0;
const float current_alt = inertial_nav.get_altitude();
const float current_alt_target = pos_control->get_alt_target();
float distance_error; float distance_error;
float velocity_correction; float velocity_correction;
float current_alt = inertial_nav.get_altitude();
uint32_t now = millis(); uint32_t now = millis();
@ -146,7 +147,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
// adjust rangefinder target alt if motors have not hit their limits // adjust rangefinder target alt if motors have not hit their limits
if ((target_rate<0 && !motors->limit.throttle_lower) || (target_rate>0 && !motors->limit.throttle_upper)) { if ((target_rate<0 && !motors->limit.throttle_lower) || (target_rate>0 && !motors->limit.throttle_upper)) {
target_rangefinder_alt += target_rate * dt; target_rangefinder_alt += target_rate * G_Dt;
} }
/* /*

2
ArduCopter/Copter.h
View File

@ -617,7 +617,7 @@ private:
void set_throttle_takeoff(); void set_throttle_takeoff();
float get_pilot_desired_climb_rate(float throttle_control); float get_pilot_desired_climb_rate(float throttle_control);
float get_non_takeoff_throttle(); float get_non_takeoff_throttle();
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); float get_surface_tracking_climb_rate(int16_t target_rate);
float get_avoidance_adjusted_climbrate(float target_rate); float get_avoidance_adjusted_climbrate(float target_rate);
void set_accel_throttle_I_from_pilot_throttle(); void set_accel_throttle_I_from_pilot_throttle();
void rotate_body_frame_to_NE(float &x, float &y); void rotate_body_frame_to_NE(float &x, float &y);

4
ArduCopter/mode.cpp
View File

@ -688,9 +688,9 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli
// these are candidates for moving into the Mode base // these are candidates for moving into the Mode base
// class. // class.
float Copter::Mode::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) float Copter::Mode::get_surface_tracking_climb_rate(int16_t target_rate)
{ {
return copter.get_surface_tracking_climb_rate(target_rate, current_alt_target, dt); return copter.get_surface_tracking_climb_rate(target_rate);
} }
float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle) float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)

2
ArduCopter/mode.h
View File

@ -197,7 +197,7 @@ protected:
// pass-through functions to reduce code churn on conversion; // pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base // these are candidates for moving into the Mode base
// class. // class.
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); float get_surface_tracking_climb_rate(int16_t target_rate);
float get_pilot_desired_yaw_rate(int16_t stick_angle); float get_pilot_desired_yaw_rate(int16_t stick_angle);
float get_pilot_desired_climb_rate(float throttle_control); float get_pilot_desired_climb_rate(float throttle_control);
float get_pilot_desired_throttle() const; float get_pilot_desired_throttle() const;

2
ArduCopter/mode_althold.cpp
View File

@ -96,7 +96,7 @@ void Copter::ModeAltHold::run()
#endif #endif
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
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);
// 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);

2
ArduCopter/mode_circle.cpp
View File

@ -44,7 +44,7 @@ void Copter::ModeCircle::run()
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) { if (copter.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);
} }
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately

2
ArduCopter/mode_flowhold.cpp
View File

@ -300,7 +300,7 @@ void Copter::ModeFlowHold::run()
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate, copter.pos_control->get_alt_target(), copter.G_Dt); target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate);
// 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);

2
ArduCopter/mode_loiter.cpp
View File

@ -187,7 +187,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
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);
// 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);

2
ArduCopter/mode_poshold.cpp
View File

@ -234,7 +234,7 @@ void Copter::ModePosHold::run()
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) { if (copter.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);
} }
// get avoidance adjusted climb rate // get avoidance adjusted climb rate

2
ArduCopter/mode_sport.cpp
View File

@ -121,7 +121,7 @@ void Copter::ModeSport::run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// adjust climb rate using rangefinder // adjust climb rate using rangefinder
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);
// 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);

2
ArduCopter/mode_zigzag.cpp
View File

@ -192,7 +192,7 @@ void Copter::ModeZigZag::manual_control()
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
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);
// 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);