mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 11:03:34 +08:00
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:
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);
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user