Browse Source

Plane: fixed reset of steering locked course

reset when we have not been steering for 1s, to ensure that an old
locked course is not used
zr-v5.1
Andrew Tridgell 3 years ago committed by Randy Mackay
parent
commit
9ee5912e8b
  1. 8
      ArduPlane/Attitude.cpp
  2. 1
      ArduPlane/Plane.h
  3. 1
      ArduPlane/mode.cpp

8
ArduPlane/Attitude.cpp

@ -592,6 +592,13 @@ void Plane::calc_nav_yaw_ground(void) @@ -592,6 +592,13 @@ void Plane::calc_nav_yaw_ground(void)
return;
}
// if we haven't been steering for 1s then clear locked course
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - steer_state.last_steer_ms > 1000) {
steer_state.locked_course = false;
}
steer_state.last_steer_ms = now_ms;
float steer_rate = (rudder_input()/4500.0f) * g.ground_steer_dps;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF ||
flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
@ -608,6 +615,7 @@ void Plane::calc_nav_yaw_ground(void) @@ -608,6 +615,7 @@ void Plane::calc_nav_yaw_ground(void)
steer_state.locked_course_err = 0;
}
}
if (!steer_state.locked_course) {
// use a rate controller at the pilot specified rate
steering_control.steering = steerController.get_steering_out_rate(steer_rate);

1
ArduPlane/Plane.h

@ -431,6 +431,7 @@ private: @@ -431,6 +431,7 @@ private:
// when ground steering is active, and for steering in auto-takeoff
bool locked_course;
float locked_course_err;
uint32_t last_steer_ms;
} steer_state;
// flight mode specific

1
ArduPlane/mode.cpp

@ -23,6 +23,7 @@ bool Mode::enter() @@ -23,6 +23,7 @@ bool Mode::enter()
// zero locked course
plane.steer_state.locked_course_err = 0;
plane.steer_state.locked_course = false;
// reset crash detection
plane.crash_state.is_crashed = false;

Loading…
Cancel
Save