diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 38df46299745..ba54a4a56a14 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -158,6 +159,8 @@ class Navigator : public ModuleBase, public ModuleParams PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */ const vehicle_roi_s &get_vroi() { return _vroi; } + struct map_projection_reference_s *get_local_reference_pos() {return &_ref_pos;} /**< Method that returns reference projection structure */ + float get_local_reference_alt() {return _ref_alt;} /**< Method that returns reference altitdue */ bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); } bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos); } @@ -307,6 +310,9 @@ class Navigator : public ModuleBase, public ModuleParams position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */ position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */ vehicle_roi_s _vroi{}; /**< vehicle ROI */ + map_projection_reference_s _ref_pos{}; /**< local reference position structure */ + hrt_abstime _ref_timestamp{0}; /**< time stamp when reference for local frame has been updated */ + float _ref_alt{0.0f}; /**< local reference altitude */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -317,6 +323,7 @@ class Navigator : public ModuleBase, public ModuleParams bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */ bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */ bool _mission_result_updated{false}; /**< flags if mission result has seen an update */ + bool _ref_alt_is_global{false}; /**< true when the reference altitude is defined in a global reference frame */ NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */ Mission _mission; /**< class that handles the missions */ @@ -369,6 +376,7 @@ class Navigator : public ModuleBase, public ModuleParams void sensor_combined_update(); void vehicle_land_detected_update(); void vehicle_status_update(); + void local_reference_update(); /** * Publish a new position setpoint triplet for position controllers diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7d70d9dc862e..d158b8b4237f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -126,6 +126,75 @@ Navigator::local_position_update() orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); } +void +Navigator::local_reference_update() +{ + // This method is the same as the method used in the mc_pos_control. + // https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/mc_pos_control_main.cpp#L869 + // It is not used anywhere yet, but the ultimate goal is to free the position controller from + // any global computation. + + if ((_local_pos.ref_timestamp != _ref_timestamp) + && ((_vstatus.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) + || (!_ref_alt_is_global && _local_pos.z_global))) { + + double lat_current_sp, lon_current_sp; + double lat_previous_sp, lon_previous_sp; + double lat_next_sp, lon_next_sp; + float alt_current_sp { 0.0f }, alt_previous_sp { 0.0f }, alt_next_sp { + 0.0f }; + + bool new_reference = _ref_timestamp != 0; + + if (new_reference) { + // calculate current position setpoint in global frame + map_projection_reproject(&_ref_pos, _pos_sp_triplet.current.x, + _pos_sp_triplet.current.y, &lat_current_sp, + &lon_current_sp); + map_projection_reproject(&_ref_pos, _pos_sp_triplet.previous.x, + _pos_sp_triplet.previous.y, &lat_previous_sp, + &lon_previous_sp); + map_projection_reproject(&_ref_pos, _pos_sp_triplet.next.x, + _pos_sp_triplet.next.y, &lat_next_sp, &lon_next_sp); + + // the altitude setpoint is the reference altitude - the current local setpoint because + // of NED frame + alt_current_sp = _ref_alt - _pos_sp_triplet.current.z; + alt_previous_sp = _ref_alt - _pos_sp_triplet.previous.z; + alt_next_sp = _ref_alt - _pos_sp_triplet.next.z; + } + + // update local projection reference including altitude + map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); + _ref_alt = _local_pos.ref_alt; + + + if (_local_pos.z_global) { + _ref_alt_is_global = true; + } + + if (new_reference) { + // reproject position setpoint to new reference + // this effectively adjusts the position setpoint to keep the vehicle + // in its current local position. It would only change its + // global position on the next setpoint update. + map_projection_project(&_ref_pos, lat_current_sp, lon_current_sp, + &_pos_sp_triplet.current.x, &_pos_sp_triplet.current.y); + map_projection_project(&_ref_pos, lat_previous_sp, lon_previous_sp, + &_pos_sp_triplet.previous.x, &_pos_sp_triplet.previous.y); + map_projection_project(&_ref_pos, lat_next_sp, lon_next_sp, + &_pos_sp_triplet.next.x, &_pos_sp_triplet.next.y); + _pos_sp_triplet.current.z = -(alt_current_sp - _ref_alt); + _pos_sp_triplet.previous.z = -(alt_previous_sp - _ref_alt); + _pos_sp_triplet.next.z = -(alt_next_sp - _ref_alt); + + } + + // we update the timestamp because the reference gets updated only if timestamps are different + _ref_timestamp = _local_pos.ref_timestamp; + } +} + void Navigator::gps_position_update() { @@ -216,6 +285,7 @@ Navigator::run() vehicle_land_detected_update(); global_position_update(); local_position_update(); + local_reference_update(); gps_position_update(); sensor_combined_update(); home_position_update(true); @@ -252,6 +322,7 @@ Navigator::run() if (fds[0].revents & POLLIN) { /* success, local pos is available */ local_position_update(); + local_reference_update(); } } @@ -696,6 +767,23 @@ Navigator::run() if (_pos_sp_triplet_updated) { _pos_sp_triplet.timestamp = hrt_absolute_time(); + + // Map to local frame + // This is not used anywhere yet. The goal is to free the position controller + // from any global computation. + // Current waypoint projection + map_projection_project(get_local_reference_pos(), _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + &_pos_sp_triplet.current.x, &_pos_sp_triplet.current.y); + _pos_sp_triplet.current.z = - (_pos_sp_triplet.current.alt - get_local_reference_alt()); + // Previous waypoint projection + map_projection_project(get_local_reference_pos(), _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, + &_pos_sp_triplet.previous.x, &_pos_sp_triplet.previous.y); + _pos_sp_triplet.previous.z = - (_pos_sp_triplet.previous.alt - get_local_reference_alt()); + // Next waypoint projection + map_projection_project(get_local_reference_pos(), _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, + &_pos_sp_triplet.next.x, &_pos_sp_triplet.next.y); + _pos_sp_triplet.next.z = - (_pos_sp_triplet.next.alt - get_local_reference_alt()); + publish_position_setpoint_triplet(); _pos_sp_triplet_updated = false; }