Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: move mode manual reset into run function removing manual early return

This commit is contained in:
Iampete1 2023-04-24 13:28:55 +01:00 committed by Andrew Tridgell
parent 160629eb18
commit 2254252aff
4 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ArduPlane/Attitude.cpp
View File

@ -346,13 +346,6 @@ void Plane::stabilize_yaw()
*/ */
void Plane::stabilize() void Plane::stabilize()
{ {
if (control_mode == &mode_manual) {
// reset steering controls
steer_state.locked_course = false;
steer_state.locked_course_err = 0;
return;
}
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
if (quadplane.available()) { if (quadplane.available()) {
@ -361,19 +354,13 @@ void Plane::stabilize()
#endif #endif
if (now - last_stabilize_ms > 2000) { if (now - last_stabilize_ms > 2000) {
// if we haven't run the rate controllers for 2 seconds then // if we haven't run the rate controllers for 2 seconds then reset
// reset the integrators control_mode->reset_controllers();
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
// and reset steering controls
steer_state.locked_course = false;
steer_state.locked_course_err = 0;
} }
last_stabilize_ms = now; last_stabilize_ms = now;
if (control_mode == &mode_training) { if (control_mode == &mode_training ||
control_mode == &mode_manual) {
plane.control_mode->run(); plane.control_mode->run();
#if AP_SCRIPTING_ENABLED #if AP_SCRIPTING_ENABLED
} else if (nav_scripting_active()) { } else if (nav_scripting_active()) {

13
ArduPlane/mode.cpp
View File

@ -212,3 +212,16 @@ void Mode::run()
plane.stabilize_pitch(); plane.stabilize_pitch();
plane.stabilize_yaw(); plane.stabilize_yaw();
} }
// Reset rate and steering controllers
void Mode::reset_controllers()
{
// reset integrators
plane.rollController.reset_I();
plane.pitchController.reset_I();
plane.yawController.reset_I();
// reset steering controls
plane.steer_state.locked_course = false;
plane.steer_state.locked_course_err = 0;
}

5
ArduPlane/mode.h
View File

@ -79,6 +79,9 @@ public:
// returns true if the vehicle can be armed in this mode // returns true if the vehicle can be armed in this mode
bool pre_arm_checks(size_t buflen, char *buffer) const; bool pre_arm_checks(size_t buflen, char *buffer) const;
// Reset rate and steering controllers
void reset_controllers();
// //
// methods that sub classes should override to affect movement of the vehicle in this mode // methods that sub classes should override to affect movement of the vehicle in this mode
// //
@ -355,6 +358,8 @@ public:
// methods that affect movement of the vehicle in this mode // methods that affect movement of the vehicle in this mode
void update() override; void update() override;
void run() override;
}; };

4
ArduPlane/mode_manual.cpp
View File

@ -11,3 +11,7 @@ void ModeManual::update()
plane.nav_pitch_cd = ahrs.pitch_sensor; plane.nav_pitch_cd = ahrs.pitch_sensor;
} }
void ModeManual::run()
{
reset_controllers();
}