From a6c5a1920639f3ff1b2d1a0cea6eeacf6676fc76 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Nov 2013 12:46:25 +0100 Subject: [PATCH] Mission topic: Use mission topic instead of global position for triplet --- src/modules/controllib/uorb/blocks.cpp | 18 ++-- src/modules/controllib/uorb/blocks.hpp | 9 +- src/modules/fixedwing_backside/fixedwing.cpp | 10 +-- src/modules/fixedwing_backside/fixedwing.hpp | 2 +- .../fw_pos_control_l1_main.cpp | 85 ++++++++++--------- src/modules/mavlink/orb_topics.h | 2 +- src/modules/navigator/navigator_main.cpp | 42 ++++----- src/modules/uORB/objects_common.cpp | 4 +- ...n_set_triplet.h => mission_item_triplet.h} | 25 +++--- 9 files changed, 100 insertions(+), 97 deletions(-) rename src/modules/uORB/topics/{vehicle_global_position_set_triplet.h => mission_item_triplet.h} (77%) diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 448a42a996..e213ac17fc 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -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 diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 46dc1bec25..335439fb77 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -43,7 +43,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -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 _attCmd; UOrbSubscription _ratesCmd; UOrbSubscription _pos; - UOrbSubscription _posCmd; + UOrbSubscription _missionCmd; UOrbSubscription _manual; UOrbSubscription _status; UOrbSubscription _param_update; diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 6dc19df41f..108e9896d6 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -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); diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index 567efeb359..b4dbc36b09 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -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: diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a9648b207d..0b147f9032 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -67,7 +67,7 @@ #include #include #include -#include +#include #include #include #include @@ -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) { diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 91773843b6..2cba25338b 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -52,7 +52,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9e73fcb229..5d4a6ef947 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include @@ -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); } } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 769fe90459..ecc1a3b3a2 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -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); diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/mission_item_triplet.h similarity index 77% rename from src/modules/uORB/topics/vehicle_global_position_set_triplet.h rename to src/modules/uORB/topics/mission_item_triplet.h index 8516b263fc..48553b0ac5 100644 --- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h +++ b/src/modules/uORB/topics/mission_item_triplet.h @@ -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 * @author Julian Oes * @author Lorenz Meier @@ -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 #include #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