From 352a1ef095d111f06e951d39dd221c1f345ddce0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 1 Jan 2014 23:20:36 +0400 Subject: [PATCH] mc_pos_control: minor fixes --- .../mc_pos_control/mc_pos_control_main.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) 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 a416228db3..3ee98bd410 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -130,8 +130,6 @@ private: struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { - param_t takeoff_alt; - param_t takeoff_gap; param_t thr_min; param_t thr_max; param_t z_p; @@ -155,8 +153,6 @@ private: } _params_handles; /**< handles for interesting parameters */ struct { - float takeoff_alt; - float takeoff_gap; float thr_min; float thr_max; float tilt_max; @@ -265,8 +261,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_sp.zero(); _vel_prev.zero(); - _params_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - _params_handles.takeoff_gap = param_find("NAV_TAKEOFF_GAP"); _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); _params_handles.z_p = param_find("MPC_Z_P"); @@ -327,8 +321,6 @@ MulticopterPositionControl::parameters_update(bool force) orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); if (updated || force) { - param_get(_params_handles.takeoff_alt, &_params.takeoff_alt); - param_get(_params_handles.takeoff_gap, &_params.takeoff_gap); param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max, &_params.tilt_max); @@ -622,6 +614,9 @@ MulticopterPositionControl::task_main() if (_mission_items.current_valid) { struct mission_item_s item = _mission_items.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); + if (item.nav_cmd == NAV_CMD_TAKEOFF) { /* use current position setpoint or current position */ if (reset_sp_xy) { @@ -637,10 +632,9 @@ MulticopterPositionControl::task_main() } else { map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); - // TODO home altitude can be != ref_alt, check home_position topic - _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); - - _att_sp.yaw_body = _mission_items.current.yaw; + if (isfinite(_mission_items.current.yaw)) { + _att_sp.yaw_body = _mission_items.current.yaw; + } /* in case of interrupted mission don't go to waypoint but stay at current position */ reset_sp_xy = true;