From 6acb8fa66f38d20af57b8c45cc7878257abb24d2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 12:16:02 +0100 Subject: Replace mission_item_triplet with position_setpoint_triplet, WIP --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 31 +++++++++++----------- 1 file changed, 15 insertions(+), 16 deletions(-) (limited to 'src/modules/mc_pos_control') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 5ce4500cd..bc20a4f47 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -117,7 +117,7 @@ private: int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _mission_items_sub; /**< mission item triplet */ + int _pos_sp_triplet_sub; /**< mission item triplet */ orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -130,7 +130,7 @@ private: struct actuator_armed_s _arming; /**< actuator arming status */ struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ - struct mission_item_triplet_s _mission_items; /**< vehicle global position setpoint */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { @@ -236,7 +236,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_sub(-1), _arming_sub(-1), _local_pos_sub(-1), - _mission_items_sub(-1), + _pos_sp_triplet_sub(-1), /* publications */ _local_pos_sp_pub(-1), @@ -250,7 +250,7 @@ MulticopterPositionControl::MulticopterPositionControl() : memset(&_arming, 0, sizeof(_arming)); memset(&_local_pos, 0, sizeof(_local_pos)); memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); - memset(&_mission_items, 0, sizeof(_mission_items)); + memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); _params.pos_p.zero(); @@ -405,10 +405,10 @@ MulticopterPositionControl::poll_subscriptions() if (updated) orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - orb_check(_mission_items_sub, &updated); + orb_check(_pos_sp_triplet_sub, &updated); if (updated) - orb_copy(ORB_ID(mission_item_triplet), _mission_items_sub, &_mission_items); + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); } float @@ -450,7 +450,7 @@ MulticopterPositionControl::task_main() _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _mission_items_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); parameters_update(true); @@ -626,16 +626,15 @@ MulticopterPositionControl::task_main() } else { /* AUTO */ - if (_mission_items.current_valid) { - struct mission_item_s item = _mission_items.current; + if (_pos_sp_triplet.current.valid) { + struct position_setpoint_s current_sp = _pos_sp_triplet.current; - // TODO home altitude can be != ref_alt, check home_position topic - _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); + _pos_sp(2) = -(current_sp.alt - ref_alt); - map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); + map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1)); - if (isfinite(_mission_items.current.yaw)) { - _att_sp.yaw_body = _mission_items.current.yaw; + if (isfinite(current_sp.yaw)) { + _att_sp.yaw_body = current_sp.yaw; } /* in case of interrupted mission don't go to waypoint but stay at current position */ @@ -688,7 +687,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled) { /* use constant descend rate when landing, ignore altitude setpoint */ - if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) { + if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) { _vel_sp(2) = _params.land_speed; } @@ -790,7 +789,7 @@ MulticopterPositionControl::task_main() float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); float tilt_max = _params.tilt_max; if (!_control_mode.flag_control_manual_enabled) { - if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) { + if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; if (thr_min < 0.0f) -- cgit v1.2.3