Unverified Commit 99501b4c authored by Daniel Agar's avatar Daniel Agar
Browse files

flight_mode_manager: ManualAcceleration support weathervane yaw handler

 - fixes https://github.com/PX4/PX4-Autopilot/issues/17911
parent 9e4a04f5
......@@ -74,6 +74,18 @@ bool FlightTaskManualAcceleration::update()
_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);
_constraints.want_takeoff = _checkTakeoff();
// check if an external yaw handler is active and if yes, let it update the yaw setpoints
if (_weathervane_yaw_handler && _weathervane_yaw_handler->is_active()) {
_yaw_setpoint = NAN;
// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
// vehicle is steady
_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
}
}
return ret;
}
......
......@@ -52,10 +52,17 @@ public:
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool update() override;
/**
* Sets an external yaw handler which can be used to implement a different yaw control strategy.
*/
void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; }
private:
StickAccelerationXY _stick_acceleration_xy;
StickYaw _stick_yaw;
void _ekfResetHandlerPositionXY() override;
void _ekfResetHandlerVelocityXY() override;
WeatherVane *_weathervane_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
};
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment