Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

navigator_main: reference update for local position #9119

Closed
wants to merge 1 commit into from
Closed

navigator_main: reference update for local position #9119

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions src/modules/navigator/navigator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@
#include <px4_module_params.h>
#include <px4_module.h>
#include <systemlib/perf_counter.h>
#include <lib/geo/geo.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/mission.h>
Expand Down Expand Up @@ -158,6 +159,8 @@ class Navigator : public ModuleBase<Navigator>, 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); }
Expand Down Expand Up @@ -307,6 +310,9 @@ class Navigator : public ModuleBase<Navigator>, 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 */

Expand All @@ -317,6 +323,7 @@ class Navigator : public ModuleBase<Navigator>, 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 */
Expand Down Expand Up @@ -369,6 +376,7 @@ class Navigator : public ModuleBase<Navigator>, 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
Expand Down
88 changes: 88 additions & 0 deletions src/modules/navigator/navigator_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
{
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -252,6 +322,7 @@ Navigator::run()
if (fds[0].revents & POLLIN) {
/* success, local pos is available */
local_position_update();
local_reference_update();
}
}

Expand Down Expand Up @@ -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;
}
Expand Down