forked from Archive/PX4-Autopilot
Replace mission_item_triplet with position_setpoint_triplet, WIP
This commit is contained in:
parent
6c07a5c2cf
commit
6acb8fa66f
|
@ -54,8 +54,8 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {};
|
||||||
|
|
||||||
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
|
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
|
||||||
vehicle_attitude_s &att,
|
vehicle_attitude_s &att,
|
||||||
mission_item_s &missionCmd,
|
position_setpoint_s &missionCmd,
|
||||||
mission_item_s &lastMissionCmd)
|
position_setpoint_s &lastMissionCmd)
|
||||||
{
|
{
|
||||||
|
|
||||||
// heading to waypoint
|
// heading to waypoint
|
||||||
|
@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
|
||||||
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
|
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
|
||||||
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
|
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
|
||||||
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
|
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
|
||||||
_missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20),
|
_missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20),
|
||||||
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
|
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
|
||||||
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
|
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
|
||||||
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
|
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
|
||||||
|
|
|
@ -43,7 +43,7 @@
|
||||||
#include <uORB/topics/vehicle_attitude.h>
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||||
#include <uORB/topics/vehicle_global_position.h>
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
#include <uORB/topics/mission_item_triplet.h>
|
#include <uORB/topics/position_setpoint_triplet.h>
|
||||||
#include <uORB/topics/manual_control_setpoint.h>
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
|
@ -82,8 +82,8 @@ public:
|
||||||
virtual ~BlockWaypointGuidance();
|
virtual ~BlockWaypointGuidance();
|
||||||
void update(vehicle_global_position_s &pos,
|
void update(vehicle_global_position_s &pos,
|
||||||
vehicle_attitude_s &att,
|
vehicle_attitude_s &att,
|
||||||
mission_item_s &missionCmd,
|
position_setpoint_s &missionCmd,
|
||||||
mission_item_s &lastMissionCmd);
|
position_setpoint_s &lastMissionCmd);
|
||||||
float getPsiCmd() { return _psiCmd; }
|
float getPsiCmd() { return _psiCmd; }
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -98,7 +98,7 @@ protected:
|
||||||
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
|
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
|
||||||
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
|
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
|
||||||
UOrbSubscription<vehicle_global_position_s> _pos;
|
UOrbSubscription<vehicle_global_position_s> _pos;
|
||||||
UOrbSubscription<mission_item_triplet_s> _missionCmd;
|
UOrbSubscription<position_setpoint_triplet_s> _missionCmd;
|
||||||
UOrbSubscription<manual_control_setpoint_s> _manual;
|
UOrbSubscription<manual_control_setpoint_s> _manual;
|
||||||
UOrbSubscription<vehicle_status_s> _status;
|
UOrbSubscription<vehicle_status_s> _status;
|
||||||
UOrbSubscription<parameter_update_s> _param_update;
|
UOrbSubscription<parameter_update_s> _param_update;
|
||||||
|
|
|
@ -264,7 +264,7 @@ private:
|
||||||
BlockParamFloat _crMax;
|
BlockParamFloat _crMax;
|
||||||
|
|
||||||
struct pollfd _attPoll;
|
struct pollfd _attPoll;
|
||||||
mission_item_triplet_s _lastMissionCmd;
|
position_setpoint_triplet_s _lastMissionCmd;
|
||||||
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
|
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
|
||||||
uint64_t _timeStamp;
|
uint64_t _timeStamp;
|
||||||
public:
|
public:
|
||||||
|
|
|
@ -68,7 +68,7 @@
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/airspeed.h>
|
#include <uORB/topics/airspeed.h>
|
||||||
#include <uORB/topics/vehicle_global_position.h>
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
#include <uORB/topics/mission_item_triplet.h>
|
#include <uORB/topics/position_setpoint_triplet.h>
|
||||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||||
#include <uORB/topics/manual_control_setpoint.h>
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
#include <uORB/topics/actuator_controls.h>
|
#include <uORB/topics/actuator_controls.h>
|
||||||
|
@ -126,7 +126,7 @@ private:
|
||||||
int _control_task; /**< task handle for sensor task */
|
int _control_task; /**< task handle for sensor task */
|
||||||
|
|
||||||
int _global_pos_sub;
|
int _global_pos_sub;
|
||||||
int _mission_item_triplet_sub;
|
int _pos_sp_triplet_sub;
|
||||||
int _att_sub; /**< vehicle attitude subscription */
|
int _att_sub; /**< vehicle attitude subscription */
|
||||||
int _attitude_sub; /**< raw rc channels data subscription */
|
int _attitude_sub; /**< raw rc channels data subscription */
|
||||||
int _airspeed_sub; /**< airspeed subscription */
|
int _airspeed_sub; /**< airspeed subscription */
|
||||||
|
@ -145,7 +145,7 @@ private:
|
||||||
struct airspeed_s _airspeed; /**< airspeed */
|
struct airspeed_s _airspeed; /**< airspeed */
|
||||||
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
|
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
|
||||||
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
||||||
struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
|
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
|
||||||
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
|
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
|
||||||
|
|
||||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||||
|
@ -332,10 +332,10 @@ private:
|
||||||
* Control position.
|
* Control position.
|
||||||
*/
|
*/
|
||||||
bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
|
bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
|
||||||
const struct mission_item_triplet_s &_mission_item_triplet);
|
const struct position_setpoint_triplet_s &_pos_sp_triplet);
|
||||||
|
|
||||||
float calculate_target_airspeed(float airspeed_demand);
|
float calculate_target_airspeed(float airspeed_demand);
|
||||||
void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet);
|
void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Shim for calling task_main from task_create.
|
* Shim for calling task_main from task_create.
|
||||||
|
@ -367,7 +367,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||||
|
|
||||||
/* subscriptions */
|
/* subscriptions */
|
||||||
_global_pos_sub(-1),
|
_global_pos_sub(-1),
|
||||||
_mission_item_triplet_sub(-1),
|
_pos_sp_triplet_sub(-1),
|
||||||
_att_sub(-1),
|
_att_sub(-1),
|
||||||
_airspeed_sub(-1),
|
_airspeed_sub(-1),
|
||||||
_control_mode_sub(-1),
|
_control_mode_sub(-1),
|
||||||
|
@ -406,7 +406,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||||
airspeed_s _airspeed = {0};
|
airspeed_s _airspeed = {0};
|
||||||
vehicle_control_mode_s _control_mode = {0};
|
vehicle_control_mode_s _control_mode = {0};
|
||||||
vehicle_global_position_s _global_pos = {0};
|
vehicle_global_position_s _global_pos = {0};
|
||||||
mission_item_triplet_s _mission_item_triplet = {0};
|
position_setpoint_triplet_s _pos_sp_triplet = {0};
|
||||||
sensor_combined_s _sensor_combined = {0};
|
sensor_combined_s _sensor_combined = {0};
|
||||||
|
|
||||||
|
|
||||||
|
@ -653,11 +653,11 @@ void
|
||||||
FixedwingPositionControl::vehicle_setpoint_poll()
|
FixedwingPositionControl::vehicle_setpoint_poll()
|
||||||
{
|
{
|
||||||
/* check if there is a new setpoint */
|
/* check if there is a new setpoint */
|
||||||
bool mission_item_triplet_updated;
|
bool pos_sp_triplet_updated;
|
||||||
orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated);
|
orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
|
||||||
|
|
||||||
if (mission_item_triplet_updated) {
|
if (pos_sp_triplet_updated) {
|
||||||
orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet);
|
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
|
||||||
_setpoint_valid = true;
|
_setpoint_valid = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -700,7 +700,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet)
|
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
|
||||||
{
|
{
|
||||||
|
|
||||||
if (_global_pos_valid) {
|
if (_global_pos_valid) {
|
||||||
|
@ -713,12 +713,12 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
|
||||||
/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
|
/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
|
||||||
float distance = 0.0f;
|
float distance = 0.0f;
|
||||||
float delta_altitude = 0.0f;
|
float delta_altitude = 0.0f;
|
||||||
if (mission_item_triplet.previous_valid) {
|
if (pos_sp_triplet.previous.valid) {
|
||||||
distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon);
|
distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
|
||||||
delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude;
|
delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt;
|
||||||
} else {
|
} else {
|
||||||
distance = get_distance_to_next_waypoint(current_position(0), current_position(1), mission_item_triplet.current.lat, mission_item_triplet.current.lon);
|
distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
|
||||||
delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt;
|
delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt;
|
||||||
}
|
}
|
||||||
|
|
||||||
float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
|
float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
|
||||||
|
@ -751,11 +751,11 @@ void FixedwingPositionControl::navigation_capabilities_publish()
|
||||||
|
|
||||||
bool
|
bool
|
||||||
FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed,
|
FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed,
|
||||||
const struct mission_item_triplet_s &mission_item_triplet)
|
const struct position_setpoint_triplet_s &pos_sp_triplet)
|
||||||
{
|
{
|
||||||
bool setpoint = true;
|
bool setpoint = true;
|
||||||
|
|
||||||
calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet);
|
calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet);
|
||||||
|
|
||||||
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
|
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
|
||||||
|
|
||||||
|
@ -767,7 +767,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
math::Vector<3> accel_earth = _R_nb * accel_body;
|
math::Vector<3> accel_earth = _R_nb * accel_body;
|
||||||
|
|
||||||
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
|
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
|
||||||
float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt;
|
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
|
||||||
|
|
||||||
/* no throttle limit as default */
|
/* no throttle limit as default */
|
||||||
float throttle_max = 1.0f;
|
float throttle_max = 1.0f;
|
||||||
|
@ -785,58 +785,56 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
_tecs.set_speed_weight(_parameters.speed_weight);
|
_tecs.set_speed_weight(_parameters.speed_weight);
|
||||||
|
|
||||||
/* current waypoint (the one currently heading for) */
|
/* current waypoint (the one currently heading for) */
|
||||||
math::Vector<2> next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
|
math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
|
||||||
|
|
||||||
/* current waypoint (the one currently heading for) */
|
/* current waypoint (the one currently heading for) */
|
||||||
math::Vector<2> curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
|
math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
|
||||||
|
|
||||||
|
|
||||||
/* previous waypoint */
|
/* previous waypoint */
|
||||||
math::Vector<2> prev_wp;
|
math::Vector<2> prev_wp;
|
||||||
|
|
||||||
if (mission_item_triplet.previous_valid) {
|
if (pos_sp_triplet.previous.valid) {
|
||||||
prev_wp(0) = mission_item_triplet.previous.lat;
|
prev_wp(0) = pos_sp_triplet.previous.lat;
|
||||||
prev_wp(1) = mission_item_triplet.previous.lon;
|
prev_wp(1) = pos_sp_triplet.previous.lon;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/*
|
/*
|
||||||
* No valid previous waypoint, go for the current wp.
|
* No valid previous waypoint, go for the current wp.
|
||||||
* This is automatically handled by the L1 library.
|
* This is automatically handled by the L1 library.
|
||||||
*/
|
*/
|
||||||
prev_wp(0) = mission_item_triplet.current.lat;
|
prev_wp(0) = pos_sp_triplet.current.lat;
|
||||||
prev_wp(1) = mission_item_triplet.current.lon;
|
prev_wp(1) = pos_sp_triplet.current.lon;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
|
if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
|
||||||
/* waypoint is a plain navigation waypoint */
|
/* waypoint is a plain navigation waypoint */
|
||||||
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
|
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
|
||||||
_att_sp.roll_body = _l1_control.nav_roll();
|
_att_sp.roll_body = _l1_control.nav_roll();
|
||||||
_att_sp.yaw_body = _l1_control.nav_bearing();
|
_att_sp.yaw_body = _l1_control.nav_bearing();
|
||||||
|
|
||||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
|
||||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||||
false, math::radians(_parameters.pitch_limit_min),
|
false, math::radians(_parameters.pitch_limit_min),
|
||||||
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
||||||
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
||||||
|
|
||||||
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
|
||||||
mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
|
||||||
mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
|
|
||||||
|
|
||||||
/* waypoint is a loiter waypoint */
|
/* waypoint is a loiter waypoint */
|
||||||
_l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius,
|
_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
|
||||||
mission_item_triplet.current.loiter_direction, ground_speed);
|
pos_sp_triplet.current.loiter_direction, ground_speed);
|
||||||
_att_sp.roll_body = _l1_control.nav_roll();
|
_att_sp.roll_body = _l1_control.nav_roll();
|
||||||
_att_sp.yaw_body = _l1_control.nav_bearing();
|
_att_sp.yaw_body = _l1_control.nav_bearing();
|
||||||
|
|
||||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
||||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||||
false, math::radians(_parameters.pitch_limit_min),
|
false, math::radians(_parameters.pitch_limit_min),
|
||||||
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
||||||
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
||||||
|
|
||||||
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
|
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
|
||||||
|
|
||||||
/* Horizontal landing control */
|
/* Horizontal landing control */
|
||||||
/* switch to heading hold for the last meters, continue heading hold after */
|
/* switch to heading hold for the last meters, continue heading hold after */
|
||||||
|
@ -847,7 +845,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
/* heading hold, along the line connecting this and the last waypoint */
|
/* heading hold, along the line connecting this and the last waypoint */
|
||||||
|
|
||||||
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
|
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
|
||||||
if (mission_item_triplet.previous_valid) {
|
if (pos_sp_triplet.previous_valid) {
|
||||||
target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
|
target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
|
||||||
} else {
|
} else {
|
||||||
target_bearing = _att.yaw;
|
target_bearing = _att.yaw;
|
||||||
|
@ -884,18 +882,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
|
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
|
||||||
// XXX this could make a great param
|
// XXX this could make a great param
|
||||||
|
|
||||||
float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1)
|
float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
|
||||||
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
|
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
|
||||||
float airspeed_land = 1.3f * _parameters.airspeed_min;
|
float airspeed_land = 1.3f * _parameters.airspeed_min;
|
||||||
float airspeed_approach = 1.3f * _parameters.airspeed_min;
|
float airspeed_approach = 1.3f * _parameters.airspeed_min;
|
||||||
|
|
||||||
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
|
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
|
||||||
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
|
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
|
||||||
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
|
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
|
if ( (_global_pos.alt < _pos_sp_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
|
||||||
|
|
||||||
/* land with minimal speed */
|
/* land with minimal speed */
|
||||||
|
|
||||||
|
@ -914,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude);
|
float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.altitude);
|
||||||
|
|
||||||
/* avoid climbout */
|
/* avoid climbout */
|
||||||
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
|
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
|
||||||
{
|
{
|
||||||
flare_curve_alt = mission_item_triplet.current.altitude;
|
flare_curve_alt = pos_sp_triplet.current.altitude;
|
||||||
land_stayonground = true;
|
land_stayonground = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -977,7 +975,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
|
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
|
||||||
|
|
||||||
/* Perform launch detection */
|
/* Perform launch detection */
|
||||||
// warnx("Launch detection running");
|
// warnx("Launch detection running");
|
||||||
|
@ -1011,9 +1009,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
if (altitude_error > 15.0f) {
|
if (altitude_error > 15.0f) {
|
||||||
|
|
||||||
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
|
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
|
||||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
|
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
|
||||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||||
true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)),
|
true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)),
|
||||||
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
||||||
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
||||||
|
|
||||||
|
@ -1022,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
||||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||||
false, math::radians(_parameters.pitch_limit_min),
|
false, math::radians(_parameters.pitch_limit_min),
|
||||||
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
||||||
|
@ -1037,14 +1035,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
|
// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
|
||||||
// (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
|
// (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
|
||||||
// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
|
// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
|
||||||
// (double)next_wp(0), (double)next_wp(1), (mission_item_triplet.previous_valid) ? "valid" : "invalid");
|
// (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
|
||||||
|
|
||||||
// XXX at this point we always want no loiter hold if a
|
// XXX at this point we always want no loiter hold if a
|
||||||
// mission is active
|
// mission is active
|
||||||
_loiter_hold = false;
|
_loiter_hold = false;
|
||||||
|
|
||||||
/* reset land state */
|
/* reset land state */
|
||||||
if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) {
|
if (pos_sp_triplet.current.nav_cmd != NAV_CMD_LAND) {
|
||||||
land_noreturn_horizontal = false;
|
land_noreturn_horizontal = false;
|
||||||
land_noreturn_vertical = false;
|
land_noreturn_vertical = false;
|
||||||
land_stayonground = false;
|
land_stayonground = false;
|
||||||
|
@ -1053,7 +1051,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||||
}
|
}
|
||||||
|
|
||||||
/* reset takeoff/launch state */
|
/* reset takeoff/launch state */
|
||||||
if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
|
if (pos_sp_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
|
||||||
launch_detected = false;
|
launch_detected = false;
|
||||||
usePreTakeoffThrust = false;
|
usePreTakeoffThrust = false;
|
||||||
}
|
}
|
||||||
|
@ -1176,7 +1174,7 @@ FixedwingPositionControl::task_main()
|
||||||
* do subscriptions
|
* do subscriptions
|
||||||
*/
|
*/
|
||||||
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||||
_mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
|
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||||
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||||
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
|
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||||
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||||
|
@ -1271,7 +1269,7 @@ FixedwingPositionControl::task_main()
|
||||||
* Attempt to control position, on success (= sensors present and not in manual mode),
|
* Attempt to control position, on success (= sensors present and not in manual mode),
|
||||||
* publish setpoint.
|
* publish setpoint.
|
||||||
*/
|
*/
|
||||||
if (control_position(current_position, ground_speed, _mission_item_triplet)) {
|
if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
|
||||||
_att_sp.timestamp = hrt_absolute_time();
|
_att_sp.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
/* lazily publish the setpoint only once available */
|
/* lazily publish the setpoint only once available */
|
||||||
|
@ -1285,7 +1283,7 @@ FixedwingPositionControl::task_main()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* XXX check if radius makes sense here */
|
/* XXX check if radius makes sense here */
|
||||||
float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius);
|
float turn_distance = _l1_control.switch_distance(_pos_sp_triplet.current.acceptance_radius);
|
||||||
|
|
||||||
/* lazily publish navigation capabilities */
|
/* lazily publish navigation capabilities */
|
||||||
if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
|
if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
|
||||||
|
|
|
@ -410,7 +410,7 @@ l_local_position(const struct listener *l)
|
||||||
void
|
void
|
||||||
l_global_position_setpoint(const struct listener *l)
|
l_global_position_setpoint(const struct listener *l)
|
||||||
{
|
{
|
||||||
struct mission_item_triplet_s triplet;
|
struct position_setpoint_triplet_s triplet;
|
||||||
orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
|
orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
|
||||||
|
|
||||||
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
|
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
|
||||||
|
|
|
@ -117,7 +117,7 @@ private:
|
||||||
int _manual_sub; /**< notification of manual control updates */
|
int _manual_sub; /**< notification of manual control updates */
|
||||||
int _arming_sub; /**< arming status of outputs */
|
int _arming_sub; /**< arming status of outputs */
|
||||||
int _local_pos_sub; /**< vehicle local position */
|
int _local_pos_sub; /**< vehicle local position */
|
||||||
int _mission_items_sub; /**< mission item triplet */
|
int _pos_sp_triplet_sub; /**< mission item triplet */
|
||||||
|
|
||||||
orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */
|
orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */
|
||||||
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
|
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
|
||||||
|
@ -130,7 +130,7 @@ private:
|
||||||
struct actuator_armed_s _arming; /**< actuator arming status */
|
struct actuator_armed_s _arming; /**< actuator arming status */
|
||||||
struct vehicle_local_position_s _local_pos; /**< vehicle local position */
|
struct vehicle_local_position_s _local_pos; /**< vehicle local position */
|
||||||
struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */
|
struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */
|
||||||
struct mission_item_triplet_s _mission_items; /**< vehicle global position setpoint */
|
struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */
|
||||||
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
|
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
|
@ -236,7 +236,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
||||||
_manual_sub(-1),
|
_manual_sub(-1),
|
||||||
_arming_sub(-1),
|
_arming_sub(-1),
|
||||||
_local_pos_sub(-1),
|
_local_pos_sub(-1),
|
||||||
_mission_items_sub(-1),
|
_pos_sp_triplet_sub(-1),
|
||||||
|
|
||||||
/* publications */
|
/* publications */
|
||||||
_local_pos_sp_pub(-1),
|
_local_pos_sp_pub(-1),
|
||||||
|
@ -250,7 +250,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
||||||
memset(&_arming, 0, sizeof(_arming));
|
memset(&_arming, 0, sizeof(_arming));
|
||||||
memset(&_local_pos, 0, sizeof(_local_pos));
|
memset(&_local_pos, 0, sizeof(_local_pos));
|
||||||
memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
|
memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
|
||||||
memset(&_mission_items, 0, sizeof(_mission_items));
|
memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
|
||||||
memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
|
memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
|
||||||
|
|
||||||
_params.pos_p.zero();
|
_params.pos_p.zero();
|
||||||
|
@ -405,10 +405,10 @@ MulticopterPositionControl::poll_subscriptions()
|
||||||
if (updated)
|
if (updated)
|
||||||
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
|
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
|
||||||
|
|
||||||
orb_check(_mission_items_sub, &updated);
|
orb_check(_pos_sp_triplet_sub, &updated);
|
||||||
|
|
||||||
if (updated)
|
if (updated)
|
||||||
orb_copy(ORB_ID(mission_item_triplet), _mission_items_sub, &_mission_items);
|
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
|
||||||
}
|
}
|
||||||
|
|
||||||
float
|
float
|
||||||
|
@ -450,7 +450,7 @@ MulticopterPositionControl::task_main()
|
||||||
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||||
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
|
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||||
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
|
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||||
_mission_items_sub = orb_subscribe(ORB_ID(mission_item_triplet));
|
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||||
|
|
||||||
parameters_update(true);
|
parameters_update(true);
|
||||||
|
|
||||||
|
@ -626,16 +626,15 @@ MulticopterPositionControl::task_main()
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* AUTO */
|
/* AUTO */
|
||||||
if (_mission_items.current_valid) {
|
if (_pos_sp_triplet.current.valid) {
|
||||||
struct mission_item_s item = _mission_items.current;
|
struct position_setpoint_s current_sp = _pos_sp_triplet.current;
|
||||||
|
|
||||||
// TODO home altitude can be != ref_alt, check home_position topic
|
_pos_sp(2) = -(current_sp.alt - ref_alt);
|
||||||
_pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt);
|
|
||||||
|
|
||||||
map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1));
|
map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1));
|
||||||
|
|
||||||
if (isfinite(_mission_items.current.yaw)) {
|
if (isfinite(current_sp.yaw)) {
|
||||||
_att_sp.yaw_body = _mission_items.current.yaw;
|
_att_sp.yaw_body = current_sp.yaw;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* in case of interrupted mission don't go to waypoint but stay at current position */
|
/* in case of interrupted mission don't go to waypoint but stay at current position */
|
||||||
|
@ -688,7 +687,7 @@ MulticopterPositionControl::task_main()
|
||||||
|
|
||||||
if (!_control_mode.flag_control_manual_enabled) {
|
if (!_control_mode.flag_control_manual_enabled) {
|
||||||
/* use constant descend rate when landing, ignore altitude setpoint */
|
/* use constant descend rate when landing, ignore altitude setpoint */
|
||||||
if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) {
|
if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) {
|
||||||
_vel_sp(2) = _params.land_speed;
|
_vel_sp(2) = _params.land_speed;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -790,7 +789,7 @@ MulticopterPositionControl::task_main()
|
||||||
float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2));
|
float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2));
|
||||||
float tilt_max = _params.tilt_max;
|
float tilt_max = _params.tilt_max;
|
||||||
if (!_control_mode.flag_control_manual_enabled) {
|
if (!_control_mode.flag_control_manual_enabled) {
|
||||||
if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) {
|
if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) {
|
||||||
/* limit max tilt and min lift when landing */
|
/* limit max tilt and min lift when landing */
|
||||||
tilt_max = _params.land_tilt_max;
|
tilt_max = _params.land_tilt_max;
|
||||||
if (thr_min < 0.0f)
|
if (thr_min < 0.0f)
|
||||||
|
|
|
@ -64,7 +64,7 @@
|
||||||
#include <uORB/uORB.h>
|
#include <uORB/uORB.h>
|
||||||
#include <uORB/topics/vehicle_global_position.h>
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
#include <uORB/topics/home_position.h>
|
#include <uORB/topics/home_position.h>
|
||||||
#include <uORB/topics/mission_item_triplet.h>
|
#include <uORB/topics/position_setpoint_triplet.h>
|
||||||
#include <uORB/topics/mission_result.h>
|
#include <uORB/topics/mission_result.h>
|
||||||
#include <uORB/topics/vehicle_status.h>
|
#include <uORB/topics/vehicle_status.h>
|
||||||
#include <uORB/topics/vehicle_control_mode.h>
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
|
@ -152,7 +152,7 @@ private:
|
||||||
int _onboard_mission_sub; /**< notification of onboard mission updates */
|
int _onboard_mission_sub; /**< notification of onboard mission updates */
|
||||||
int _capabilities_sub; /**< notification of vehicle capabilities updates */
|
int _capabilities_sub; /**< notification of vehicle capabilities updates */
|
||||||
|
|
||||||
orb_advert_t _triplet_pub; /**< publish position setpoint triplet */
|
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
|
||||||
orb_advert_t _mission_result_pub; /**< publish mission result topic */
|
orb_advert_t _mission_result_pub; /**< publish mission result topic */
|
||||||
orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */
|
orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */
|
||||||
|
|
||||||
|
@ -160,8 +160,10 @@ private:
|
||||||
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
|
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
|
||||||
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
||||||
struct home_position_s _home_pos; /**< home position for RTL */
|
struct home_position_s _home_pos; /**< home position for RTL */
|
||||||
struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
|
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
|
||||||
struct mission_result_s _mission_result; /**< mission result for commander/mavlink */
|
struct mission_result_s _mission_result; /**< mission result for commander/mavlink */
|
||||||
|
struct mission_item_s _mission_item; /**< current mission item */
|
||||||
|
bool _mission_item_valid; /**< current mission item valid */
|
||||||
|
|
||||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||||
|
|
||||||
|
@ -323,35 +325,24 @@ private:
|
||||||
void set_rtl_item();
|
void set_rtl_item();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Helper function to get a loiter item
|
* Set position setpoint for mission item
|
||||||
*/
|
*/
|
||||||
void get_loiter_item(mission_item_s *item);
|
void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Helper function to get a takeoff item
|
* Helper function to get a takeoff item
|
||||||
*/
|
*/
|
||||||
void get_takeoff_item(mission_item_s *item);
|
void get_takeoff_setpoint(position_setpoint_s *pos_sp);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Publish a new mission item triplet for position controller
|
* Publish a new mission item triplet for position controller
|
||||||
*/
|
*/
|
||||||
void publish_mission_item_triplet();
|
void publish_position_setpoint_triplet();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Publish vehicle_control_mode topic for controllers
|
* Publish vehicle_control_mode topic for controllers
|
||||||
*/
|
*/
|
||||||
void publish_control_mode();
|
void publish_control_mode();
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Compare two mission items if they are equivalent
|
|
||||||
* Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ.
|
|
||||||
*
|
|
||||||
* @return true if equivalent, false otherwise
|
|
||||||
*/
|
|
||||||
bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b);
|
|
||||||
|
|
||||||
void add_home_pos_to_rtl(struct mission_item_s *new_mission_item);
|
|
||||||
};
|
};
|
||||||
|
|
||||||
namespace navigator
|
namespace navigator
|
||||||
|
@ -385,7 +376,7 @@ Navigator::Navigator() :
|
||||||
_capabilities_sub(-1),
|
_capabilities_sub(-1),
|
||||||
|
|
||||||
/* publications */
|
/* publications */
|
||||||
_triplet_pub(-1),
|
_pos_sp_triplet_pub(-1),
|
||||||
_mission_result_pub(-1),
|
_mission_result_pub(-1),
|
||||||
_control_mode_pub(-1),
|
_control_mode_pub(-1),
|
||||||
|
|
||||||
|
@ -402,6 +393,7 @@ Navigator::Navigator() :
|
||||||
_waypoint_yaw_reached(false),
|
_waypoint_yaw_reached(false),
|
||||||
_time_first_inside_orbit(0),
|
_time_first_inside_orbit(0),
|
||||||
_set_nav_state_timestamp(0),
|
_set_nav_state_timestamp(0),
|
||||||
|
_mission_item_valid(false),
|
||||||
_need_takeoff(true),
|
_need_takeoff(true),
|
||||||
_do_takeoff(false),
|
_do_takeoff(false),
|
||||||
_geofence_violation_warning_sent(false)
|
_geofence_violation_warning_sent(false)
|
||||||
|
@ -414,14 +406,9 @@ Navigator::Navigator() :
|
||||||
_parameter_handles.land_alt = param_find("NAV_LAND_ALT");
|
_parameter_handles.land_alt = param_find("NAV_LAND_ALT");
|
||||||
_parameter_handles.rtl_alt = param_find("NAV_RTL_ALT");
|
_parameter_handles.rtl_alt = param_find("NAV_RTL_ALT");
|
||||||
|
|
||||||
_mission_item_triplet.previous_valid = false;
|
memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s));
|
||||||
_mission_item_triplet.current_valid = false;
|
|
||||||
_mission_item_triplet.next_valid = false;
|
|
||||||
memset(&_mission_item_triplet.previous, 0, sizeof(struct mission_item_s));
|
|
||||||
memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s));
|
|
||||||
memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s));
|
|
||||||
|
|
||||||
memset(&_mission_result, 0, sizeof(struct mission_result_s));
|
memset(&_mission_result, 0, sizeof(struct mission_result_s));
|
||||||
|
memset(&_mission_item, 0, sizeof(struct mission_item_s));
|
||||||
|
|
||||||
nav_states_str[0] = "NONE";
|
nav_states_str[0] = "NONE";
|
||||||
nav_states_str[1] = "READY";
|
nav_states_str[1] = "READY";
|
||||||
|
@ -482,7 +469,6 @@ Navigator::parameters_update()
|
||||||
void
|
void
|
||||||
Navigator::global_position_update()
|
Navigator::global_position_update()
|
||||||
{
|
{
|
||||||
/* load local copies */
|
|
||||||
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
|
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -938,23 +924,25 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
|
||||||
void
|
void
|
||||||
Navigator::start_none()
|
Navigator::start_none()
|
||||||
{
|
{
|
||||||
_mission_item_triplet.previous_valid = false;
|
_pos_sp_triplet.previous.valid = false;
|
||||||
_mission_item_triplet.current_valid = false;
|
_pos_sp_triplet.current.valid = false;
|
||||||
_mission_item_triplet.next_valid = false;
|
_pos_sp_triplet.next.valid = false;
|
||||||
|
_mission_item_valid = false;
|
||||||
|
|
||||||
_reset_loiter_pos = true;
|
_reset_loiter_pos = true;
|
||||||
_do_takeoff = false;
|
_do_takeoff = false;
|
||||||
_rtl_state = RTL_STATE_NONE;
|
_rtl_state = RTL_STATE_NONE;
|
||||||
|
|
||||||
publish_mission_item_triplet();
|
publish_position_setpoint_triplet();
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
Navigator::start_ready()
|
Navigator::start_ready()
|
||||||
{
|
{
|
||||||
_mission_item_triplet.previous_valid = false;
|
_pos_sp_triplet.previous.valid = false;
|
||||||
_mission_item_triplet.current_valid = false;
|
_pos_sp_triplet.current.valid = false;
|
||||||
_mission_item_triplet.next_valid = false;
|
_pos_sp_triplet.next.valid = false;
|
||||||
|
_mission_item_valid = false;
|
||||||
|
|
||||||
_reset_loiter_pos = true;
|
_reset_loiter_pos = true;
|
||||||
_do_takeoff = false;
|
_do_takeoff = false;
|
||||||
|
@ -964,7 +952,7 @@ Navigator::start_ready()
|
||||||
_rtl_state = RTL_STATE_NONE;
|
_rtl_state = RTL_STATE_NONE;
|
||||||
}
|
}
|
||||||
|
|
||||||
publish_mission_item_triplet();
|
publish_position_setpoint_triplet();
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -973,38 +961,38 @@ Navigator::start_loiter()
|
||||||
_do_takeoff = false;
|
_do_takeoff = false;
|
||||||
|
|
||||||
/* set loiter position if needed */
|
/* set loiter position if needed */
|
||||||
if (_reset_loiter_pos || !_mission_item_triplet.current_valid) {
|
if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) {
|
||||||
_reset_loiter_pos = false;
|
_reset_loiter_pos = false;
|
||||||
|
|
||||||
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
|
_pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d;
|
||||||
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
|
_pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d;
|
||||||
_mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw
|
_pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw
|
||||||
|
|
||||||
_mission_item_triplet.current.altitude_is_relative = false;
|
|
||||||
float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude;
|
float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude;
|
||||||
|
|
||||||
/* use current altitude if above min altitude set by parameter */
|
/* use current altitude if above min altitude set by parameter */
|
||||||
if (_global_pos.alt < min_alt_amsl) {
|
if (_global_pos.alt < min_alt_amsl) {
|
||||||
_mission_item_triplet.current.altitude = min_alt_amsl;
|
_pos_sp_triplet.current.altitude = min_alt_amsl;
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
|
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
|
||||||
} else {
|
} else {
|
||||||
_mission_item_triplet.current.altitude = _global_pos.alt;
|
_pos_sp_triplet.current.altitude = _global_pos.alt;
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
|
mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
|
||||||
|
|
||||||
if (_rtl_state == RTL_STATE_LAND) {
|
if (_rtl_state == RTL_STATE_LAND) {
|
||||||
/* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */
|
/* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */
|
||||||
_rtl_state = RTL_STATE_DESCEND;
|
_rtl_state = RTL_STATE_DESCEND;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
_mission_item_triplet.previous_valid = false;
|
_pos_sp_triplet.previous.valid = false;
|
||||||
_mission_item_triplet.current_valid = true;
|
_pos_sp_triplet.current.valid = true;
|
||||||
_mission_item_triplet.next_valid = false;
|
_pos_sp_triplet.next.valid = false;
|
||||||
|
_mission_item_valid = false;
|
||||||
|
|
||||||
get_loiter_item(&_mission_item_triplet.current);
|
publish_position_setpoint_triplet();
|
||||||
|
|
||||||
publish_mission_item_triplet();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -1020,8 +1008,7 @@ void
|
||||||
Navigator::set_mission_item()
|
Navigator::set_mission_item()
|
||||||
{
|
{
|
||||||
/* copy current mission to previous item */
|
/* copy current mission to previous item */
|
||||||
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
|
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
|
||||||
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
|
|
||||||
|
|
||||||
_reset_loiter_pos = true;
|
_reset_loiter_pos = true;
|
||||||
_do_takeoff = false;
|
_do_takeoff = false;
|
||||||
|
@ -1030,36 +1017,34 @@ Navigator::set_mission_item()
|
||||||
bool onboard;
|
bool onboard;
|
||||||
unsigned index;
|
unsigned index;
|
||||||
|
|
||||||
ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index);
|
ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
|
||||||
|
|
||||||
if (ret == OK) {
|
if (ret == OK) {
|
||||||
_mission_item_triplet.current_valid = true;
|
_mission_item_valid = true;
|
||||||
add_home_pos_to_rtl(&_mission_item_triplet.current);
|
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
|
||||||
|
|
||||||
if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
|
if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
|
||||||
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
|
_mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
|
||||||
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
|
_mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
|
||||||
_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
|
_mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
|
||||||
/* don't reset RTL state on RTL or LOITER items */
|
/* don't reset RTL state on RTL or LOITER items */
|
||||||
_rtl_state = RTL_STATE_NONE;
|
_rtl_state = RTL_STATE_NONE;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_vstatus.is_rotary_wing) {
|
if (_vstatus.is_rotary_wing) {
|
||||||
if (_need_takeoff && (
|
if (_need_takeoff && (
|
||||||
_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF ||
|
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
|
||||||
_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT ||
|
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
|
||||||
_mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
|
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
|
||||||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
||||||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
||||||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED
|
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED
|
||||||
)) {
|
)) {
|
||||||
/* do special TAKEOFF handling for VTOL */
|
/* do special TAKEOFF handling for VTOL */
|
||||||
_need_takeoff = false;
|
_need_takeoff = false;
|
||||||
|
|
||||||
/* calculate desired takeoff altitude AMSL */
|
/* calculate desired takeoff altitude AMSL */
|
||||||
float takeoff_alt_amsl = _mission_item_triplet.current.altitude;
|
float takeoff_alt_amsl = _pos_sp_triplet.current.alt;
|
||||||
if (_mission_item_triplet.current.altitude_is_relative)
|
|
||||||
takeoff_alt_amsl += _home_pos.altitude;
|
|
||||||
|
|
||||||
if (_vstatus.condition_landed) {
|
if (_vstatus.condition_landed) {
|
||||||
/* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */
|
/* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */
|
||||||
|
@ -1067,30 +1052,29 @@ Navigator::set_mission_item()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* check if we really need takeoff */
|
/* check if we really need takeoff */
|
||||||
if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item_triplet.current.acceptance_radius) {
|
if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - item.acceptance_radius) {
|
||||||
/* force TAKEOFF if landed or waypoint altitude is more than current */
|
/* force TAKEOFF if landed or waypoint altitude is more than current */
|
||||||
_do_takeoff = true;
|
_do_takeoff = true;
|
||||||
|
|
||||||
/* move current mission item to next */
|
/* move current position setpoint to next */
|
||||||
memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s));
|
memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
|
||||||
_mission_item_triplet.next_valid = true;
|
|
||||||
|
|
||||||
/* set current item to TAKEOFF */
|
/* set current setpoint to takeoff */
|
||||||
get_takeoff_item(&_mission_item_triplet.current);
|
|
||||||
|
|
||||||
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
|
_pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d;
|
||||||
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
|
_pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d;
|
||||||
_mission_item_triplet.current.altitude = takeoff_alt_amsl;
|
_pos_sp_triplet.current.alt = takeoff_alt_amsl;
|
||||||
_mission_item_triplet.current.altitude_is_relative = false;
|
_pos_sp_triplet.current.yaw = NAN;
|
||||||
|
_pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF;
|
||||||
}
|
}
|
||||||
} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
|
} else if (item.nav_cmd == NAV_CMD_LAND) {
|
||||||
/* will need takeoff after landing */
|
/* will need takeoff after landing */
|
||||||
_need_takeoff = true;
|
_need_takeoff = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_do_takeoff) {
|
if (_do_takeoff) {
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude);
|
mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.altitude);
|
||||||
} else {
|
} else {
|
||||||
if (onboard) {
|
if (onboard) {
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
|
mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
|
||||||
|
@ -1100,23 +1084,24 @@ Navigator::set_mission_item()
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
/* since a mission is not advanced without WPs available, this is not supposed to happen */
|
/* since a mission is not advanced without WPs available, this is not supposed to happen */
|
||||||
_mission_item_triplet.current_valid = false;
|
_mission_item_valid = false;
|
||||||
|
_pos_sp_triplet.current.valid = false;
|
||||||
warnx("ERROR: current WP can't be set");
|
warnx("ERROR: current WP can't be set");
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!_do_takeoff) {
|
if (!_do_takeoff) {
|
||||||
ret = _mission.get_next_mission_item(&_mission_item_triplet.next);
|
mission_item_s item_next;
|
||||||
|
ret = _mission.get_next_mission_item(&item_next);
|
||||||
|
|
||||||
if (ret == OK) {
|
if (ret == OK) {
|
||||||
add_home_pos_to_rtl(&_mission_item_triplet.next);
|
position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next);
|
||||||
_mission_item_triplet.next_valid = true;
|
|
||||||
} else {
|
} else {
|
||||||
/* this will fail for the last WP */
|
/* this will fail for the last WP */
|
||||||
_mission_item_triplet.next_valid = false;
|
_pos_sp_triplet.next.valid = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
publish_mission_item_triplet();
|
publish_position_setpoint_triplet();
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -1129,8 +1114,8 @@ Navigator::start_rtl()
|
||||||
} else {
|
} else {
|
||||||
_rtl_state = RTL_STATE_RETURN;
|
_rtl_state = RTL_STATE_RETURN;
|
||||||
if (_reset_loiter_pos) {
|
if (_reset_loiter_pos) {
|
||||||
_mission_item_triplet.current.altitude_is_relative = false;
|
_mission_item.altitude_is_relative = false;
|
||||||
_mission_item_triplet.current.altitude = _global_pos.alt;
|
_mission_item.altitude = _global_pos.alt;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1144,99 +1129,110 @@ Navigator::set_rtl_item()
|
||||||
{
|
{
|
||||||
switch (_rtl_state) {
|
switch (_rtl_state) {
|
||||||
case RTL_STATE_CLIMB: {
|
case RTL_STATE_CLIMB: {
|
||||||
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
|
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
|
||||||
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
|
|
||||||
|
|
||||||
_mission_item_triplet.current_valid = true;
|
|
||||||
_mission_item_triplet.next_valid = false;
|
|
||||||
|
|
||||||
float climb_alt = _home_pos.altitude + _parameters.rtl_alt;
|
float climb_alt = _home_pos.altitude + _parameters.rtl_alt;
|
||||||
if (_vstatus.condition_landed)
|
if (_vstatus.condition_landed) {
|
||||||
climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
|
climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
|
||||||
|
}
|
||||||
|
|
||||||
|
_mission_item_valid = true;
|
||||||
|
|
||||||
|
_mission_item.lat = (double)_global_pos.lat / 1e7d;
|
||||||
|
_mission_item.lon = (double)_global_pos.lon / 1e7d;
|
||||||
|
_mission_item.altitude_is_relative = false;
|
||||||
|
_mission_item.altitude = climb_alt;
|
||||||
|
_mission_item.yaw = NAN;
|
||||||
|
_mission_item.loiter_radius = _parameters.loiter_radius;
|
||||||
|
_mission_item.loiter_direction = 1;
|
||||||
|
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
|
||||||
|
_mission_item.acceptance_radius = _parameters.acceptance_radius;
|
||||||
|
_mission_item.time_inside = 0.0f;
|
||||||
|
_mission_item.pitch_min = 0.0f;
|
||||||
|
_mission_item.autocontinue = true;
|
||||||
|
_mission_item.origin = ORIGIN_ONBOARD;
|
||||||
|
|
||||||
|
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
|
||||||
|
|
||||||
|
_pos_sp_triplet.next.valid = false;
|
||||||
|
|
||||||
_mission_item_triplet.current.altitude_is_relative = false;
|
|
||||||
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
|
|
||||||
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
|
|
||||||
_mission_item_triplet.current.altitude = climb_alt;
|
|
||||||
_mission_item_triplet.current.yaw = NAN;
|
|
||||||
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
|
|
||||||
_mission_item_triplet.current.loiter_direction = 1;
|
|
||||||
_mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF;
|
|
||||||
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
|
|
||||||
_mission_item_triplet.current.time_inside = 0.0f;
|
|
||||||
_mission_item_triplet.current.pitch_min = 0.0f;
|
|
||||||
_mission_item_triplet.current.autocontinue = true;
|
|
||||||
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
|
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude);
|
mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case RTL_STATE_RETURN: {
|
case RTL_STATE_RETURN: {
|
||||||
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
|
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
|
||||||
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
|
|
||||||
|
|
||||||
_mission_item_triplet.current_valid = true;
|
_mission_item_valid = true;
|
||||||
_mission_item_triplet.next_valid = false;
|
|
||||||
|
_mission_item.lat = _home_pos.lat;
|
||||||
|
_mission_item.lon = _home_pos.lon;
|
||||||
|
// don't change altitude
|
||||||
|
_mission_item.yaw = NAN; // TODO set heading to home
|
||||||
|
_mission_item.loiter_radius = _parameters.loiter_radius;
|
||||||
|
_mission_item.loiter_direction = 1;
|
||||||
|
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||||
|
_mission_item.acceptance_radius = _parameters.acceptance_radius;
|
||||||
|
_mission_item.time_inside = 0.0f;
|
||||||
|
_mission_item.pitch_min = 0.0f;
|
||||||
|
_mission_item.autocontinue = true;
|
||||||
|
_mission_item.origin = ORIGIN_ONBOARD;
|
||||||
|
|
||||||
|
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
|
||||||
|
|
||||||
|
_pos_sp_triplet.next.valid = false;
|
||||||
|
|
||||||
_mission_item_triplet.current.lat = _home_pos.lat;
|
|
||||||
_mission_item_triplet.current.lon = _home_pos.lon;
|
|
||||||
// don't change altitude setpoint
|
|
||||||
_mission_item_triplet.current.yaw = NAN;
|
|
||||||
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
|
|
||||||
_mission_item_triplet.current.loiter_direction = 1;
|
|
||||||
_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
|
|
||||||
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
|
|
||||||
_mission_item_triplet.current.time_inside = 0.0f;
|
|
||||||
_mission_item_triplet.current.pitch_min = 0.0f;
|
|
||||||
_mission_item_triplet.current.autocontinue = true;
|
|
||||||
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
|
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
|
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case RTL_STATE_DESCEND: {
|
case RTL_STATE_DESCEND: {
|
||||||
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
|
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
|
||||||
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
|
|
||||||
|
|
||||||
_mission_item_triplet.current_valid = true;
|
_mission_item_valid = true;
|
||||||
_mission_item_triplet.next_valid = false;
|
|
||||||
|
|
||||||
float descend_alt = _home_pos.altitude + _parameters.land_alt;
|
_mission_item.lat = _home_pos.lat;
|
||||||
|
_mission_item.lon = _home_pos.lon;
|
||||||
|
_mission_item.altitude_is_relative = false;
|
||||||
|
_mission_item.altitude = _home_pos.altitude + _parameters.land_alt;
|
||||||
|
_mission_item.yaw = NAN;
|
||||||
|
_mission_item.loiter_radius = _parameters.loiter_radius;
|
||||||
|
_mission_item.loiter_direction = 1;
|
||||||
|
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||||
|
_mission_item.acceptance_radius = _parameters.acceptance_radius;
|
||||||
|
_mission_item.time_inside = 0.0f;
|
||||||
|
_mission_item.pitch_min = 0.0f;
|
||||||
|
_mission_item.autocontinue = true;
|
||||||
|
_mission_item.origin = ORIGIN_ONBOARD;
|
||||||
|
|
||||||
|
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
|
||||||
|
|
||||||
|
_pos_sp_triplet.next.valid = false;
|
||||||
|
|
||||||
_mission_item_triplet.current.altitude_is_relative = false;
|
|
||||||
_mission_item_triplet.current.lat = _home_pos.lat;
|
|
||||||
_mission_item_triplet.current.lon = _home_pos.lon;
|
|
||||||
_mission_item_triplet.current.altitude = descend_alt;
|
|
||||||
_mission_item_triplet.current.yaw = NAN;
|
|
||||||
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
|
|
||||||
_mission_item_triplet.current.loiter_direction = 1;
|
|
||||||
_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
|
|
||||||
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
|
|
||||||
_mission_item_triplet.current.time_inside = 0.0f;
|
|
||||||
_mission_item_triplet.current.pitch_min = 0.0f;
|
|
||||||
_mission_item_triplet.current.autocontinue = true;
|
|
||||||
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
|
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude);
|
mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case RTL_STATE_LAND: {
|
case RTL_STATE_LAND: {
|
||||||
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
|
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
|
||||||
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
|
|
||||||
|
|
||||||
_mission_item_triplet.current_valid = true;
|
_mission_item_valid = true;
|
||||||
_mission_item_triplet.next_valid = false;
|
|
||||||
|
_mission_item.lat = _home_pos.lat;
|
||||||
|
_mission_item.lon = _home_pos.lon;
|
||||||
|
_mission_item.altitude_is_relative = false;
|
||||||
|
_mission_item.altitude = _home_pos.altitude;
|
||||||
|
_mission_item.yaw = NAN;
|
||||||
|
_mission_item.loiter_radius = _parameters.loiter_radius;
|
||||||
|
_mission_item.loiter_direction = 1;
|
||||||
|
_mission_item.nav_cmd = NAV_CMD_LAND;
|
||||||
|
_mission_item.acceptance_radius = _parameters.acceptance_radius;
|
||||||
|
_mission_item.time_inside = 0.0f;
|
||||||
|
_mission_item.pitch_min = 0.0f;
|
||||||
|
_mission_item.autocontinue = true;
|
||||||
|
_mission_item.origin = ORIGIN_ONBOARD;
|
||||||
|
|
||||||
|
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
|
||||||
|
|
||||||
|
_pos_sp_triplet.next.valid = false;
|
||||||
|
|
||||||
_mission_item_triplet.current.altitude_is_relative = false;
|
|
||||||
_mission_item_triplet.current.lat = _home_pos.lat;
|
|
||||||
_mission_item_triplet.current.lon = _home_pos.lon;
|
|
||||||
_mission_item_triplet.current.altitude = _home_pos.altitude;
|
|
||||||
_mission_item_triplet.current.yaw = NAN;
|
|
||||||
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
|
|
||||||
_mission_item_triplet.current.loiter_direction = 1;
|
|
||||||
_mission_item_triplet.current.nav_cmd = NAV_CMD_LAND;
|
|
||||||
_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
|
|
||||||
_mission_item_triplet.current.time_inside = 0.0f;
|
|
||||||
_mission_item_triplet.current.pitch_min = 0.0f;
|
|
||||||
_mission_item_triplet.current.autocontinue = true;
|
|
||||||
_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
|
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
|
mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -1247,18 +1243,46 @@ Navigator::set_rtl_item()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
publish_mission_item_triplet();
|
publish_position_setpoint_triplet();
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
|
||||||
|
{
|
||||||
|
sp->valid = true;
|
||||||
|
if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
|
||||||
|
/* set home position for RTL item */
|
||||||
|
sp->lat = _home_pos.lat;
|
||||||
|
sp->lon = _home_pos.lon;
|
||||||
|
sp->alt = _home_pos.altitude + _parameters.rtl_alt;
|
||||||
|
} else {
|
||||||
|
sp->lat = item->lat;
|
||||||
|
sp->lon = item->lon;
|
||||||
|
sp->alt = item->altitude_is_relative ? item->alt + _home_pos.altitude : item->alt;
|
||||||
|
}
|
||||||
|
sp->yaw = item->yaw;
|
||||||
|
if (item->nav_cmd == NAV_CMD_TAKEOFF) {
|
||||||
|
sp->type = SETPOINT_TYPE_TAKEOFF;
|
||||||
|
} else if (item->nav_cmd == NAV_CMD_LAND) {
|
||||||
|
sp->type = SETPOINT_TYPE_LAND;
|
||||||
|
} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
||||||
|
item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
||||||
|
item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
|
||||||
|
sp->type = SETPOINT_TYPE_LOITER;
|
||||||
|
} else {
|
||||||
|
sp->type = SETPOINT_TYPE_NORMAL;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
Navigator::check_mission_item_reached()
|
Navigator::check_mission_item_reached()
|
||||||
{
|
{
|
||||||
/* only check if there is actually a mission item to check */
|
/* only check if there is actually a mission item to check */
|
||||||
if (!_mission_item_triplet.current_valid) {
|
if (!_mission_item_valid) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
|
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
|
||||||
if (_vstatus.is_rotary_wing) {
|
if (_vstatus.is_rotary_wing) {
|
||||||
return _vstatus.condition_landed;
|
return _vstatus.condition_landed;
|
||||||
} else {
|
} else {
|
||||||
|
@ -1269,10 +1293,10 @@ Navigator::check_mission_item_reached()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* XXX TODO count turns */
|
/* XXX TODO count turns */
|
||||||
if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
||||||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
||||||
_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
|
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
|
||||||
_mission_item_triplet.current.loiter_radius > 0.01f) {
|
_mission_item.loiter_radius > 0.01f) {
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -1282,8 +1306,8 @@ Navigator::check_mission_item_reached()
|
||||||
if (!_waypoint_position_reached) {
|
if (!_waypoint_position_reached) {
|
||||||
float acceptance_radius;
|
float acceptance_radius;
|
||||||
|
|
||||||
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) {
|
if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) {
|
||||||
acceptance_radius = _mission_item_triplet.current.acceptance_radius;
|
acceptance_radius = _mission_item.acceptance_radius;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
acceptance_radius = _parameters.acceptance_radius;
|
acceptance_radius = _parameters.acceptance_radius;
|
||||||
|
@ -1294,11 +1318,11 @@ Navigator::check_mission_item_reached()
|
||||||
float dist_z = -1.0f;
|
float dist_z = -1.0f;
|
||||||
|
|
||||||
/* current relative or AMSL altitude depending on mission item altitude_is_relative flag */
|
/* current relative or AMSL altitude depending on mission item altitude_is_relative flag */
|
||||||
float wp_alt_amsl = _mission_item_triplet.current.altitude;
|
float wp_alt_amsl = _mission_item.altitude;
|
||||||
if (_mission_item_triplet.current.altitude_is_relative)
|
if (_mission_item.altitude_is_relative)
|
||||||
_mission_item_triplet.current.altitude += _home_pos.altitude;
|
_mission_itemt.altitude += _home_pos.altitude;
|
||||||
|
|
||||||
dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl,
|
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
|
||||||
(double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
|
(double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
|
||||||
&dist_xy, &dist_z);
|
&dist_xy, &dist_z);
|
||||||
|
|
||||||
|
@ -1315,9 +1339,9 @@ Navigator::check_mission_item_reached()
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!_waypoint_yaw_reached) {
|
if (!_waypoint_yaw_reached) {
|
||||||
if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) {
|
if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) {
|
||||||
/* check yaw if defined only for rotary wing except takeoff */
|
/* check yaw if defined only for rotary wing except takeoff */
|
||||||
float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw);
|
float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
|
||||||
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
|
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
|
||||||
_waypoint_yaw_reached = true;
|
_waypoint_yaw_reached = true;
|
||||||
}
|
}
|
||||||
|
@ -1330,14 +1354,14 @@ Navigator::check_mission_item_reached()
|
||||||
if (_waypoint_position_reached && _waypoint_yaw_reached) {
|
if (_waypoint_position_reached && _waypoint_yaw_reached) {
|
||||||
if (_time_first_inside_orbit == 0) {
|
if (_time_first_inside_orbit == 0) {
|
||||||
_time_first_inside_orbit = now;
|
_time_first_inside_orbit = now;
|
||||||
if (_mission_item_triplet.current.time_inside > 0.01f) {
|
if (_mission_item.time_inside > 0.01f) {
|
||||||
mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside);
|
mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* check if the MAV was long enough inside the waypoint orbit */
|
/* check if the MAV was long enough inside the waypoint orbit */
|
||||||
if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6)
|
if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
|
||||||
|| _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
|
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||||
_time_first_inside_orbit = 0;
|
_time_first_inside_orbit = 0;
|
||||||
_waypoint_yaw_reached = false;
|
_waypoint_yaw_reached = false;
|
||||||
_waypoint_position_reached = false;
|
_waypoint_position_reached = false;
|
||||||
|
@ -1389,67 +1413,16 @@ Navigator::on_mission_item_reached()
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
Navigator::get_loiter_item(struct mission_item_s *item)
|
Navigator::publish_position_setpoint_triplet()
|
||||||
{
|
|
||||||
//item->altitude_is_relative
|
|
||||||
//item->lat
|
|
||||||
//item->lon
|
|
||||||
//item->altitude
|
|
||||||
//item->yaw
|
|
||||||
item->loiter_radius = _parameters.loiter_radius;
|
|
||||||
item->loiter_direction = 1;
|
|
||||||
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
|
|
||||||
item->acceptance_radius = _parameters.acceptance_radius;
|
|
||||||
item->time_inside = 0.0f;
|
|
||||||
item->pitch_min = 0.0f;
|
|
||||||
item->autocontinue = false;
|
|
||||||
item->origin = ORIGIN_ONBOARD;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
Navigator::get_takeoff_item(mission_item_s *item)
|
|
||||||
{
|
|
||||||
//item->altitude_is_relative
|
|
||||||
//item->lat
|
|
||||||
//item->lon
|
|
||||||
//item->altitude
|
|
||||||
item->yaw = NAN;
|
|
||||||
item->loiter_radius = _parameters.loiter_radius;
|
|
||||||
item->loiter_direction = 1;
|
|
||||||
item->nav_cmd = NAV_CMD_TAKEOFF;
|
|
||||||
item->acceptance_radius = _parameters.acceptance_radius;
|
|
||||||
item->time_inside = 0.0f;
|
|
||||||
item->pitch_min = 0.0;
|
|
||||||
item->autocontinue = true;
|
|
||||||
item->origin = ORIGIN_ONBOARD;
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item)
|
|
||||||
{
|
|
||||||
if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
|
|
||||||
/* append the home position to RTL item */
|
|
||||||
new_mission_item->lat = _home_pos.lat;
|
|
||||||
new_mission_item->lon = _home_pos.lon;
|
|
||||||
new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt;
|
|
||||||
new_mission_item->altitude_is_relative = false;
|
|
||||||
new_mission_item->loiter_radius = _parameters.loiter_radius;
|
|
||||||
new_mission_item->acceptance_radius = _parameters.acceptance_radius;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
Navigator::publish_mission_item_triplet()
|
|
||||||
{
|
{
|
||||||
/* lazily publish the mission triplet only once available */
|
/* lazily publish the mission triplet only once available */
|
||||||
if (_triplet_pub > 0) {
|
if (_pos_sp_triplet_pub > 0) {
|
||||||
/* publish the mission triplet */
|
/* publish the mission triplet */
|
||||||
orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet);
|
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* advertise and publish */
|
/* advertise and publish */
|
||||||
_triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet);
|
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1534,24 +1507,6 @@ Navigator::publish_control_mode()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) {
|
|
||||||
if (a.altitude_is_relative == b.altitude_is_relative &&
|
|
||||||
fabs(a.lat - b.lat) < FLT_EPSILON &&
|
|
||||||
fabs(a.lon - b.lon) < FLT_EPSILON &&
|
|
||||||
fabsf(a.altitude - b.altitude) < FLT_EPSILON &&
|
|
||||||
fabsf(a.yaw - b.yaw) < FLT_EPSILON &&
|
|
||||||
fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON &&
|
|
||||||
a.loiter_direction == b.loiter_direction &&
|
|
||||||
a.nav_cmd == b.nav_cmd &&
|
|
||||||
fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON &&
|
|
||||||
fabsf(a.time_inside - b.time_inside) < FLT_EPSILON &&
|
|
||||||
a.autocontinue == b.autocontinue) {
|
|
||||||
return true;
|
|
||||||
} else {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Navigator::add_fence_point(int argc, char *argv[])
|
void Navigator::add_fence_point(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
_geofence.addPoint(argc, argv);
|
_geofence.addPoint(argc, argv);
|
||||||
|
|
|
@ -750,7 +750,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||||
struct vehicle_local_position_s local_pos;
|
struct vehicle_local_position_s local_pos;
|
||||||
struct vehicle_local_position_setpoint_s local_pos_sp;
|
struct vehicle_local_position_setpoint_s local_pos_sp;
|
||||||
struct vehicle_global_position_s global_pos;
|
struct vehicle_global_position_s global_pos;
|
||||||
struct mission_item_triplet_s triplet;
|
struct position_setpoint_triplet_s triplet;
|
||||||
struct vehicle_gps_position_s gps_pos;
|
struct vehicle_gps_position_s gps_pos;
|
||||||
struct vehicle_vicon_position_s vicon_pos;
|
struct vehicle_vicon_position_s vicon_pos;
|
||||||
struct optical_flow_s flow;
|
struct optical_flow_s flow;
|
||||||
|
|
|
@ -117,8 +117,8 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
|
||||||
#include "topics/vehicle_bodyframe_speed_setpoint.h"
|
#include "topics/vehicle_bodyframe_speed_setpoint.h"
|
||||||
ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
|
ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
|
||||||
|
|
||||||
#include "topics/mission_item_triplet.h"
|
#include "topics/position_setpoint_triplet.h"
|
||||||
ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s);
|
ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
|
||||||
|
|
||||||
#include "topics/vehicle_global_velocity_setpoint.h"
|
#include "topics/vehicle_global_velocity_setpoint.h"
|
||||||
ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
|
ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
|
||||||
|
|
|
@ -46,31 +46,39 @@
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include "../uORB.h"
|
#include "../uORB.h"
|
||||||
|
|
||||||
#include "mission.h"
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @addtogroup topics
|
* @addtogroup topics
|
||||||
* @{
|
* @{
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
enum SETPOINT_TYPE
|
||||||
|
{
|
||||||
|
SETPOINT_TYPE_NORMAL = 0,
|
||||||
|
SETPOINT_TYPE_LOITER,
|
||||||
|
SETPOINT_TYPE_TAKEOFF,
|
||||||
|
SETPOINT_TYPE_LAND,
|
||||||
|
};
|
||||||
|
|
||||||
|
struct position_setpoint_s
|
||||||
|
{
|
||||||
|
bool valid; /**< true if point is valid */
|
||||||
|
double lat; /**< latitude, in deg */
|
||||||
|
double lon; /**< longitude, in deg */
|
||||||
|
float alt; /**< altitude AMSL, in m */
|
||||||
|
float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN to hold current yaw */
|
||||||
|
enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */
|
||||||
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Global position setpoint triplet in WGS84 coordinates.
|
* Global position setpoint triplet in WGS84 coordinates.
|
||||||
*
|
*
|
||||||
* This are the three next waypoints (or just the next two or one).
|
* This are the three next waypoints (or just the next two or one).
|
||||||
*/
|
*/
|
||||||
struct mission_item_triplet_s
|
struct position_setpoint_triplet_s
|
||||||
{
|
{
|
||||||
bool previous_valid;
|
struct position_setpoint_s previous;
|
||||||
bool current_valid; /**< flag indicating previous mission item is valid */
|
struct position_setpoint_s current;
|
||||||
bool next_valid; /**< flag indicating next mission item is valid */
|
struct position_setpoint_s next;
|
||||||
|
|
||||||
struct mission_item_s previous;
|
|
||||||
struct mission_item_s current;
|
|
||||||
struct mission_item_s next;
|
|
||||||
|
|
||||||
int previous_index;
|
|
||||||
int current_index;
|
|
||||||
int next_index;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -78,6 +86,6 @@ struct mission_item_triplet_s
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* register this as object request broker structure */
|
/* register this as object request broker structure */
|
||||||
ORB_DECLARE(mission_item_triplet);
|
ORB_DECLARE(position_setpoint_triplet);
|
||||||
|
|
||||||
#endif
|
#endif
|
Loading…
Reference in New Issue