diff options
Diffstat (limited to 'src/modules/mc_pos_control/mc_pos_control_main.cpp')
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 95 |
1 files changed, 61 insertions, 34 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 948459c43..e2a5a5def 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -61,7 +61,7 @@ #include <uORB/topics/parameter_update.h> #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/mission_item_triplet.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <systemlib/param/param.h> #include <systemlib/err.h> @@ -113,7 +113,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 _global_pos_sp_sub; /**< vehicle global position setpoint */ + int _mission_items_sub; /**< mission item triplet */ orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -126,7 +126,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 vehicle_global_position_setpoint_s _global_pos_sp; /**< vehicle global position setpoint */ + struct mission_item_triplet_s _mission_items; /**< vehicle global position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { @@ -232,7 +232,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_sub(-1), _arming_sub(-1), _local_pos_sub(-1), - _global_pos_sp_sub(-1), + _mission_items_sub(-1), /* publications */ _local_pos_sp_pub(-1), @@ -246,7 +246,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(&_global_pos_sp, 0, sizeof(_global_pos_sp)); + memset(&_mission_items, 0, sizeof(_mission_items)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); _params.pos_p.zero(); @@ -395,9 +395,9 @@ MulticopterPositionControl::poll_subscriptions() if (updated) orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - orb_check(_global_pos_sp_sub, &updated); + orb_check(_mission_items_sub, &updated); if (updated) - orb_copy(ORB_ID(vehicle_global_position_setpoint), _global_pos_sp_sub, &_global_pos_sp); + orb_copy(ORB_ID(mission_item_triplet), _mission_items_sub, &_mission_items); } float @@ -439,7 +439,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)); - _global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + _mission_items_sub = orb_subscribe(ORB_ID(mission_item_triplet)); parameters_update(true); @@ -449,8 +449,6 @@ MulticopterPositionControl::task_main() /* get an initial update for all sensor and status data */ poll_subscriptions(); - bool reset_mission_sp = false; - bool global_pos_sp_valid = false; bool reset_man_sp_z = true; bool reset_man_sp_xy = true; bool reset_int_z = true; @@ -466,9 +464,10 @@ MulticopterPositionControl::task_main() const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; + hrt_abstime ref_timestamp = 0; + int32_t ref_lat = 0.0f; + int32_t ref_lon = 0.0f; float ref_alt = 0.0f; - hrt_abstime ref_alt_t = 0; - hrt_abstime local_ref_timestamp = 0; math::Vector<3> sp_move_rate; sp_move_rate.zero(); @@ -533,22 +532,32 @@ MulticopterPositionControl::task_main() sp_move_rate.zero(); - if (_control_mode.flag_control_manual_enabled) { - /* manual control */ - /* check for reference point updates and correct setpoint */ - if (_local_pos.ref_timestamp != ref_alt_t) { - if (ref_alt_t != 0) { - /* home alt changed, don't follow large ground level changes in manual flight */ - _pos_sp(2) += _local_pos.ref_alt - ref_alt; - } - - ref_alt_t = _local_pos.ref_timestamp; - ref_alt = _local_pos.ref_alt; - // TODO also correct XY setpoint + if (_local_pos.ref_timestamp != ref_timestamp) { + /* initialize local projection with new reference */ + double lat_home = _local_pos.ref_lat * 1e-7; + double lon_home = _local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); + + if (_control_mode.flag_control_manual_enabled && ref_timestamp != 0) { + /* correct setpoint in manual mode to stay in the same point */ + float ref_change_x = 0.0f; + float ref_change_y = 0.0f; + map_projection_project(ref_lat, ref_lon, &ref_change_x, &ref_change_y); + _pos_sp(0) += ref_change_x; + _pos_sp(1) += ref_change_y; + _pos_sp(2) += _local_pos.ref_alt - ref_alt; } + ref_timestamp = _local_pos.ref_timestamp; + ref_lat = _local_pos.ref_lat; + ref_lon = _local_pos.ref_lon; + ref_alt = _local_pos.ref_alt; + } - /* reset setpoints to current position if needed */ + if (_control_mode.flag_control_manual_enabled) { + /* manual control */ if (_control_mode.flag_control_altitude_enabled) { + /* reset setpoint Z to current altitude if needed */ if (reset_man_sp_z) { reset_man_sp_z = false; _pos_sp(2) = _pos(2); @@ -560,6 +569,7 @@ MulticopterPositionControl::task_main() } if (_control_mode.flag_control_position_enabled) { + /* reset setpoint XY to current position if needed */ if (reset_man_sp_xy) { reset_man_sp_xy = false; _pos_sp(0) = _pos(0); @@ -594,21 +604,39 @@ MulticopterPositionControl::task_main() _pos_sp = _pos + pos_sp_offs.emult(_params.vel_max); } - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - _local_pos_sp.yaw = _att_sp.yaw_body; - /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ reset_auto_sp_xy = !_control_mode.flag_control_position_enabled; reset_auto_sp_z = !_control_mode.flag_control_altitude_enabled; reset_takeoff_sp = true; - - /* force reprojection of global setpoint after manual mode */ - reset_mission_sp = true; } else { - // TODO AUTO - _pos_sp = _pos; + /* AUTO */ + if (_mission_items.current_valid) { + struct mission_item_s item = _mission_items.current; + 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); + + /* in case of interrupted mission don't go to waypoint but stop */ + reset_auto_sp_xy = true; + reset_auto_sp_z = true; + + } else { + /* no waypoint, loiter, reset position setpoint if needed */ + if (reset_auto_sp_xy) { + reset_auto_sp_xy = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + } + if (reset_auto_sp_z) { + reset_auto_sp_z = false; + _pos_sp(2) = _pos(2); + } + } } + /* copy resulting setpoint to vehicle_local_position_setpoint topic for logging */ + _local_pos_sp.yaw = _att_sp.yaw_body; _local_pos_sp.x = _pos_sp(0); _local_pos_sp.y = _pos_sp(1); _local_pos_sp.z = _pos_sp(2); @@ -874,7 +902,6 @@ MulticopterPositionControl::task_main() reset_man_sp_xy = true; reset_int_z = true; reset_int_xy = true; - reset_mission_sp = true; reset_auto_sp_xy = true; reset_auto_sp_z = true; } |