From 28f4ac7bbcc24147e2c10de9c9eeb07a79bdc3d6 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 21 Feb 2023 17:24:05 -0800 Subject: [PATCH] AP_Mission: add Jump_TAG support --- libraries/AP_Mission/AP_Mission.cpp | 74 ++++++++++++++++++++++++++++- libraries/AP_Mission/AP_Mission.h | 16 +++++++ 2 files changed, 88 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index a0fe500b46..647c56e3fa 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -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 diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 91cc7a3512..ca62b666fd 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -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