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

Jump to bottom

AP_Mission: Relative Mission (translation, rotation, altitude-adaption) #14100

Closed
wants to merge 12 commits into from
Closed

AP_Mission: Relative Mission (translation, rotation, altitude-adaption) #14100

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
368f453
AP_Mission: Relative Mission (translation, rotation, altitude-adaption)
WillyZehnder Apr 15, 2020
98b56a9
Update AP_Mission.cpp: sqrt replaced by sqrtf
WillyZehnder Apr 15, 2020
a17b1a9
Update AP_Mission.cpp: cos()/sin() -> cosf()/sinf()
WillyZehnder Apr 15, 2020
754673a
Update AP_Mission.cpp
WillyZehnder Apr 15, 2020
df78659
AP_Mission Update: correction of lng based on lat-translation
WillyZehnder Apr 19, 2020
aefcfba
Merge branch 'master' into MissionRelative
WillyZehnder Apr 23, 2020
085b35b
AP_Mission: Complete Rollback
WillyZehnder May 6, 2020
5da292d
Revert "AP_Mission: Complete Rollback"
WillyZehnder May 6, 2020
ecf0845
AP_Mission: Complete Rollback
WillyZehnder May 7, 2020
9749d74
Update AP_Mission_Commands.cpp
WillyZehnder May 7, 2020
7c912ce
Merge branch 'master' into MissionRelative
WillyZehnder May 7, 2020
1a5843a
Update ChibiOS
WillyZehnder May 7, 2020
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
176 changes: 165 additions & 11 deletions libraries/AP_Mission/AP_Mission.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,19 +18,27 @@ const AP_Param::GroupInfo AP_Mission::var_info[] = {
AP_GROUPINFO_FLAGS("TOTAL", 0, AP_Mission, _cmd_total, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),

// @Param: RESTART
// @DisplayName: Mission Restart when entering Auto mode
// @DisplayName: Mission - Restart Behaviour when entering AUTO mode
// @Description: Controls mission starting point when entering Auto mode (either restart from beginning of mission or resume from last command run)
// @Values: 0:Resume Mission, 1:Restart Mission
// @Range: 0 3
// @Values: 0:Resume Mission at the WayPoint it has been suspended, 1:Restart Mission, 2:Restart PARALLEL translated Relative Mission, 3:Restart ROTATED translated Relative Mission
// @User: Advanced
AP_GROUPINFO("RESTART", 1, AP_Mission, _restart, AP_MISSION_RESTART_DEFAULT),

// @Param: OPTIONS
// @DisplayName: Mission options bitmask
// @DisplayName: Mission - options bitmask
// @Description: Bitmask of what options to use in missions.
// @Bitmask: 0:Clear Mission on reboot, 1:Use distance to land calc on battery failsafe
// @Bitmask: 0:Clear Mission on reboot, 1:Use distance to land calc on battery failsafe, 2: At restart skip altitude at first Waypoint, 3: At restart use altitude offset (Start-Waypoint to first Waypoint) for all Waypoints
// @User: Advanced
AP_GROUPINFO("OPTIONS", 2, AP_Mission, _options, AP_MISSION_OPTIONS_DEFAULT),

// @Param: NO_TRANS
// @DisplayName: Mission - Radius to ignore translation
// @Description: Radius around HomeLocation therein translation of Relative Mission will be ignored (suggestion: Copter/Rover/Boat 5m, Plane 100m)
// @Units: m
// @User: Advanced
AP_GROUPINFO("NO_TRANS", 3, AP_Mission, _no_translation, AP_MISSION_NO_TRANSLATION_DEFAULT),

AP_GROUPEND
};

Expand Down Expand Up @@ -65,6 +73,63 @@ void AP_Mission::init()
/// To-Do: should we validate the mission first and return true/false?
void AP_Mission::start()
{
_skip_first_wp = false;

switch (_restart) { // fix the behaviour at restart of the Mission
case 0:
restart_behaviour = Restart_Behaviour::RESUME_AT_LAST_WP;
break;
case 1:
restart_behaviour = Restart_Behaviour::RESTART_AT_BEGINNING;
break;
case 2:
restart_behaviour = Restart_Behaviour::RESTART_PARALLEL_TRANSLATED;
break;
case 3:
restart_behaviour = Restart_Behaviour::RESTART_ROTATED_TRANSLATED;
break;
default:
gcs().send_text(MAV_SEVERITY_NOTICE, "MIS_RESTART out of range: %i", (int)_restart);
restart_behaviour = Restart_Behaviour::RESUME_AT_LAST_WP;
}

if (restart_behaviour >= Restart_Behaviour::RESTART_PARALLEL_TRANSLATED) { // memorize the Start-Waypoint (location of switching to AUTO) for Relative Mission

_translation.calculated = false;

// determine the distance in [m] between the Homepoint and the location of switching to AUTO, to decide if a translation will happen
AP::ahrs().get_position(_start_loc); // .alt absolute [cm] in every case
Mission_Command tmp;
tmp.content.location = AP::ahrs().get_home();
float tmp_distance = tmp.content.location.get_distance(_start_loc); // in [m]
_translation.do_translation = (tmp_distance < _no_translation) ? false : true; // no translation if the distance to Homepoint is too small
_translation.alt = tmp.content.location.alt; // altitude of Homepoint, necessary for calculation of translation at Restart

// check if one of the Waypoints has (terrain_alt == 1) -> no translation allowed
uint16_t i=0;
while (get_next_cmd(i, tmp, true) && (_translation.do_translation) && (tmp.id != MAV_CMD_DO_LAND_START)) { // advance to the next command
// check if navigation or "do" command
if (is_nav_cmd(tmp)) {
// check if Waypoint
if (!(tmp.content.location.lat == 0 && tmp.content.location.lng == 0)) {
switch (tmp.id) { // no translation for TAKEOFF or LAND commands
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_NAV_LAND:
case MAV_CMD_NAV_VTOL_TAKEOFF:
case MAV_CMD_NAV_VTOL_LAND:
break;
default:
if (tmp.content.location.terrain_alt == 1) {
_translation.do_translation = false;
}
}
}
}
// move on to next command
i++;
}
}

_flags.state = MISSION_RUNNING;

reset(); // reset mission to the first command, resets jump tracking
Expand Down Expand Up @@ -151,7 +216,7 @@ void AP_Mission::resume()
bool AP_Mission::starts_with_takeoff_cmd()
{
Mission_Command cmd = {};
uint16_t cmd_index = _restart ? AP_MISSION_CMD_INDEX_NONE : _nav_cmd.index;
uint16_t cmd_index = (restart_behaviour==Restart_Behaviour::RESUME_AT_LAST_WP) ? AP_MISSION_CMD_INDEX_NONE : _nav_cmd.index;
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
}
Expand All @@ -178,10 +243,10 @@ bool AP_Mission::starts_with_takeoff_cmd()
return false;
}

/// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()
/// start_or_resume - if MIS_AUTORESTART==0 this will call resume(), otherwise it will call start()
void AP_Mission::start_or_resume()
{
if (_restart == 1 && !_force_resume) {
if (restart_behaviour >= Restart_Behaviour::RESTART_AT_BEGINNING && !_force_resume) {
start();
} else {
resume();
Expand Down Expand Up @@ -1564,17 +1629,103 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
// save previous nav command index
_prev_nav_cmd_id = _nav_cmd.id;
_prev_nav_cmd_index = _nav_cmd.index;
// save separate previous nav command index if it contains lat,long,alt
// if it contains lat,long ...
if (!(cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) {
// save separate previous nav command index
_prev_nav_cmd_wp_index = _nav_cmd.index;

// calculate and do translation/rotation for Relative Mission
if (_translation.do_translation) { // no translation if we are behind DO_LAND_START
switch (cmd.id) { // no translation for TAKEOFF or LAND commands
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_NAV_LAND:
case MAV_CMD_NAV_VTOL_TAKEOFF:
case MAV_CMD_NAV_VTOL_LAND:
break;
default:
// calculate parallel translation: from Base-Waypoint (first Waypoint of Mission) to Start-Waypoint (switch-to-AUTO-position)
if ((!_translation.calculated)&&(restart_behaviour >= Restart_Behaviour::RESTART_PARALLEL_TRANSLATED)) {
if (AP_MISSION_MASK_SKIP_FIRST_WP & _options) {
_skip_first_wp = true;
}
_translation.calculated = true; // do that just once at very first WayPoint (Start-Waypoint)
_translation.lat = _start_loc.lat - cmd.content.location.lat;
_translation.lng = _start_loc.lng - cmd.content.location.lng;

// calculate altitude-translation
if (cmd.content.location.relative_alt == 1) {
_translation.alt = _start_loc.alt - cmd.content.location.alt - _translation.alt; // subtract Home-Altitude if WP-altitude is relative to
} else {
_translation.alt = _start_loc.alt - cmd.content.location.alt;
}

Mission_Command tmp;
// get direction from Homepoint to Start-Waypoint
tmp.content.location = AP::ahrs().get_home();
_translation.direction = tmp.content.location.get_bearing_to(_start_loc); // in centidegrees from 0 36000

// direction from Homepoint to not translated Base-Waypoint / amount of rotation (relative to Base-Waypoint)
_translation.direction -= tmp.content.location.get_bearing_to(cmd.content.location);
if (_translation.direction < 0) {
_translation.direction += 36000;
}

cmd.content.location = _start_loc;
switch (restart_behaviour) {
case Restart_Behaviour::RESTART_PARALLEL_TRANSLATED:
gcs().send_text(MAV_SEVERITY_INFO, "Mission: %u %s Restart Parallel Translated", cmd.index, cmd.type());
break;
case Restart_Behaviour::RESTART_ROTATED_TRANSLATED:
gcs().send_text(MAV_SEVERITY_INFO, "Mission: %u %s Restart Rotated", cmd.index, cmd.type());
break;
default:
gcs().send_text(MAV_SEVERITY_NOTICE, "Mission: %u %s Restart Mode unknown %i", cmd.index, cmd.type(), static_cast<int>(restart_behaviour));
}
gcs().send_text(MAV_SEVERITY_INFO, "Mission: alt-transmission %.2fm", static_cast<float>(_translation.alt)/100.0);
}
else {
if (restart_behaviour >= Restart_Behaviour::RESTART_PARALLEL_TRANSLATED){ // do at least parallel translation
cmd.content.location.lat += _translation.lat;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is not accurately translating anything. You can't simply add lat/long pairs, you actually have to project it down a bearing at a distance. It probably looks correct at a small scale, but it will slowly be warping the mission.

cmd.content.location.lng += _translation.lng;
if (AP_MISSION_MASK_USE_ALT_OFFSET & _options) { // do altitude translation
cmd.content.location.alt += _translation.alt;
}
}

if (restart_behaviour >= Restart_Behaviour::RESTART_ROTATED_TRANSLATED){ // do additional rotation
Rotation tmp;
float rel_lat, rel_lng; // position of currently parallel translated WayPoint relative to Start-Waypoint
float rel_distance; // imaginary distance lat/lng from Start-Waypoint to current WP in [10^7 Degrees]
rel_lat = cmd.content.location.lat - _start_loc.lat;
rel_lng = (cmd.content.location.lng - _start_loc.lng)*cmd.content.location.longitude_scale();;
tmp.direction = _start_loc.get_bearing_to(cmd.content.location); // direction from Start_waypoint to current WP in centidegrees
tmp.direction = _translation.direction + tmp.direction; // total rotation direction in centidegrees
if (tmp.direction < 0) {
tmp.direction =+ 36000;
}
rel_distance = sqrtf((rel_lat*rel_lat)+(rel_lng*rel_lng));
tmp.lat = (int32_t)(cosf((float)tmp.direction/100.0*DEG_TO_RAD)*rel_distance);
tmp.lng = (int32_t)(sinf((float)tmp.direction/100.0*DEG_TO_RAD)*rel_distance)/cmd.content.location.longitude_scale();
cmd.content.location.lat = _start_loc.lat + tmp.lat;
cmd.content.location.lng = _start_loc.lng + tmp.lng;
}
}
}
}
}
// set current navigation command and start it
_nav_cmd = cmd;
if (start_command(_nav_cmd)) {
_flags.nav_cmd_loaded = true;
if (!_skip_first_wp) // skip first Waypoint of a translated Mission
{
if (start_command(_nav_cmd)) {
_flags.nav_cmd_loaded = true;
}
}else{
_skip_first_wp = false;
gcs().send_text(MAV_SEVERITY_INFO, "Mission: %u %s First-WP skipped", cmd.index, cmd.type());
}
// save a loaded wp index in history array for when _repeat_dist is set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST
// and prevent history being re-written until vehicle returns to interupted position
// and prevent history being re-written until vehicle returns to interrupted position
if(_repeat_dist > 0 && !_flags.resuming_mission && _nav_cmd.index != AP_MISSION_CMD_INDEX_NONE && !(_nav_cmd.content.location.lat == 0 && _nav_cmd.content.location.lng == 0)) {
// update mission history. last index position is always the most recent wp loaded.
for (uint8_t i=0; i<AP_MISSION_MAX_WP_HISTORY-1; i++) {
Expand All @@ -1592,6 +1743,9 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
}else{
// set current do command and start it (if not already set)
if (!_flags.do_cmd_loaded) {
if (cmd.id == MAV_CMD_DO_LAND_START) {
_translation.do_translation = false; // no translation of landing-locations
}
_do_cmd = cmd;
_flags.do_cmd_loaded = true;
start_command(_do_cmd);
Expand Down
39 changes: 36 additions & 3 deletions libraries/AP_Mission/AP_Mission.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,11 @@
#define AP_MISSION_RESTART_DEFAULT 0 // resume the mission from the last command run by default

#define AP_MISSION_OPTIONS_DEFAULT 0 // Do not clear the mission when rebooting
#define AP_MISSION_NO_TRANSLATION_DEFAULT 50 // no translation of the Mission within a radius of 50m around Homepoint (it's a compromise: for Copter/Rover/Boat 5-10m, for Plane 50-100m should be sufficient)
#define AP_MISSION_MASK_MISSION_CLEAR (1<<0) // If set then Clear the mission on boot
#define AP_MISSION_MASK_DIST_TO_LAND_CALC (1<<1) // Allow distance to best landing calculation to be run on failsafe
#define AP_MISSION_MASK_SKIP_FIRST_WP (1<<2) // Skip first Waypoint altitude at Restart of Mission
#define AP_MISSION_MASK_USE_ALT_OFFSET (1<<3) // Use altitude offset at Restart of Mission for all Waypoints

#define AP_MISSION_MAX_WP_HISTORY 7 // The maximum number of previous wp commands that will be stored from the active missions history
#define LAST_WP_PASSED (AP_MISSION_MAX_WP_HISTORY-2)
Expand Down Expand Up @@ -196,6 +199,13 @@ class AP_Mission {
float release_rate; // release rate in meters/second
};

enum class Restart_Behaviour {
RESUME_AT_LAST_WP,
RESTART_AT_BEGINNING,
RESTART_PARALLEL_TRANSLATED,
RESTART_ROTATED_TRANSLATED
};

union Content {
// jump structure
Jump_Command jump;
Expand Down Expand Up @@ -264,6 +274,24 @@ class AP_Mission {
Location location{}; // Waypoint location
};

// for lat/lng translation of a Relative Mission
struct Translation {
int32_t lat; // latitude-displacement of location where AUTO has been switched on, relative to first waypoint [10^7�]
int32_t lng; // longitude-displacement of location where AUTO has been switched on, relative to first waypoint [10^7�]
int32_t alt; // altitude-displacement of location where AUTO has been switched on, relative to first waypoint [cm]
int32_t direction; // direction from HomePoint to the location where AUTO has been switched on [10^2�] North=0 East=9000
bool do_translation; // for marking if location where AUTO has been switched is far enough from home-location
bool calculated; // for marking if first waypoint is still proceeded and displacement is calculated
};

// for lat/lng rotation of a Relative Mission
struct Rotation {
int32_t lat; // latitude-displacement of location where AUTO has been switched on relative to current translated waypoint [10^7�]
int32_t lng; // longitude-displacement of location where AUTO has been switched on relative to current translated waypoint [10^7�]
int32_t direction; // direction from the location where AUTO has been switched on to the current translated waypoint [10^2�] North=0 East=9000
};


// command structure
struct Mission_Command {
uint16_t index; // this commands position in the command list
Expand Down Expand Up @@ -595,16 +623,18 @@ class AP_Mission {
static MAV_MISSION_RESULT sanity_check_params(const mavlink_mission_item_int_t& packet);

// parameters
AP_Int16 _cmd_total; // total number of commands in the mission
AP_Int8 _restart; // controls mission starting point when entering Auto mode (either restart from beginning of mission or resume from last command run)
AP_Int16 _options; // bitmask options for missions, currently for mission clearing on reboot but can be expanded as required
AP_Int16 _cmd_total; // total number of commands in the mission
AP_Int8 _restart; // controls mission starting point when entering Auto mode (either restart from beginning of mission or resume from last command run)
AP_Int16 _options; // bitmask options for missions, currently for mission clearing on reboot but can be expanded as required
AP_Float _no_translation;// Distance from HomeLocation wherein a translation of a Relative Mission will be ignored

// pointer to main program functions
mission_cmd_fn_t _cmd_start_fn; // pointer to function which will be called when a new command is started
mission_cmd_fn_t _cmd_verify_fn; // pointer to function which will be called repeatedly to ensure a command is progressing
mission_complete_fn_t _mission_complete_fn; // pointer to function which will be called when mission completes

// internal variables
AP_Mission::Restart_Behaviour restart_behaviour; // behaviour at restart of a Mission
struct Mission_Command _nav_cmd; // current "navigation" command. It's position in the command list is held in _nav_cmd.index
struct Mission_Command _do_cmd; // current "do" command. It's position in the command list is held in _do_cmd.index
struct Mission_Command _resume_cmd; // virtual wp command that is used to resume mission if the mission needs to be rewound on resume.
Expand All @@ -613,6 +643,9 @@ class AP_Mission {
uint16_t _prev_nav_cmd_wp_index; // index of the previous "navigation" command that contains a waypoint. Rarely used which is why we don't store the whole command
bool _force_resume; // when set true it forces mission to resume irrespective of MIS_RESTART param.
struct Location _exit_position; // the position in the mission that the mission was exited
struct Location _start_loc; // the location where the MODE has been switched to AUTO
struct Translation _translation; // info concerning the translation of a Relative Mission
bool _skip_first_wp; // when set in Relative Mission it forces to skip the first Waypoint at beginning of Mission

// jump related variables
struct jump_tracking_struct {
Expand Down