fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 15:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mission topic: Use mission topic instead of global position for triplet

Browse Source
This commit is contained in:
Julian Oes
2013-11-20 12:46:25 +01:00
parent a9e51105c8
commit a6c5a19206
9 changed files with 100 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/controllib/uorb/blocks.cpp
+9 -9
View File
@@ -54,26 +54,26 @@ 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,
vehicle_global_position_setpoint_s &posCmd, mission_item_s &missionCmd,
vehicle_global_position_setpoint_s &lastPosCmd) mission_item_s &lastMissionCmd)
{ {
// heading to waypoint // heading to waypoint
float psiTrack = get_bearing_to_next_waypoint( float psiTrack = get_bearing_to_next_waypoint(
(double)pos.lat / (double)1e7d, (double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d, (double)pos.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d, missionCmd.lat,
(double)posCmd.lon / (double)1e7d); missionCmd.lon);
// cross track // cross track
struct crosstrack_error_s xtrackError; struct crosstrack_error_s xtrackError;
get_distance_to_line(&xtrackError, get_distance_to_line(&xtrackError,
(double)pos.lat / (double)1e7d, (double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d, (double)pos.lon / (double)1e7d,
(double)lastPosCmd.lat / (double)1e7d, lastMissionCmd.lat,
(double)lastPosCmd.lon / (double)1e7d, lastMissionCmd.lon,
(double)posCmd.lat / (double)1e7d, missionCmd.lat,
(double)posCmd.lon / (double)1e7d); missionCmd.lon);
_psiCmd = _wrap_2pi(psiTrack - _psiCmd = _wrap_2pi(psiTrack -
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
@@ -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),
_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), _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
src/modules/controllib/uorb/blocks.hpp
+5 -4
View File
@@ -43,7 +43,8 @@
#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/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/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 +83,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,
vehicle_global_position_setpoint_s &posCmd, mission_item_s &missionCmd,
vehicle_global_position_setpoint_s &lastPosCmd); mission_item_s &lastMissionCmd);
float getPsiCmd() { return _psiCmd; } float getPsiCmd() { return _psiCmd; }
}; };
@@ -98,7 +99,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<vehicle_global_position_set_triplet_s> _posCmd; UOrbSubscription<mission_item_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;
src/modules/fixedwing_backside/fixedwing.cpp
+5 -5
View File
@@ -117,7 +117,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
_vCmd(this, "V_CMD"), _vCmd(this, "V_CMD"),
_crMax(this, "CR_MAX"), _crMax(this, "CR_MAX"),
_attPoll(), _attPoll(),
_lastPosCmd(), _lastMissionCmd(),
_timeStamp(0) _timeStamp(0)
{ {
_attPoll.fd = _att.getHandle(); _attPoll.fd = _att.getHandle();
@@ -141,8 +141,8 @@ void BlockMultiModeBacksideAutopilot::update()
setDt(dt); setDt(dt);
// store old position command before update if new command sent // store old position command before update if new command sent
if (_posCmd.updated()) { if (_missionCmd.updated()) {
_lastPosCmd = _posCmd.getData(); _lastMissionCmd = _missionCmd.getData();
} }
// check for new updates // check for new updates
@@ -159,7 +159,7 @@ void BlockMultiModeBacksideAutopilot::update()
if (_status.main_state == MAIN_STATE_AUTO) { if (_status.main_state == MAIN_STATE_AUTO) {
// TODO use vehicle_control_mode here? // TODO use vehicle_control_mode here?
// update guidance // 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 // XXX handle STABILIZED (loiter on spot) as well
@@ -182,7 +182,7 @@ void BlockMultiModeBacksideAutopilot::update()
float vCmd = _vLimit.update(_vCmd.get()); float vCmd = _vLimit.update(_vCmd.get());
// altitude hold // altitude hold
float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt); float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt);
// heading hold // heading hold
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
src/modules/fixedwing_backside/fixedwing.hpp
+1 -1
View File
@@ -264,7 +264,7 @@ private:
BlockParamFloat _crMax; BlockParamFloat _crMax;
struct pollfd _attPoll; struct pollfd _attPoll;
vehicle_global_position_set_triplet_s _lastPosCmd; mission_item_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:
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+43 -42
View File
@@ -67,7 +67,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/vehicle_global_position_set_triplet.h> #include <uORB/topics/mission_item_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>
@@ -120,7 +120,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 _global_set_triplet_sub; int _mission_item_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 */
@@ -139,7 +139,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 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 */ struct accel_report _accel; /**< body frame accelerations */
perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _loop_perf; /**< loop performance counter */
@@ -283,7 +283,7 @@ private:
* Control position. * Control position.
*/ */
bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, 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); float calculate_target_airspeed(float airspeed_demand);
void calculate_gndspeed_undershoot(); void calculate_gndspeed_undershoot();
@@ -318,7 +318,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* subscriptions */ /* subscriptions */
_global_pos_sub(-1), _global_pos_sub(-1),
_global_set_triplet_sub(-1), _mission_item_triplet_sub(-1),
_att_sub(-1), _att_sub(-1),
_airspeed_sub(-1), _airspeed_sub(-1),
_control_mode_sub(-1), _control_mode_sub(-1),
@@ -548,11 +548,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 global_sp_updated; bool mission_item_triplet_updated;
orb_check(_global_set_triplet_sub, &global_sp_updated); orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated);
if (global_sp_updated) { if (mission_item_triplet_updated) {
orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet); orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet);
_setpoint_valid = true; _setpoint_valid = true;
} }
} }
@@ -625,7 +625,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
bool bool
FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed, FixedwingPositionControl::control_position(const math::Vector2f &current_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; bool setpoint = true;
@@ -641,7 +641,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
math::Vector3 accel_earth = _R_nb.transpose() * accel_body; 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); _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 */ /* no throttle limit as default */
float throttle_max = 1.0f; float throttle_max = 1.0f;
@@ -662,27 +662,27 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
if (_setpoint_valid) { if (_setpoint_valid) {
/* current waypoint (the one currently heading for) */ /* 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 */ /* previous waypoint */
math::Vector2f prev_wp; math::Vector2f prev_wp;
if (global_triplet.previous_valid) { if (mission_item_triplet.previous_valid) {
prev_wp.setX(global_triplet.previous.lat / 1e7f); prev_wp.setX(mission_item_triplet.previous.lat);
prev_wp.setY(global_triplet.previous.lon / 1e7f); prev_wp.setY(mission_item_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.setX(global_triplet.current.lat / 1e7f); prev_wp.setX(mission_item_triplet.current.lat);
prev_wp.setY(global_triplet.current.lon / 1e7f); prev_wp.setY(mission_item_triplet.current.lon);
} }
// XXX add RTL switch // 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); math::Vector2f rtl_pos(_launch_lat, _launch_lon);
@@ -698,35 +698,35 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
// XXX handle case when having arrived at home (loiter) // 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 */ /* waypoint is a plain navigation waypoint */
_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); _l1_control.navigate_waypoints(prev_wp, next_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, _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, _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 (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
/* waypoint is a loiter waypoint */ /* waypoint is a loiter waypoint */
_l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius, _l1_control.navigate_loiter(next_wp, current_position, mission_item_triplet.current.loiter_radius,
global_triplet.current.loiter_direction, ground_speed); mission_item_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, _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, _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 (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 */ /* switch to heading hold for the last meters, continue heading hold after */
@@ -737,7 +737,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* heading hold, along the line connecting this and the last waypoint */ /* 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()); // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
// } else { // } else {
@@ -760,7 +760,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* do not go down too early */ /* do not go down too early */
if (wp_distance > 50.0f) { 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 &current_positio
/* 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_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 land_pitch_min = math::radians(5.0f);
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 = _parameters.airspeed_min; float airspeed_land = _parameters.airspeed_min;
@@ -783,7 +783,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
_tecs.set_speed_weight(2.0f); _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, _airspeed.indicated_airspeed_m_s, eas2tas,
false, flare_angle_rad, false, flare_angle_rad,
0.0f, _parameters.throttle_max, throttle_land, 0.0f, _parameters.throttle_max, throttle_land,
@@ -799,7 +799,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* minimize speed to approach speed */ /* 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, _airspeed.indicated_airspeed_m_s, eas2tas,
false, flare_angle_rad, false, flare_angle_rad,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -809,14 +809,14 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* normal cruise speed */ /* 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, _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 (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); _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll(); _att_sp.roll_body = _l1_control.nav_roll();
@@ -826,9 +826,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
if (altitude_error > 10.0f) { if (altitude_error > 10.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, _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, _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, _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));
@@ -837,7 +837,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
} else { } 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, _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,
@@ -848,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
// 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.getX(), (double)prev_wp.getY(), // 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 // XXX at this point we always want no loiter hold if a
// mission is active // mission is active
@@ -902,7 +902,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
} }
/* reset land state */ /* 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; land_noreturn = false;
} }
@@ -1018,7 +1018,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));
_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)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _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), * Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint. * 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(); _att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */ /* 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); _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 */ /* lazily publish navigation capabilities */
if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
src/modules/mavlink/orb_topics.h
+1 -1
View File
@@ -52,7 +52,7 @@
#include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.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_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
src/modules/navigator/navigator_main.cpp
+21 -21
View File
@@ -56,7 +56,7 @@
#include <arch/board/board.h> #include <arch/board/board.h>
#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/vehicle_global_position_set_triplet.h> #include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>
@@ -128,12 +128,12 @@ private:
struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_status_s _vstatus; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */ 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 */ perf_counter_t _loop_perf; /**< loop performance counter */
unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ unsigned _mission_item_count; /**< maximum number of mission items supported */
struct mission_item_s * _mission_items; /**< storage for mission items */ struct mission_item_s * _mission_items; /**< storage for mission */
bool _mission_valid; /**< flag if mission is valid */ 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); math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
// Current waypoint // 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 // Global position
math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
@@ -441,17 +441,17 @@ Navigator::task_main()
// Next waypoint // Next waypoint
math::Vector2f prev_wp; math::Vector2f prev_wp;
if (_global_triplet.previous_valid) { if (_mission_item_triplet.previous_valid) {
prev_wp.setX(_global_triplet.previous.lat / 1e7f); prev_wp.setX(_mission_item_triplet.previous.lat / 1e7f);
prev_wp.setY(_global_triplet.previous.lon / 1e7f); prev_wp.setY(_mission_item_triplet.previous.lon / 1e7f);
} else { } else {
/* /*
* No valid next waypoint, go for heading hold. * No valid next waypoint, go for heading hold.
* This is automatically handled by the L1 library. * This is automatically handled by the L1 library.
*/ */
prev_wp.setX(_global_triplet.current.lat / 1e7f); prev_wp.setX(_mission_item_triplet.current.lat / 1e7f);
prev_wp.setY(_global_triplet.current.lon / 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 // 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 */ /* waypoint is a plain navigation waypoint */
} else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
/* waypoint is a loiter waypoint */ /* waypoint is a loiter waypoint */
@@ -504,28 +504,28 @@ Navigator::task_main()
/******** MAIN NAVIGATION STATE MACHINE ********/ /******** 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 // 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 // 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 */ /* apply minimum pitch if altitude has not yet been reached */
// if (_global_pos.alt < _global_triplet.current.altitude) { // if (_global_pos.alt < _mission_item_triplet.current.altitude) {
// _att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1); // _att_sp.pitch_body = math::max(_att_sp.pitch_body, _mission_item_triplet.current.param1);
// } // }
} }
/* lazily publish the setpoint only once available */ /* lazily publish the setpoint only once available */
if (_triplet_pub > 0) { if (_triplet_pub > 0) {
/* publish the attitude setpoint */ /* 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 { } else {
/* advertise and publish */ /* 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);
} }
} }
src/modules/uORB/objects_common.cpp
+2 -2
View File
@@ -120,8 +120,8 @@ ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setp
#include "topics/vehicle_global_position_setpoint.h" #include "topics/vehicle_global_position_setpoint.h"
ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s); ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
#include "topics/vehicle_global_position_set_triplet.h" #include "topics/mission_item_triplet.h"
ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s); ORB_DEFINE(mission_item_triplet, struct mission_item_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);
src/modules/uORB/topics/vehicle_global_position_set_triplet.h → src/modules/uORB/topics/mission_item_triplet.h
+13 -12
View File
@@ -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: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.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. * Definition of the global WGS84 position setpoint uORB topic.
*/ */
#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ #ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ #define TOPIC_MISSION_ITEM_TRIPLET_H_
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include "../uORB.h" #include "../uORB.h"
#include "vehicle_global_position_setpoint.h" #include "mission.h"
/** /**
* @addtogroup topics * @addtogroup topics
@@ -58,14 +58,15 @@
* *
* 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 vehicle_global_position_set_triplet_s struct mission_item_triplet_s
{ {
bool previous_valid; /**< flag indicating previous position is valid */ bool previous_valid;
bool next_valid; /**< flag indicating next position is 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 mission_item_s previous;
struct vehicle_global_position_setpoint_s current; struct mission_item_s current;
struct vehicle_global_position_setpoint_s next; struct mission_item_s next;
}; };
/** /**
@@ -73,6 +74,6 @@ struct vehicle_global_position_set_triplet_s
*/ */
/* register this as object request broker structure */ /* register this as object request broker structure */
ORB_DECLARE(vehicle_global_position_set_triplet); ORB_DECLARE(mission_item_triplet);
#endif #endif