Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

FlightModeManager: handle commands all the time

Browse Source 
Moving the command handling to a separate function that gets called
whenever a vehicle command is available to always react on commands
and not just when already a task is running.

This solves e.g. commanding an Orbit when in Staibilized.
This commit is contained in:
Matthias Grob
2021-01-17 15:01:28 +01:00
committed by Lorenz Meier
parent df54f938ef
commit 031bbb7f3e
2 changed files with 49 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/FlightModeManager.cpp
+48 -45
View File
@@ -141,6 +141,10 @@ void FlightModeManager::Run()
start_flight_task();
if (_vehicle_command_sub.updated()) {
handleCommand();
}
if (isAnyTaskActive()) {
generateTrajectorySetpoint(dt, vehicle_local_position);
}
@@ -433,53 +437,52 @@ void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state)
_vehicle_command_pub.publish(command);
}
void FlightModeManager::handleCommand()
{
// get command
vehicle_command_s command{};
_vehicle_command_sub.copy(&command);
// check what command it is
FlightTaskIndex desired_task = switchVehicleCommand(command.command);
// ignore all unkown commands
if (desired_task != FlightTaskIndex::None) {
// switch to the commanded task
FlightTaskError switch_result = switchTask(desired_task);
uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
// if we are in/switched to the desired task
if (switch_result >= FlightTaskError::NoError) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
// if the task is running apply parameters to it and see if it rejects
if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
// if we just switched and parameters are not accepted, go to failsafe
if (switch_result >= FlightTaskError::NoError) {
switchTask(FlightTaskIndex::ManualPosition);
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
}
}
}
// send back acknowledgment
vehicle_command_ack_s command_ack{};
command_ack.command = command.command;
command_ack.result = cmd_result;
command_ack.result_param1 = static_cast<int>(switch_result);
command_ack.target_system = command.source_system;
command_ack.target_component = command.source_component;
command_ack.timestamp = hrt_absolute_time();
_vehicle_command_ack_pub.publish(command_ack);
}
}
void FlightModeManager::generateTrajectorySetpoint(const float dt,
const vehicle_local_position_s &vehicle_local_position)
{
// In case flight task was not able to update correctly we send the empty setpoint which makes the position controller failsafe.
if (_vehicle_command_sub.updated()) {
// get command
vehicle_command_s command{};
_vehicle_command_sub.copy(&command);
// check what command it is
FlightTaskIndex desired_task = switchVehicleCommand(command.command);
// ignore all unkown commands
if (desired_task != FlightTaskIndex::None) {
// switch to the commanded task
FlightTaskError switch_result = switchTask(desired_task);
uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
// if we are in/switched to the desired task
if (switch_result >= FlightTaskError::NoError) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
// if the task is running apply parameters to it and see if it rejects
if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
// if we just switched and parameters are not accepted, go to failsafe
if (switch_result >= FlightTaskError::NoError) {
switchTask(FlightTaskIndex::ManualPosition);
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
}
}
}
// send back acknowledgment
vehicle_command_ack_s command_ack{};
command_ack.command = command.command;
command_ack.result = cmd_result;
command_ack.result_param1 = static_cast<int>(switch_result);
command_ack.target_system = command.source_system;
command_ack.target_component = command.source_component;
command_ack.timestamp = hrt_absolute_time();
_vehicle_command_ack_pub.publish(command_ack);
}
}
_current_task.task->setYawHandler(_wv_controller);
// Inform FlightTask about the input and output of the velocity controller
@@ -494,7 +497,7 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
}
}
// initialize with empty NAN setpoints, if the task fails they get sent out and controller's emergency failsafe kicks in
// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint;
vehicle_constraints_s constraints = FlightTask::empty_constraints;
src/modules/flight_mode_manager/FlightModeManager.hpp
+1
View File
@@ -90,6 +90,7 @@ private:
void start_flight_task();
void check_failure(bool task_failure, uint8_t nav_state);
void send_vehicle_cmd_do(uint8_t nav_state);
void handleCommand();
void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position);
void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint);