fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: start flight task already when disarmed

Browse Source 
such that the controller always publishes attitude setpoints and there are no initialization issues.
This commit is contained in:
Matthias Grob 2019-10-16 16:28:05 +02:00 committed by Daniel Agar
parent a69e6b92c9
commit d3bd251ffc
1 changed files with 6 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

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