forked from Archive/PX4-Autopilot
Mission topic: Use mission topic instead of global position for triplet
This commit is contained in:
Julian Oes
parent
a9e51105c8
commit
a6c5a19206
|
|
@ -54,26 +54,26 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {};
|
|||
|
||||
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
|
||||
vehicle_attitude_s &att,
|
||||
vehicle_global_position_setpoint_s &posCmd,
|
||||
vehicle_global_position_setpoint_s &lastPosCmd)
|
||||
mission_item_s &missionCmd,
|
||||
mission_item_s &lastMissionCmd)
|
||||
{
|
||||
|
||||
// heading to waypoint
|
||||
float psiTrack = get_bearing_to_next_waypoint(
|
||||
(double)pos.lat / (double)1e7d,
|
||||
(double)pos.lon / (double)1e7d,
|
||||
(double)posCmd.lat / (double)1e7d,
|
||||
(double)posCmd.lon / (double)1e7d);
|
||||
missionCmd.lat,
|
||||
missionCmd.lon);
|
||||
|
||||
// cross track
|
||||
struct crosstrack_error_s xtrackError;
|
||||
get_distance_to_line(&xtrackError,
|
||||
(double)pos.lat / (double)1e7d,
|
||||
(double)pos.lon / (double)1e7d,
|
||||
(double)lastPosCmd.lat / (double)1e7d,
|
||||
(double)lastPosCmd.lon / (double)1e7d,
|
||||
(double)posCmd.lat / (double)1e7d,
|
||||
(double)posCmd.lon / (double)1e7d);
|
||||
lastMissionCmd.lat,
|
||||
lastMissionCmd.lon,
|
||||
missionCmd.lat,
|
||||
missionCmd.lon);
|
||||
|
||||
_psiCmd = _wrap_2pi(psiTrack -
|
||||
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
|
||||
|
|
@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
|
|||
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
|
||||
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
|
||||
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
|
||||
_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
|
||||
_missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20),
|
||||
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
|
||||
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
|
||||
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
|
||||
|
|
|
|||
|
|
@ -43,7 +43,8 @@
|
|||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_set_triplet.h>
|
||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/mission_item_triplet.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
|
@ -82,8 +83,8 @@ public:
|
|||
virtual ~BlockWaypointGuidance();
|
||||
void update(vehicle_global_position_s &pos,
|
||||
vehicle_attitude_s &att,
|
||||
vehicle_global_position_setpoint_s &posCmd,
|
||||
vehicle_global_position_setpoint_s &lastPosCmd);
|
||||
mission_item_s &missionCmd,
|
||||
mission_item_s &lastMissionCmd);
|
||||
float getPsiCmd() { return _psiCmd; }
|
||||
};
|
||||
|
||||
|
|
@ -98,7 +99,7 @@ protected:
|
|||
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
|
||||
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
|
||||
UOrbSubscription<vehicle_global_position_s> _pos;
|
||||
UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
|
||||
UOrbSubscription<mission_item_triplet_s> _missionCmd;
|
||||
UOrbSubscription<manual_control_setpoint_s> _manual;
|
||||
UOrbSubscription<vehicle_status_s> _status;
|
||||
UOrbSubscription<parameter_update_s> _param_update;
|
||||
|
|
|
|||
|
|
@ -117,7 +117,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
|
|||
_vCmd(this, "V_CMD"),
|
||||
_crMax(this, "CR_MAX"),
|
||||
_attPoll(),
|
||||
_lastPosCmd(),
|
||||
_lastMissionCmd(),
|
||||
_timeStamp(0)
|
||||
{
|
||||
_attPoll.fd = _att.getHandle();
|
||||
|
|
@ -141,8 +141,8 @@ void BlockMultiModeBacksideAutopilot::update()
|
|||
setDt(dt);
|
||||
|
||||
// store old position command before update if new command sent
|
||||
if (_posCmd.updated()) {
|
||||
_lastPosCmd = _posCmd.getData();
|
||||
if (_missionCmd.updated()) {
|
||||
_lastMissionCmd = _missionCmd.getData();
|
||||
}
|
||||
|
||||
// check for new updates
|
||||
|
|
@ -159,7 +159,7 @@ void BlockMultiModeBacksideAutopilot::update()
|
|||
if (_status.main_state == MAIN_STATE_AUTO) {
|
||||
// TODO use vehicle_control_mode here?
|
||||
// update guidance
|
||||
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
|
||||
_guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current);
|
||||
}
|
||||
|
||||
// XXX handle STABILIZED (loiter on spot) as well
|
||||
|
|
@ -182,7 +182,7 @@ void BlockMultiModeBacksideAutopilot::update()
|
|||
float vCmd = _vLimit.update(_vCmd.get());
|
||||
|
||||
// altitude hold
|
||||
float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
|
||||
float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt);
|
||||
|
||||
// heading hold
|
||||
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
|
||||
|
|
|
|||
|
|
@ -264,7 +264,7 @@ private:
|
|||
BlockParamFloat _crMax;
|
||||
|
||||
struct pollfd _attPoll;
|
||||
vehicle_global_position_set_triplet_s _lastPosCmd;
|
||||
mission_item_triplet_s _lastMissionCmd;
|
||||
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
|
||||
uint64_t _timeStamp;
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -67,7 +67,7 @@
|
|||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_set_triplet.h>
|
||||
#include <uORB/topics/mission_item_triplet.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
|
@ -120,7 +120,7 @@ private:
|
|||
int _control_task; /**< task handle for sensor task */
|
||||
|
||||
int _global_pos_sub;
|
||||
int _global_set_triplet_sub;
|
||||
int _mission_item_triplet_sub;
|
||||
int _att_sub; /**< vehicle attitude subscription */
|
||||
int _attitude_sub; /**< raw rc channels data subscription */
|
||||
int _airspeed_sub; /**< airspeed subscription */
|
||||
|
|
@ -139,7 +139,7 @@ private:
|
|||
struct airspeed_s _airspeed; /**< airspeed */
|
||||
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
|
||||
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
||||
struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */
|
||||
struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
|
||||
struct accel_report _accel; /**< body frame accelerations */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
|
@ -283,7 +283,7 @@ private:
|
|||
* Control position.
|
||||
*/
|
||||
bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
|
||||
const struct vehicle_global_position_set_triplet_s &global_triplet);
|
||||
const struct mission_item_triplet_s &_mission_item_triplet);
|
||||
|
||||
float calculate_target_airspeed(float airspeed_demand);
|
||||
void calculate_gndspeed_undershoot();
|
||||
|
|
@ -318,7 +318,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
|||
|
||||
/* subscriptions */
|
||||
_global_pos_sub(-1),
|
||||
_global_set_triplet_sub(-1),
|
||||
_mission_item_triplet_sub(-1),
|
||||
_att_sub(-1),
|
||||
_airspeed_sub(-1),
|
||||
_control_mode_sub(-1),
|
||||
|
|
@ -548,11 +548,11 @@ void
|
|||
FixedwingPositionControl::vehicle_setpoint_poll()
|
||||
{
|
||||
/* check if there is a new setpoint */
|
||||
bool global_sp_updated;
|
||||
orb_check(_global_set_triplet_sub, &global_sp_updated);
|
||||
bool mission_item_triplet_updated;
|
||||
orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated);
|
||||
|
||||
if (global_sp_updated) {
|
||||
orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet);
|
||||
if (mission_item_triplet_updated) {
|
||||
orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet);
|
||||
_setpoint_valid = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -625,7 +625,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
|
|||
|
||||
bool
|
||||
FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed,
|
||||
const struct vehicle_global_position_set_triplet_s &global_triplet)
|
||||
const struct mission_item_triplet_s &mission_item_triplet)
|
||||
{
|
||||
bool setpoint = true;
|
||||
|
||||
|
|
@ -641,7 +641,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
math::Vector3 accel_earth = _R_nb.transpose() * accel_body;
|
||||
|
||||
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
|
||||
float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
|
||||
float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt;
|
||||
|
||||
/* no throttle limit as default */
|
||||
float throttle_max = 1.0f;
|
||||
|
|
@ -662,27 +662,27 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
if (_setpoint_valid) {
|
||||
|
||||
/* current waypoint (the one currently heading for) */
|
||||
math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
|
||||
math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon);
|
||||
|
||||
/* previous waypoint */
|
||||
math::Vector2f prev_wp;
|
||||
|
||||
if (global_triplet.previous_valid) {
|
||||
prev_wp.setX(global_triplet.previous.lat / 1e7f);
|
||||
prev_wp.setY(global_triplet.previous.lon / 1e7f);
|
||||
if (mission_item_triplet.previous_valid) {
|
||||
prev_wp.setX(mission_item_triplet.previous.lat);
|
||||
prev_wp.setY(mission_item_triplet.previous.lon);
|
||||
|
||||
} else {
|
||||
/*
|
||||
* No valid previous waypoint, go for the current wp.
|
||||
* This is automatically handled by the L1 library.
|
||||
*/
|
||||
prev_wp.setX(global_triplet.current.lat / 1e7f);
|
||||
prev_wp.setY(global_triplet.current.lon / 1e7f);
|
||||
prev_wp.setX(mission_item_triplet.current.lat);
|
||||
prev_wp.setY(mission_item_triplet.current.lon);
|
||||
|
||||
}
|
||||
|
||||
// XXX add RTL switch
|
||||
if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) {
|
||||
if (mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) {
|
||||
|
||||
math::Vector2f rtl_pos(_launch_lat, _launch_lon);
|
||||
|
||||
|
|
@ -698,35 +698,35 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
|
||||
// XXX handle case when having arrived at home (loiter)
|
||||
|
||||
} else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
|
||||
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
|
||||
/* waypoint is a plain navigation waypoint */
|
||||
_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
|
||||
_att_sp.roll_body = _l1_control.nav_roll();
|
||||
_att_sp.yaw_body = _l1_control.nav_bearing();
|
||||
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||
false, math::radians(_parameters.pitch_limit_min),
|
||||
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
||||
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
||||
|
||||
} else if (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
||||
global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
||||
global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
|
||||
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
||||
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 */
|
||||
_l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius,
|
||||
global_triplet.current.loiter_direction, ground_speed);
|
||||
_l1_control.navigate_loiter(next_wp, current_position, mission_item_triplet.current.loiter_radius,
|
||||
mission_item_triplet.current.loiter_direction, ground_speed);
|
||||
_att_sp.roll_body = _l1_control.nav_roll();
|
||||
_att_sp.yaw_body = _l1_control.nav_bearing();
|
||||
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||
false, math::radians(_parameters.pitch_limit_min),
|
||||
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
||||
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
||||
|
||||
} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
|
||||
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
|
||||
|
||||
/* switch to heading hold for the last meters, continue heading hold after */
|
||||
|
||||
|
|
@ -737,7 +737,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
/* heading hold, along the line connecting this and the last waypoint */
|
||||
|
||||
|
||||
// if (global_triplet.previous_valid) {
|
||||
// if (mission_item_triplet.previous_valid) {
|
||||
// target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
|
||||
// } else {
|
||||
|
||||
|
|
@ -760,7 +760,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
|
||||
/* do not go down too early */
|
||||
if (wp_distance > 50.0f) {
|
||||
altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
|
||||
altitude_error = (_mission_item_triplet.current.altitude + 25.0f) - _global_pos.alt;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -770,7 +770,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
/* 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
|
||||
|
||||
float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
|
||||
float flare_angle_rad = math::radians(10.0f);//math::radians(mission_item_triplet.current.param1)
|
||||
float land_pitch_min = math::radians(5.0f);
|
||||
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
|
||||
float airspeed_land = _parameters.airspeed_min;
|
||||
|
|
@ -783,7 +783,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
|
||||
_tecs.set_speed_weight(2.0f);
|
||||
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
|
||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||
false, flare_angle_rad,
|
||||
0.0f, _parameters.throttle_max, throttle_land,
|
||||
|
|
@ -799,7 +799,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
|
||||
/* minimize speed to approach speed */
|
||||
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
|
||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||
false, flare_angle_rad,
|
||||
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
||||
|
|
@ -809,14 +809,14 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
|
||||
/* normal cruise speed */
|
||||
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||
false, math::radians(_parameters.pitch_limit_min),
|
||||
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
||||
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
||||
}
|
||||
|
||||
} else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||
|
||||
_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
|
||||
_att_sp.roll_body = _l1_control.nav_roll();
|
||||
|
|
@ -826,9 +826,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
if (altitude_error > 10.0f) {
|
||||
|
||||
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
|
||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||
true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)),
|
||||
true, math::max(math::radians(mission_item_triplet.current.radius), math::radians(10.0f)),
|
||||
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
||||
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
|
||||
|
||||
|
|
@ -837,7 +837,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
|
||||
} else {
|
||||
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
|
||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||
false, math::radians(_parameters.pitch_limit_min),
|
||||
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
|
||||
|
|
@ -848,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
// 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());
|
||||
// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(),
|
||||
// (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid");
|
||||
// (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid");
|
||||
|
||||
// XXX at this point we always want no loiter hold if a
|
||||
// mission is active
|
||||
|
|
@ -902,7 +902,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
}
|
||||
|
||||
/* reset land state */
|
||||
if (global_triplet.current.nav_cmd != NAV_CMD_LAND) {
|
||||
if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) {
|
||||
land_noreturn = false;
|
||||
}
|
||||
|
||||
|
|
@ -1018,7 +1018,7 @@ FixedwingPositionControl::task_main()
|
|||
* do subscriptions
|
||||
*/
|
||||
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
_global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet));
|
||||
_mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
|
||||
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
|
||||
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
|
|
@ -1108,7 +1108,7 @@ FixedwingPositionControl::task_main()
|
|||
* Attempt to control position, on success (= sensors present and not in manual mode),
|
||||
* publish setpoint.
|
||||
*/
|
||||
if (control_position(current_position, ground_speed, _global_triplet)) {
|
||||
if (control_position(current_position, ground_speed, _mission_item_triplet)) {
|
||||
_att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
/* lazily publish the setpoint only once available */
|
||||
|
|
@ -1121,7 +1121,8 @@ FixedwingPositionControl::task_main()
|
|||
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
|
||||
}
|
||||
|
||||
float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy);
|
||||
/* XXX check if radius makes sense here */
|
||||
float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.radius);
|
||||
|
||||
/* lazily publish navigation capabilities */
|
||||
if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@
|
|||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_global_position_set_triplet.h>
|
||||
#include <uORB/topics/mission_item_triplet.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
|
|
|
|||
|
|
@ -56,7 +56,7 @@
|
|||
#include <arch/board/board.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_set_triplet.h>
|
||||
#include <uORB/topics/mission_item_triplet.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
|
|
@ -128,12 +128,12 @@ private:
|
|||
|
||||
struct vehicle_status_s _vstatus; /**< vehicle status */
|
||||
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
||||
struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */
|
||||
struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
unsigned _mission_items_maxcount; /**< maximum number of mission items supported */
|
||||
struct mission_item_s * _mission_items; /**< storage for mission items */
|
||||
unsigned _mission_item_count; /**< maximum number of mission items supported */
|
||||
struct mission_item_s * _mission_items; /**< storage for mission */
|
||||
bool _mission_valid; /**< flag if mission is valid */
|
||||
|
||||
|
||||
|
|
@ -427,7 +427,7 @@ Navigator::task_main()
|
|||
|
||||
math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
|
||||
// Current waypoint
|
||||
math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
|
||||
math::Vector2f next_wp(_mission_item_triplet.current.lat / 1e7f, _mission_item_triplet.current.lon / 1e7f);
|
||||
// Global position
|
||||
math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
|
||||
|
||||
|
|
@ -441,17 +441,17 @@ Navigator::task_main()
|
|||
// Next waypoint
|
||||
math::Vector2f prev_wp;
|
||||
|
||||
if (_global_triplet.previous_valid) {
|
||||
prev_wp.setX(_global_triplet.previous.lat / 1e7f);
|
||||
prev_wp.setY(_global_triplet.previous.lon / 1e7f);
|
||||
if (_mission_item_triplet.previous_valid) {
|
||||
prev_wp.setX(_mission_item_triplet.previous.lat / 1e7f);
|
||||
prev_wp.setY(_mission_item_triplet.previous.lon / 1e7f);
|
||||
|
||||
} else {
|
||||
/*
|
||||
* No valid next waypoint, go for heading hold.
|
||||
* This is automatically handled by the L1 library.
|
||||
*/
|
||||
prev_wp.setX(_global_triplet.current.lat / 1e7f);
|
||||
prev_wp.setY(_global_triplet.current.lon / 1e7f);
|
||||
prev_wp.setX(_mission_item_triplet.current.lat / 1e7f);
|
||||
prev_wp.setY(_mission_item_triplet.current.lon / 1e7f);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -461,13 +461,13 @@ Navigator::task_main()
|
|||
|
||||
// XXX to be put in its own class
|
||||
|
||||
if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
|
||||
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
|
||||
/* waypoint is a plain navigation waypoint */
|
||||
|
||||
|
||||
} else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
||||
_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
||||
_global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
|
||||
} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
|
||||
_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 */
|
||||
|
||||
|
|
@ -504,28 +504,28 @@ Navigator::task_main()
|
|||
|
||||
/******** MAIN NAVIGATION STATE MACHINE ********/
|
||||
|
||||
if (_global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
|
||||
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
|
||||
// XXX define launch position and return
|
||||
|
||||
} else if (_global_triplet.current.nav_cmd == NAV_CMD_LAND) {
|
||||
} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
|
||||
// XXX flared descent on final approach
|
||||
|
||||
} else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||
} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||
|
||||
/* apply minimum pitch if altitude has not yet been reached */
|
||||
// if (_global_pos.alt < _global_triplet.current.altitude) {
|
||||
// _att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1);
|
||||
// if (_global_pos.alt < _mission_item_triplet.current.altitude) {
|
||||
// _att_sp.pitch_body = math::max(_att_sp.pitch_body, _mission_item_triplet.current.param1);
|
||||
// }
|
||||
}
|
||||
|
||||
/* lazily publish the setpoint only once available */
|
||||
if (_triplet_pub > 0) {
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_global_position_set_triplet), _triplet_pub, &_global_triplet);
|
||||
orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet);
|
||||
_triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -120,8 +120,8 @@ ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setp
|
|||
#include "topics/vehicle_global_position_setpoint.h"
|
||||
ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
|
||||
|
||||
#include "topics/vehicle_global_position_set_triplet.h"
|
||||
ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
|
||||
#include "topics/mission_item_triplet.h"
|
||||
ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s);
|
||||
|
||||
#include "topics/vehicle_global_velocity_setpoint.h"
|
||||
ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
|
|
@ -35,18 +35,18 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vehicle_global_position_setpoint.h
|
||||
* @file mission_item_triplet.h
|
||||
* Definition of the global WGS84 position setpoint uORB topic.
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
|
||||
#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
|
||||
#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
|
||||
#define TOPIC_MISSION_ITEM_TRIPLET_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
#include "vehicle_global_position_setpoint.h"
|
||||
#include "mission.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
|
|
@ -58,14 +58,15 @@
|
|||
*
|
||||
* This are the three next waypoints (or just the next two or one).
|
||||
*/
|
||||
struct vehicle_global_position_set_triplet_s
|
||||
struct mission_item_triplet_s
|
||||
{
|
||||
bool previous_valid; /**< flag indicating previous position is valid */
|
||||
bool next_valid; /**< flag indicating next position is valid */
|
||||
bool previous_valid;
|
||||
bool current_valid; /**< flag indicating previous mission item is valid */
|
||||
bool next_valid; /**< flag indicating next mission item is valid */
|
||||
|
||||
struct vehicle_global_position_setpoint_s previous;
|
||||
struct vehicle_global_position_setpoint_s current;
|
||||
struct vehicle_global_position_setpoint_s next;
|
||||
struct mission_item_s previous;
|
||||
struct mission_item_s current;
|
||||
struct mission_item_s next;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -73,6 +74,6 @@ struct vehicle_global_position_set_triplet_s
|
|||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(vehicle_global_position_set_triplet);
|
||||
ORB_DECLARE(mission_item_triplet);
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue