Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mission: add Jump_TAG support

This commit is contained in:
Tom Pittenger 2023-02-21 17:24:05 -08:00 committed by Tom Pittenger
parent b5bbcffaac
commit 28f4ac7bbc
2 changed files with 88 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
libraries/AP_Mission/AP_Mission.cpp
View File

@ -280,6 +280,12 @@ void AP_Mission::update()
update_exit_position();
// mission_change events
if (_last_change_time_prev_ms != _last_change_time_ms) {
_last_change_time_prev_ms = _last_change_time_ms;
on_mission_timestamp_change();
}
// save persistent waypoint_num for watchdog restore
hal.util->persistent_data.waypoint_num = _nav_cmd.index;
@ -317,6 +323,12 @@ void AP_Mission::update()
}
}
// handle events for when the mission has been updated (but maybe not changed)
void AP_Mission::on_mission_timestamp_change()
{
_jump_tag.age = 0;
}
bool AP_Mission::verify_command(const Mission_Command& cmd)
{
switch (cmd.id) {
@ -337,6 +349,7 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
case MAV_CMD_DO_AUX_FUNCTION:
case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case MAV_CMD_JUMP_TAG:
return true;
default:
return _cmd_verify_fn(cmd);
@ -382,6 +395,10 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
return command_do_set_repeat_dist(cmd);
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
return start_command_do_gimbal_manager_pitchyaw(cmd);
case MAV_CMD_JUMP_TAG:
_jump_tag.tag = cmd.content.jump.target;
_jump_tag.age = 1;
FALLTHROUGH; // fall through in case the vehicle handles tag events
default:
return _cmd_start_fn(cmd);
}
@ -1044,10 +1061,15 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
break;
case MAV_CMD_DO_JUMP: // MAV ID: 177
cmd.content.jump.target = packet.param1; // jump-to command number
case MAV_CMD_DO_JUMP_TAG: // MAV ID: 601
cmd.content.jump.target = packet.param1; // jump-to command/tag number
cmd.content.jump.num_times = packet.param2; // repeat count
break;
case MAV_CMD_JUMP_TAG: // MAV ID: 600
cmd.content.jump.target = packet.param1; // jump-to tag number
break;
case MAV_CMD_DO_CHANGE_SPEED: // MAV ID: 178
cmd.content.speed.speed_type = packet.param1; // 0 = airspeed, 1 = ground speed
cmd.content.speed.target_ms = packet.param2; // target speed in m/s
@ -1528,10 +1550,15 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
break;
case MAV_CMD_DO_JUMP: // MAV ID: 177
packet.param1 = cmd.content.jump.target; // jump-to command number
case MAV_CMD_DO_JUMP_TAG: // MAV ID: 601
packet.param1 = cmd.content.jump.target; // jump-to command/tag number
packet.param2 = cmd.content.jump.num_times; // repeat count
break;
case MAV_CMD_JUMP_TAG: // MAV ID: 600
packet.param1 = cmd.content.jump.target; // jump-to tag number
break;
case MAV_CMD_DO_CHANGE_SPEED: // MAV ID: 178
packet.param1 = cmd.content.speed.speed_type; // 0 = airspeed, 1 = ground speed
packet.param2 = cmd.content.speed.target_ms; // speed in m/s
@ -1844,6 +1871,10 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
_nav_cmd = cmd;
if (start_command(_nav_cmd)) {
_flags.nav_cmd_loaded = true;
if (_jump_tag.age > 0 && _jump_tag.age < UINT16_MAX) {
// we're tracking a tag so increase it's age on every new NAV item
_jump_tag.age++;
}
}
// 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 interrupted position
@ -1939,6 +1970,13 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i
return false;
}
// check for do-jump-tag command and convert target tag to do-jump target index and do-jump to it
if (temp_cmd.id == MAV_CMD_DO_JUMP_TAG) {
// convert tmp_cmd target from a target tag to a target index
temp_cmd.content.jump.target = get_index_of_jump_tag(temp_cmd.content.jump.target);
temp_cmd.id = MAV_CMD_DO_JUMP;
}
// check for do-jump command
if (temp_cmd.id == MAV_CMD_DO_JUMP) {
@ -2025,6 +2063,34 @@ bool AP_Mission::get_next_do_cmd(uint16_t start_index, Mission_Command& cmd)
/// jump handling methods
///
// Set the mission index to the first JUMP_TAG with this tag.
// Returns true on success, else false if no appropriate JUMP_TAG match can be found or if setting the index failed
bool AP_Mission::jump_to_tag(const uint16_t tag)
{
const uint16_t index = get_index_of_jump_tag(tag);
if (index == 0) {
return false;
}
return set_current_cmd(index);
}
// find the first JUMP_TAG with this tag and return its index.
// Returns 0 if no appropriate JUMP_TAG match can be found.
uint16_t AP_Mission::get_index_of_jump_tag(const uint16_t tag) const
{
const auto count = num_commands();
for (uint16_t i = 1; i < count; i++) {
Mission_Command tmp;
if (!read_cmd_from_storage(i, tmp)) {
continue;
}
if (tmp.id == MAV_CMD_JUMP_TAG && tmp.content.jump.target == tag) {
return i;
}
}
return 0;
}
// init_jump_tracking - initialise jump_tracking variables
void AP_Mission::init_jump_tracking()
{
@ -2467,6 +2533,10 @@ const char *AP_Mission::Mission_Command::type() const
return "Scripting";
case MAV_CMD_DO_JUMP:
return "Jump";
case MAV_CMD_DO_JUMP_TAG:
return "JumpToTag";
case MAV_CMD_JUMP_TAG:
return "Tag";
case MAV_CMD_DO_GO_AROUND:
return "Go Around";
#if AP_SCRIPTING_ENABLED

16
libraries/AP_Mission/AP_Mission.h
View File

@ -659,6 +659,14 @@ public:
bool get_item(uint16_t index, mavlink_mission_item_int_t& result) const ;
bool set_item(uint16_t index, mavlink_mission_item_int_t& source) ;
// Set the mission index to the first JUMP_TAG with this tag.
// Returns true on success, else false if no appropriate JUMP_TAG match can be found or if setting the index failed
bool jump_to_tag(const uint16_t tag);
// find the first JUMP_TAG with this tag and return its index.
// Returns 0 if no appropriate JUMP_TAG match can be found.
uint16_t get_index_of_jump_tag(const uint16_t tag) const;
private:
static AP_Mission *_singleton;
@ -666,6 +674,11 @@ private:
static bool stored_in_location(uint16_t id);
struct {
uint16_t age; // a value of 0 means we have never seen a tag. Once a tag is seen, age will increment every time the mission index changes.
uint16_t tag; // most recent tag that was successfully jumped to. Only valid if age > 0
} _jump_tag;
struct Mission_Flags {
mission_state state;
bool nav_cmd_loaded; // true if a "navigation" command has been loaded into _nav_cmd
@ -744,6 +757,8 @@ private:
// update progress made in mission to store last position in the event of mission exit
void update_exit_position(void);
void on_mission_timestamp_change();
/// sanity checks that the masked fields are not NaN's or infinite
static MAV_MISSION_RESULT sanity_check_params(const mavlink_mission_item_int_t& packet);
@ -779,6 +794,7 @@ private:
// last time that mission changed
uint32_t _last_change_time_ms;
uint32_t _last_change_time_prev_ms;
// memoisation of contains-relative:
bool _contains_terrain_alt_items; // true if the mission has terrain-relative items