Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 15:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: make libraries get EKF control limits themselves

Browse Source
This commit is contained in:
Peter Barker 2018-09-05 16:47:50 +10:00 committed by Andrew Tridgell
parent 3faf7824c0
commit 26ca75efae
10 changed files with 13 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/Copter.h
View File

@ -294,12 +294,6 @@ private:
OpticalFlow optflow{ahrs};
#endif
// gnd speed limit required to observe optical flow sensor limits
float ekfGndSpdLimit;
// scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise
float ekfNavVelGainScaler;
// system time in milliseconds of last recorded yaw reset from ekf
uint32_t ekfYawReset_ms;
int8_t ekf_primary_core;

9
ArduCopter/mode.cpp
View File

@ -23,12 +23,10 @@ Copter::Mode::Mode(void) :
channel_throttle(copter.channel_throttle),
channel_yaw(copter.channel_yaw),
G_Dt(copter.G_Dt),
ap(copter.ap),
ekfGndSpdLimit(copter.ekfGndSpdLimit),
#if FRAME_CONFIG == HELI_FRAME
heli_flags(copter.heli_flags),
#endif
ekfNavVelGainScaler(copter.ekfNavVelGainScaler)
ap(copter.ap)
{ };
float Copter::Mode::auto_takeoff_no_nav_alt_cm = 0;
@ -251,9 +249,6 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
// called at 100hz or more
void Copter::update_flight_mode()
{
// Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
target_rangefinder_alt_used = false;
flightmode->run();
@ -519,7 +514,7 @@ void Copter::Mode::land_run_horizontal_control()
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
// run loiter controller
loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
loiter_nav->update();
int32_t nav_roll = loiter_nav->get_roll();
int32_t nav_pitch = loiter_nav->get_pitch();

6
ArduCopter/mode.h
View File

@ -173,12 +173,6 @@ protected:
// altitude below which we do no navigation in auto takeoff
static float auto_takeoff_no_nav_alt_cm;
// gnd speed limit required to observe optical flow sensor limits
float &ekfGndSpdLimit;
// scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise
float &ekfNavVelGainScaler;
#if FRAME_CONFIG == HELI_FRAME
heli_flags_t &heli_flags;
#endif

2
ArduCopter/mode_auto.cpp
View File

@ -980,7 +980,7 @@ void Copter::ModeAuto::payload_place_run_loiter()
land_run_horizontal_control();
// run loiter controller
loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
loiter_nav->update();
// call attitude controller
const float target_yaw_rate = 0;

2
ArduCopter/mode_brake.cpp
View File

@ -56,7 +56,7 @@ void Copter::ModeBrake::run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run brake controller
wp_nav->update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);
wp_nav->update_brake();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f);

4
ArduCopter/mode_guided.cpp
View File

@ -497,7 +497,7 @@ void Copter::ModeGuided::vel_control_run()
}
// call velocity controller which includes z axis controller
pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
pos_control->update_vel_controller_xyz();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
@ -564,7 +564,7 @@ void Copter::ModeGuided::posvel_control_run()
pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y);
// run position controllers
pos_control->update_xy_controller(ekfNavVelGainScaler);
pos_control->update_xy_controller();
pos_control->update_z_controller();
// call attitude controller

6
ArduCopter/mode_loiter.cpp
View File

@ -143,7 +143,7 @@ void Copter::ModeLoiter::run()
attitude_control->set_yaw_target_to_current_heading();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif
loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
loiter_nav->update();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
pos_control->update_z_controller();
break;
@ -168,7 +168,7 @@ void Copter::ModeLoiter::run()
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// run loiter controller
loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
loiter_nav->update();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
@ -206,7 +206,7 @@ void Copter::ModeLoiter::run()
#endif
// run loiter controller
loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
loiter_nav->update();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);

4
ArduCopter/mode_poshold.cpp
View File

@ -442,7 +442,7 @@ void Copter::ModePosHold::run()
poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
// run loiter controller
loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
loiter_nav->update();
// calculate final roll and pitch output by mixing loiter and brake controls
poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, loiter_nav->get_roll());
@ -473,7 +473,7 @@ void Copter::ModePosHold::run()
case POSHOLD_LOITER:
// run loiter controller
loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
loiter_nav->update();
// set roll angle based on loiter controller outputs
poshold.roll = loiter_nav->get_roll();

2
ArduCopter/mode_rtl.cpp
View File

@ -304,7 +304,7 @@ void Copter::ModeRTL::descent_run()
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
// run loiter controller
loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
loiter_nav->update();
// call z-axis position controller
pos_control->set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt);

2
ArduCopter/mode_throw.cpp
View File

@ -169,7 +169,7 @@ void Copter::ModeThrow::run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run loiter controller
loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
loiter_nav->update();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), 0.0f);