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

Copter: make surface_tracking a class, various functions methods

Browse Source
This commit is contained in:
Peter Barker 2019-06-11 13:13:24 +10:00 committed by Randy Mackay
parent 6896b7b302
commit b0428f0fe8
11 changed files with 55 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
ArduCopter/Attitude.cpp
View File

@ -119,9 +119,10 @@ float Copter::get_non_takeoff_throttle()
return MAX(0,motors->get_throttle_hover()/2.0f);
}
// 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
float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
// adjust_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
float Copter::SurfaceTracking::adjust_climb_rate(int16_t target_rate)
{
#if RANGEFINDER_ENABLED == ENABLED
if (!copter.rangefinder_alt_ok()) {
@ -129,24 +130,24 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
return target_rate;
}
const float current_alt = inertial_nav.get_altitude();
const float current_alt_target = pos_control->get_alt_target();
const float current_alt = copter.inertial_nav.get_altitude();
const float current_alt_target = copter.pos_control->get_alt_target();
float distance_error;
float velocity_correction;
uint32_t now = millis();
surface_tracking.valid_for_logging = true;
valid_for_logging = true;
// reset target altitude if this controller has just been engaged
if (now - surface_tracking.last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
surface_tracking.target_alt_cm = rangefinder_state.alt_cm + current_alt_target - current_alt;
if (now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_target - current_alt;
}
surface_tracking.last_update_ms = now;
last_update_ms = now;
// 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)) {
surface_tracking.target_alt_cm += target_rate * G_Dt;
if ((target_rate<0 && !copter.motors->limit.throttle_lower) || (target_rate>0 && !copter.motors->limit.throttle_upper)) {
target_alt_cm += target_rate * copter.G_Dt;
}
/*
@ -157,27 +158,27 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
row. When that happens we reset the target altitude to the new
reading
*/
int32_t glitch_cm = rangefinder_state.alt_cm - surface_tracking.target_alt_cm;
int32_t glitch_cm = copter.rangefinder_state.alt_cm - target_alt_cm;
if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
rangefinder_state.glitch_count = MAX(rangefinder_state.glitch_count+1,1);
copter.rangefinder_state.glitch_count = MAX(copter.rangefinder_state.glitch_count+1,1);
} else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
rangefinder_state.glitch_count = MIN(rangefinder_state.glitch_count-1,-1);
copter.rangefinder_state.glitch_count = MIN(copter.rangefinder_state.glitch_count-1,-1);
} else {
rangefinder_state.glitch_count = 0;
copter.rangefinder_state.glitch_count = 0;
}
if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
if (abs(copter.rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
// shift to the new rangefinder reading
surface_tracking.target_alt_cm = rangefinder_state.alt_cm;
rangefinder_state.glitch_count = 0;
target_alt_cm = copter.rangefinder_state.alt_cm;
copter.rangefinder_state.glitch_count = 0;
}
if (rangefinder_state.glitch_count != 0) {
if (copter.rangefinder_state.glitch_count != 0) {
// we are currently glitching, just use the target rate
return target_rate;
}
// calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
distance_error = (surface_tracking.target_alt_cm - rangefinder_state.alt_cm) - (current_alt_target - current_alt);
velocity_correction = distance_error * g.rangefinder_gain;
distance_error = (target_alt_cm - copter.rangefinder_state.alt_cm) - (current_alt_target - current_alt);
velocity_correction = distance_error * copter.g.rangefinder_gain;
velocity_correction = constrain_float(velocity_correction, -SURFACE_TRACKING_VELZ_MAX, SURFACE_TRACKING_VELZ_MAX);
// return combined pilot climb rate + rate to correct rangefinder alt error
@ -189,21 +190,21 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
// get surfacing tracking alt
// returns true if there is a valid target
bool Copter::get_surface_tracking_target_alt_cm(float &target_alt_cm) const
bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
{
// check target has been updated recently
if (AP_HAL::millis() - surface_tracking.last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
return false;
}
target_alt_cm = surface_tracking.target_alt_cm;
_target_alt_cm = target_alt_cm;
return true;
}
// set surface tracking target altitude
void Copter::set_surface_tracking_target_alt_cm(float target_alt_cm)
void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
{
surface_tracking.target_alt_cm = target_alt_cm;
surface_tracking.last_update_ms = AP_HAL::millis();
target_alt_cm = _target_alt_cm;
last_update_ms = AP_HAL::millis();
}
// get target climb rate reduced to avoid obstacles and altitude fence

20
ArduCopter/Copter.h
View File

@ -287,7 +287,22 @@ private:
int8_t glitch_count;
} rangefinder_state;
struct {
class SurfaceTracking {
public:
float adjust_climb_rate(int16_t target_rate);
bool get_target_alt_cm(float &target_alt_cm) const;
void set_target_alt_cm(float target_alt_cm);
float logging_target_alt() const {
if (!valid_for_logging) {
return AP::logger().quiet_nan();
}
return target_alt_cm * 0.01f; // cm->m
}
void invalidate_for_logging() {
valid_for_logging = false;
}
private:
float target_alt_cm; // desired altitude in cm above the ground
uint32_t last_update_ms; // system time of last update to target_alt_cm
bool valid_for_logging; // true if target_alt_cm is valid for logging
@ -646,9 +661,6 @@ private:
void set_throttle_takeoff();
float get_pilot_desired_climb_rate(float throttle_control);
float get_non_takeoff_throttle();
float get_surface_tracking_climb_rate(int16_t target_rate);
bool get_surface_tracking_target_alt_cm(float &target_alt_cm) const;
void set_surface_tracking_target_alt_cm(float target_alt_cm);
float get_avoidance_adjusted_climbrate(float target_rate);
void set_accel_throttle_I_from_pilot_throttle();
void rotate_body_frame_to_NE(float &x, float &y);

8
ArduCopter/Log.cpp
View File

@ -39,12 +39,6 @@ void Copter::Log_Write_Control_Tuning()
target_climb_rate_cms = pos_control->get_vel_target_z();
}
float surface_tracking_target_alt;
if (surface_tracking.valid_for_logging) {
surface_tracking_target_alt = surface_tracking.target_alt_cm * 0.01f; // cm->m
} else {
surface_tracking_target_alt = logger.quiet_nan();
}
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
time_us : AP_HAL::micros64(),
@ -55,7 +49,7 @@ void Copter::Log_Write_Control_Tuning()
desired_alt : des_alt_m,
inav_alt : inertial_nav.get_altitude() / 100.0f,
baro_alt : baro_alt,
desired_rangefinder_alt : surface_tracking_target_alt,
desired_rangefinder_alt : surface_tracking.logging_target_alt(),
rangefinder_alt : rangefinder_state.alt_cm,
terr_alt : terr_alt,
target_climb_rate : target_climb_rate_cms,

2
ArduCopter/mode.cpp
View File

@ -265,7 +265,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
// called at 100hz or more
void Copter::update_flight_mode()
{
surface_tracking.valid_for_logging = false; // invalidate surface tracking alt, flight mode will set to true if used
surface_tracking.invalidate_for_logging(); // invalidate surface tracking alt, flight mode will set to true if used
flightmode->run();
}

2
ArduCopter/mode_althold.cpp
View File

@ -92,7 +92,7 @@ void ModeAltHold::run()
#endif
// adjust climb rate using rangefinder
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate);
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

2
ArduCopter/mode_circle.cpp
View File

@ -44,7 +44,7 @@ void ModeCircle::run()
// adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate);
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
}
// if not armed set throttle to zero and exit immediately

2
ArduCopter/mode_flowhold.cpp
View File

@ -296,7 +296,7 @@ void ModeFlowHold::run()
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// adjust climb rate using rangefinder
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate);
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate);

2
ArduCopter/mode_loiter.cpp
View File

@ -183,7 +183,7 @@ void ModeLoiter::run()
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
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate);
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

2
ArduCopter/mode_poshold.cpp
View File

@ -177,7 +177,7 @@ void ModePosHold::run()
// adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate);
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
}
// get avoidance adjusted climb rate

2
ArduCopter/mode_sport.cpp
View File

@ -117,7 +117,7 @@ void ModeSport::run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// adjust climb rate using rangefinder
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate);
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

6
ArduCopter/mode_zigzag.cpp
View File

@ -125,7 +125,7 @@ void ModeZigZag::return_to_manual_control(bool maintain_target)
const Vector3f wp_dest = wp_nav->get_wp_destination();
loiter_nav->init_target(wp_dest);
if (maintain_target && wp_nav->origin_and_destination_are_terrain_alt()) {
copter.set_surface_tracking_target_alt_cm(wp_dest.z);
copter.surface_tracking.set_target_alt_cm(wp_dest.z);
}
gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control");
}
@ -200,7 +200,7 @@ void ModeZigZag::manual_control()
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
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate);
target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
@ -283,7 +283,7 @@ bool ModeZigZag::calculate_next_dest(uint8_t dest_num, bool use_wpnav_alt, Vecto
// if we have a downward facing range finder then use terrain altitude targets
terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used();
if (terrain_alt) {
if (!copter.get_surface_tracking_target_alt_cm(next_dest.z)) {
if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) {
next_dest.z = copter.rangefinder_state.alt_cm_filt.get();
}
} else {