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

Sub: make libraries get EKF control limits themselves

Browse Source
This commit is contained in:
Peter Barker 2018-09-06 08:37:00 +10:00 committed by Andrew Tridgell
parent 4e3e39a3e8
commit 47b5cf98ea
5 changed files with 4 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduSub/Sub.h
View File

@ -207,12 +207,6 @@ private:
OpticalFlow optflow{ahrs}; OpticalFlow optflow{ahrs};
#endif #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 // system time in milliseconds of last recorded yaw reset from ekf
uint32_t ekfYawReset_ms = 0; uint32_t ekfYawReset_ms = 0;

2
ArduSub/control_auto.cpp
View File

@ -734,7 +734,7 @@ void Sub::auto_terrain_recover_run()
} }
// run loiter controller // run loiter controller
loiter_nav.update(ekfGndSpdLimit, ekfNavVelGainScaler); loiter_nav.update();
/////////////////////// ///////////////////////
// update xy targets // // update xy targets //

4
ArduSub/control_guided.cpp
View File

@ -369,7 +369,7 @@ void Sub::guided_vel_control_run()
} }
// call velocity controller which includes z axis controller // call velocity controller which includes z axis controller
pos_control.update_vel_controller_xyz(ekfNavVelGainScaler); pos_control.update_vel_controller_xyz();
float lateral_out, forward_out; float lateral_out, forward_out;
translate_pos_control_rp(lateral_out, forward_out); translate_pos_control_rp(lateral_out, forward_out);
@ -440,7 +440,7 @@ void Sub::guided_posvel_control_run()
pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y); pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y);
// run position controller // run position controller
pos_control.update_xy_controller(ekfNavVelGainScaler); pos_control.update_xy_controller();
float lateral_out, forward_out; float lateral_out, forward_out;
translate_pos_control_rp(lateral_out, forward_out); translate_pos_control_rp(lateral_out, forward_out);

2
ArduSub/control_poshold.cpp
View File

@ -52,7 +52,7 @@ void Sub::poshold_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run loiter controller // run loiter controller
loiter_nav.update(ekfGndSpdLimit, ekfNavVelGainScaler); loiter_nav.update();
/////////////////////// ///////////////////////
// update xy outputs // // update xy outputs //

3
ArduSub/flight_mode.cpp
View File

@ -98,9 +98,6 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
// called at 100hz or more // called at 100hz or more
void Sub::update_flight_mode() void Sub::update_flight_mode()
{ {
// Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
switch (control_mode) { switch (control_mode) {
case ACRO: case ACRO:
acro_run(); acro_run();