mc_pos_control: start flight task already when disarmed

such that the controller always publishes attitude setpoints and there are no initialization issues.
This commit is contained in:
Matthias Grob
2019-11-24 11:06:03 -05:00
committed by Daniel Agar
parent a69e6b92c9
commit d3bd251ffc
@@ -568,15 +568,8 @@ MulticopterPositionControl::Run()
_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f,
!_control_mode.flag_control_climb_rate_enabled, time_stamp_now); !_control_mode.flag_control_climb_rate_enabled, time_stamp_now);
// takeoff delay for motors to reach idle speed // switch to the required flighttask
if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) { start_flight_task();
// when vehicle is ready switch to the required flighttask
start_flight_task();
} else {
// stop flighttask while disarmed
_flight_tasks.switchTask(FlightTaskIndex::None);
}
// check if any task is active // check if any task is active
if (_flight_tasks.isAnyTaskActive()) { if (_flight_tasks.isAnyTaskActive()) {
@@ -705,10 +698,10 @@ MulticopterPositionControl::Run()
} }
// publish attitude setpoint // publish attitude setpoint
// Note: this requires review. The reason for not sending // It's important to publish also when disarmed otheriwse the attitude setpoint stays uninitialized.
// an attitude setpoint is because for non-flighttask modes // Not publishing when not running a flight task
// the attitude septoint should come from another source, otherwise // in stabilized mode attitude setpoints get ignored
// they might conflict with each other such as in offboard attitude control. // in offboard with attitude setpoints they come from MAVLink directly
if (_attitude_setpoint_id != nullptr) { if (_attitude_setpoint_id != nullptr) {
orb_publish_auto(_attitude_setpoint_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); orb_publish_auto(_attitude_setpoint_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
} }