forked from Archive/PX4-Autopilot
FlightTask: add methods to get the desired trajectory waypoints
This commit is contained in:
parent
17a08a9de7
commit
b856c1048d
|
@ -47,6 +47,17 @@ const vehicle_constraints_s FlightTasks::getConstraints()
|
|||
}
|
||||
}
|
||||
|
||||
const vehicle_trajectory_waypoint_s FlightTasks::getAvoidanceWaypoint()
|
||||
{
|
||||
|
||||
if (isAnyTaskActive()) {
|
||||
return _current_task.task->getAvoidanceWaypoint();
|
||||
|
||||
} else {
|
||||
return FlightTask::empty_trajectory_waypoint;
|
||||
}
|
||||
}
|
||||
|
||||
int FlightTasks::switchTask(FlightTaskIndex new_task_index)
|
||||
{
|
||||
// switch to the running task, nothing to do
|
||||
|
|
|
@ -77,6 +77,12 @@ public:
|
|||
*/
|
||||
const vehicle_constraints_s getConstraints();
|
||||
|
||||
/**
|
||||
* Get task avoidance desired waypoints
|
||||
* @return auto triplets in the mc_pos_control
|
||||
*/
|
||||
const vehicle_trajectory_waypoint_s getAvoidanceWaypoint();
|
||||
|
||||
/**
|
||||
* Switch to the next task in the available list (for testing)
|
||||
* @return 1 on success, <0 on error
|
||||
|
|
|
@ -5,7 +5,17 @@
|
|||
constexpr uint64_t FlightTask::_timeout;
|
||||
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
|
||||
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}};
|
||||
|
||||
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, vehicle_constraints_s::GEAR_KEEP, {}};
|
||||
const vehicle_trajectory_waypoint_s FlightTask::empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
|
||||
{ {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false},
|
||||
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false},
|
||||
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false},
|
||||
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false},
|
||||
{0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false}
|
||||
}
|
||||
};
|
||||
|
||||
bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
|
||||
{
|
||||
if (!subscription_array.get(ORB_ID(vehicle_local_position), _sub_vehicle_local_position)) {
|
||||
|
@ -69,6 +79,7 @@ void FlightTask::_resetSetpoints()
|
|||
_acceleration_setpoint *= NAN;
|
||||
_thrust_setpoint *= NAN;
|
||||
_yaw_setpoint = _yawspeed_setpoint = NAN;
|
||||
_desired_waypoint = FlightTask::empty_trajectory_waypoint;
|
||||
}
|
||||
|
||||
bool FlightTask::_evaluateVehicleLocalPosition()
|
||||
|
|
|
@ -50,6 +50,7 @@
|
|||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_constraints.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_trajectory_waypoint.h>
|
||||
#include "SubscriptionArray.hpp"
|
||||
|
||||
class FlightTask : public ModuleParams
|
||||
|
@ -107,6 +108,8 @@ public:
|
|||
*/
|
||||
const vehicle_constraints_s getConstraints() {return _constraints;};
|
||||
|
||||
const vehicle_trajectory_waypoint_s getAvoidanceWaypoint() {return _desired_waypoint;};
|
||||
|
||||
/**
|
||||
* Empty setpoint.
|
||||
* All setpoints are set to NAN.
|
||||
|
@ -119,6 +122,12 @@ public:
|
|||
*/
|
||||
static const vehicle_constraints_s empty_constraints;
|
||||
|
||||
/**
|
||||
* Empty desired waypoints.
|
||||
* All waypoints are set to NAN.
|
||||
*/
|
||||
static const vehicle_trajectory_waypoint_s empty_trajectory_waypoint;
|
||||
|
||||
/**
|
||||
* Call this whenever a parameter update notification is received (parameter_update uORB message)
|
||||
*/
|
||||
|
@ -183,6 +192,12 @@ protected:
|
|||
*/
|
||||
vehicle_constraints_s _constraints;
|
||||
|
||||
/**
|
||||
* Desired waypoints.
|
||||
* Goals set by the FCU to be sent to the obstacle avoidance system.
|
||||
*/
|
||||
vehicle_trajectory_waypoint_s _desired_waypoint;
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
|
||||
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) MPC_XY_VEL_MAX,
|
||||
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) MPC_Z_VEL_MAX_DN,
|
||||
|
|
Loading…
Reference in New Issue