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

FlightTaskOffboard: only start task if control mode flags are met

Browse Source
This commit is contained in:
Dennis Mannhart 2018-06-25 11:33:30 +02:00 committed by Lorenz Meier
parent bf4ac7a9d6
commit b5731e0ccd
2 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp
View File

@ -55,6 +55,8 @@ bool FlightTaskOffboard::initializeSubscriptions(SubscriptionArray &subscription
bool FlightTaskOffboard::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
// require a valid triplet
ret = ret && _sub_triplet_setpoint->get().current.valid;
// require valid position / velocity in xy
return ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
@ -245,5 +247,4 @@ bool FlightTaskOffboard::update()
}
return true;
}

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

@ -519,7 +519,7 @@ MulticopterPositionControl::task_main()
setDt(dt);
if (_control_mode.flag_armed) {
// as soon vehicle is armed, start flighttask
// as soon vehicle is armed check for flighttask
start_flight_task();
// arm hysteresis prevents vehicle to takeoff
// before propeller reached idle speed.
@ -646,7 +646,7 @@ MulticopterPositionControl::task_main()
// publish attitude setpoint
// Note: this requires review. The reason for not sending
// an attitude setpoint is because for none-flighttask modes
// 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.
publish_attitude();
@ -678,7 +678,13 @@ MulticopterPositionControl::start_flight_task()
bool task_failure = false;
// offboard
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
&& (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled)) {
int error = _flight_tasks.switchTask(FlightTaskIndex::Offboard);
if (error != 0) {