Skip to content

Commit

Permalink
mc_pos_control: start flight task already when disarmed
Browse files Browse the repository at this point in the history
such that the controller always publishes attitude setpoints and there are no initialization issues.
  • Loading branch information
MaEtUgR committed Oct 23, 2019
1 parent 549fb0d commit ed705e9
Showing 1 changed file with 6 additions and 13 deletions.
19 changes: 6 additions & 13 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -562,15 +562,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_current);

// 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()) {
Expand Down Expand Up @@ -702,10 +695,10 @@ MulticopterPositionControl::Run()
_att_sp.apply_flaps = false;

// 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
publish_attitude();

// if there's any change in landing gear setpoint publish it
Expand Down

0 comments on commit ed705e9

Please sign in to comment.