/// @file AP_Mission.cpp /// @brief Handles the MAVLINK command mission stack. Reads and writes mission to storage. #include "AP_Mission_config.h" #if AP_MISSION_ENABLED #include #include #include #include #include "AP_Mission.h" #include #include #include #include #include #include const AP_Param::GroupInfo AP_Mission::var_info[] = { // @Param: TOTAL // @DisplayName: Total mission commands // @Description: The number of mission mission items that has been loaded by the ground station. Do not change this manually. // @Range: 0 32766 // @Increment: 1 // @User: Advanced // @ReadOnly: True 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 // @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 // @User: Advanced AP_GROUPINFO("RESTART", 1, AP_Mission, _restart, AP_MISSION_RESTART_DEFAULT), // @Param: OPTIONS // @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,2:ContinueAfterLand // @Bitmask{Copter}: 0:Clear Mission on reboot, 2:ContinueAfterLand // @Bitmask{Rover, Sub}: 0:Clear Mission on reboot // @User: Advanced AP_GROUPINFO("OPTIONS", 2, AP_Mission, _options, AP_MISSION_OPTIONS_DEFAULT), AP_GROUPEND }; extern const AP_HAL::HAL& hal; // storage object StorageAccess AP_Mission::_storage(StorageManager::StorageMission); HAL_Semaphore AP_Mission::_rsem; /// /// public mission methods /// /// init - initialises this library including checks the version in eeprom matches this library void AP_Mission::init() { #if AP_SDCARD_STORAGE_ENABLED // check for extra storage on microsd const auto *bc = AP::boardConfig(); if (bc != nullptr) { const auto size_kb = bc->get_sdcard_mission_kb(); if (size_kb > 0) { _failed_sdcard_storage = !_storage.attach_file(AP_MISSION_SDCARD_FILENAME, size_kb); if (_failed_sdcard_storage) { // wipe mission if storage not available, but don't save. This allows sdcard error to be fixed and reboot _cmd_total.set(0); } } } #endif // work out maximum index for our storage size if (_storage.size() >= AP_MISSION_EEPROM_COMMAND_SIZE+4) { _commands_max = (_storage.size()-4U) / AP_MISSION_EEPROM_COMMAND_SIZE; } if (_cmd_total.get() > _commands_max) { // wipe mission if storage not available, but don't save. This allows sdcard error to be fixed and reboot _cmd_total.set(0); } // check_eeprom_version - checks version of missions stored in eeprom matches this library // command list will be cleared if they do not match check_eeprom_version(); // initialize the jump tracking array init_jump_tracking(); // If Mission Clear bit is set then it should clear the mission, otherwise retain the mission. if (AP_MISSION_MASK_MISSION_CLEAR & _options) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Clearing Mission"); clear(); } _last_change_time_ms = AP_HAL::millis(); } /// start - resets current commands to point to the beginning of the mission /// To-Do: should we validate the mission first and return true/false? void AP_Mission::start() { _flags.state = MISSION_RUNNING; reset(); // reset mission to the first command, resets jump tracking // advance to the first command if (!advance_current_nav_cmd()) { // on failure set mission complete complete(); } } /// stop - stops mission execution. subsequent calls to update() will have no effect until the mission is started or resumed void AP_Mission::stop() { _flags.state = MISSION_STOPPED; } /// resume - continues the mission execution from where we last left off /// previous running commands will be re-initialized void AP_Mission::resume() { // if mission had completed then start it from the first command if (_flags.state == MISSION_COMPLETE) { start(); return; } // if mission had stopped then restart it if (_flags.state == MISSION_STOPPED) { _flags.state = MISSION_RUNNING; // if no valid nav command index restart from beginning if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) { start(); return; } } // ensure cache coherence if (!read_cmd_from_storage(_nav_cmd.index, _nav_cmd)) { // if we failed to read the command from storage, then the command may have // been from a previously loaded mission it is illogical to ever resume // flying to a command that has been excluded from the current mission start(); return; } // rewind the mission wp if the repeat distance has been set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST if (_repeat_dist > 0 && _wp_index_history[LAST_WP_PASSED] != AP_MISSION_CMD_INDEX_NONE) { // if not already in a resume state calculate the position to rewind to Mission_Command tmp_cmd; if (!_flags.resuming_mission && calc_rewind_pos(tmp_cmd)) { _resume_cmd = tmp_cmd; } // resume mission to rewound position if (_resume_cmd.index != AP_MISSION_CMD_INDEX_NONE && start_command(_resume_cmd)) { _nav_cmd = _resume_cmd; _flags.nav_cmd_loaded = true; // set flag to prevent history being re-written _flags.resuming_mission = true; return; } } // restart active navigation command. We run these on resume() // regardless of whether the mission was stopped, as we may be // re-entering AUTO mode and the nav_cmd callback needs to be run // to setup the current target waypoint if (_flags.do_cmd_loaded && _do_cmd.index != AP_MISSION_CMD_INDEX_NONE) { // restart the active do command, which will also load the nav command for us set_current_cmd(_do_cmd.index); } else if (_flags.nav_cmd_loaded) { // restart the active nav command set_current_cmd(_nav_cmd.index); } // Note: if there is no active command then the mission must have been stopped just after the previous nav command completed // update will take care of finding and starting the nav command } /// check if the next nav command is a takeoff, skipping delays bool AP_Mission::is_takeoff_next(uint16_t cmd_index) { Mission_Command cmd = {}; // check a maximum of 16 items, remembering that missions can have // loops in them for (uint8_t i=0; i<16; i++, cmd_index++) { if (!get_next_nav_cmd(cmd_index, cmd)) { return false; } switch (cmd.id) { // any of these are considered a takeoff command: case MAV_CMD_NAV_VTOL_TAKEOFF: case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_TAKEOFF_LOCAL: return true; // any of these are considered "skippable" when determining if // we "start with a takeoff command" case MAV_CMD_DO_AUX_FUNCTION: case MAV_CMD_NAV_DELAY: continue; default: return false; } } return false; } /// check mission starts with a takeoff command bool AP_Mission::starts_with_takeoff_cmd() { uint16_t cmd_index = _restart ? AP_MISSION_CMD_INDEX_NONE : _nav_cmd.index; if (cmd_index == AP_MISSION_CMD_INDEX_NONE) { cmd_index = AP_MISSION_FIRST_REAL_COMMAND; } return is_takeoff_next(cmd_index); } /* return true if MIS_OPTIONS is set to allow continue of mission logic after a land and the next waypoint is a takeoff. If this is false then after a landing is complete the vehicle should disarm and mission logic should stop */ bool AP_Mission::continue_after_land_check_for_takeoff() { if (!continue_after_land()) { return false; } if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) { return false; } return is_takeoff_next(_nav_cmd.index+1); } /// 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) { start(); } else { resume(); _force_resume = false; } } /// reset - reset mission to the first command void AP_Mission::reset() { _flags.nav_cmd_loaded = false; _flags.do_cmd_loaded = false; _flags.do_cmd_all_done = false; _flags.in_landing_sequence = false; _flags.in_return_path = false; _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE; _do_cmd.index = AP_MISSION_CMD_INDEX_NONE; _prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE; _prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE; _prev_nav_cmd_id = AP_MISSION_CMD_ID_NONE; init_jump_tracking(); reset_wp_history(); } /// clear - clears out mission /// returns false if mission was running so it could not be cleared bool AP_Mission::clear() { // do not allow clearing the mission while it is running unless disarmed if (hal.util->get_soft_armed() && _flags.state == MISSION_RUNNING) { return false; } // remove all commands truncate(0); // clear index to commands _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE; _do_cmd.index = AP_MISSION_CMD_INDEX_NONE; _flags.nav_cmd_loaded = false; _flags.do_cmd_loaded = false; _flags.state = MISSION_STOPPED; // return success return true; } /// trucate - truncate any mission items beyond index void AP_Mission::truncate(uint16_t index) { if ((unsigned)_cmd_total > index) { _cmd_total.set_and_save(index); _last_change_time_ms = AP_HAL::millis(); } } /// update - ensures the command queues are loaded with the next command and calls main programs command_init and command_verify functions to progress the mission /// should be called at 10hz or higher void AP_Mission::update() { // exit immediately if not running or no mission commands if (_flags.state != MISSION_RUNNING || _cmd_total == 0) { return; } 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; // check if we have an active nav command if (!_flags.nav_cmd_loaded || _nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) { // advance in mission if no active nav command if (!advance_current_nav_cmd()) { // failure to advance nav command means mission has completed complete(); return; } } else { // run the active nav command if (verify_command(_nav_cmd)) { // market _nav_cmd as complete (it will be started on the next iteration) _flags.nav_cmd_loaded = false; // immediately advance to the next mission command if (!advance_current_nav_cmd()) { // failure to advance nav command means mission has completed complete(); return; } } } // check if we have an active do command if (!_flags.do_cmd_loaded) { advance_current_do_cmd(); } else { // check the active do command if (verify_command(_do_cmd)) { // mark _do_cmd as complete _flags.do_cmd_loaded = false; } } } // 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) { // do-commands always return true for verify: #if AP_GRIPPER_ENABLED case MAV_CMD_DO_GRIPPER: #endif case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_SET_RELAY: case MAV_CMD_DO_REPEAT_SERVO: case MAV_CMD_DO_REPEAT_RELAY: case MAV_CMD_DO_DIGICAM_CONFIGURE: case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_SET_CAM_TRIGG_DIST: case MAV_CMD_DO_PARACHUTE: case MAV_CMD_DO_SEND_SCRIPT_MESSAGE: case MAV_CMD_DO_SPRAYER: 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: case MAV_CMD_IMAGE_START_CAPTURE: case MAV_CMD_IMAGE_STOP_CAPTURE: case MAV_CMD_SET_CAMERA_ZOOM: case MAV_CMD_SET_CAMERA_FOCUS: case MAV_CMD_SET_CAMERA_SOURCE: case MAV_CMD_VIDEO_START_CAPTURE: case MAV_CMD_VIDEO_STOP_CAPTURE: return true; default: return _cmd_verify_fn(cmd); } } bool AP_Mission::start_command(const Mission_Command& cmd) { // check for landing related commands and set flags if (is_landing_type_cmd(cmd.id) || cmd.id == MAV_CMD_DO_LAND_START) { _flags.in_landing_sequence = true; } else if (cmd.id == MAV_CMD_DO_RETURN_PATH_START) { _flags.in_return_path = true; } else if (is_takeoff_type_cmd(cmd.id)) { // Clear landing and return path flags on takeoff _flags.in_landing_sequence = false; _flags.in_return_path = false; } if (cmd.id == MAV_CMD_DO_JUMP || cmd.id == MAV_CMD_JUMP_TAG || cmd.id == MAV_CMD_DO_JUMP_TAG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u %s %u", cmd.index, cmd.type(), (unsigned)cmd.p1); } else { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: %u %s", cmd.index, cmd.type()); } switch (cmd.id) { #if AP_RC_CHANNEL_ENABLED case MAV_CMD_DO_AUX_FUNCTION: return start_command_do_aux_function(cmd); #endif #if AP_GRIPPER_ENABLED case MAV_CMD_DO_GRIPPER: return start_command_do_gripper(cmd); #endif #if AP_SERVORELAYEVENTS_ENABLED case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_SET_RELAY: case MAV_CMD_DO_REPEAT_SERVO: case MAV_CMD_DO_REPEAT_RELAY: return start_command_do_servorelayevents(cmd); #endif #if AP_CAMERA_ENABLED case MAV_CMD_DO_DIGICAM_CONFIGURE: case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_SET_CAM_TRIGG_DIST: case MAV_CMD_IMAGE_START_CAPTURE: case MAV_CMD_IMAGE_STOP_CAPTURE: case MAV_CMD_SET_CAMERA_ZOOM: case MAV_CMD_SET_CAMERA_FOCUS: case MAV_CMD_SET_CAMERA_SOURCE: case MAV_CMD_VIDEO_START_CAPTURE: case MAV_CMD_VIDEO_STOP_CAPTURE: return start_command_camera(cmd); #endif case MAV_CMD_DO_PARACHUTE: return start_command_parachute(cmd); case MAV_CMD_DO_SEND_SCRIPT_MESSAGE: return start_command_do_scripting(cmd); case MAV_CMD_DO_SPRAYER: return start_command_do_sprayer(cmd); case MAV_CMD_DO_SET_RESUME_REPEAT_DIST: 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); } } /// /// public command methods /// /// add_cmd - adds a command to the end of the command list and writes to storage /// returns true if successfully added, false on failure /// cmd.index is updated with it's new position in the mission bool AP_Mission::add_cmd(Mission_Command& cmd) { // attempt to write the command to storage bool ret = write_cmd_to_storage(_cmd_total, cmd); if (ret) { // update command's index cmd.index = _cmd_total; // increment total number of commands _cmd_total.set_and_save(_cmd_total + 1); } return ret; } /// replace_cmd - replaces the command at position 'index' in the command list with the provided cmd /// replacing the current active command will have no effect until the command is restarted /// returns true if successfully replaced, false on failure bool AP_Mission::replace_cmd(uint16_t index, const Mission_Command& cmd) { // sanity check index if (index >= (unsigned)_cmd_total) { return false; } // attempt to write the command to storage return write_cmd_to_storage(index, cmd); } /// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command bool AP_Mission::is_nav_cmd(const Mission_Command& cmd) { // NAV commands all have ids below MAV_CMD_NAV_LAST, plus some exceptions return (cmd.id <= MAV_CMD_NAV_LAST || cmd.id == MAV_CMD_NAV_SET_YAW_SPEED || cmd.id == MAV_CMD_NAV_SCRIPT_TIME || cmd.id == MAV_CMD_NAV_ATTITUDE_TIME); } /// get_next_nav_cmd - gets next "navigation" command found at or after start_index /// returns true if found, false if not found (i.e. reached end of mission command list) /// accounts for do_jump commands but never increments the jump's num_times_run (advance_current_nav_cmd is responsible for this) bool AP_Mission::get_next_nav_cmd(uint16_t start_index, Mission_Command& cmd) { // search until the end of the mission command list for (uint16_t cmd_index = start_index; cmd_index < (unsigned)_cmd_total; cmd_index++) { // get next command if (!get_next_cmd(cmd_index, cmd, false)) { // no more commands so return failure return false; } // if found a "navigation" command then return it if (is_nav_cmd(cmd)) { return true; } } // if we got this far we did not find a navigation command return false; } /// get the ground course of the next navigation leg in centidegrees /// from 0 36000. Return default_angle if next navigation /// leg cannot be determined int32_t AP_Mission::get_next_ground_course_cd(int32_t default_angle) { Mission_Command cmd; if (!get_next_nav_cmd(_nav_cmd.index+1, cmd)) { return default_angle; } // special handling for nav commands with no target location if (cmd.id == MAV_CMD_NAV_GUIDED_ENABLE || cmd.id == MAV_CMD_NAV_DELAY) { return default_angle; } if (cmd.id == MAV_CMD_NAV_SET_YAW_SPEED) { return (_nav_cmd.content.set_yaw_speed.angle_deg * 100); } return _nav_cmd.content.location.get_bearing_to(cmd.content.location); } // set_current_cmd - jumps to command specified by index bool AP_Mission::set_current_cmd(uint16_t index) { // Clear flags _flags.in_landing_sequence = false; _flags.in_return_path = false; // read command to check for DO_LAND_START and DO_RETURN_PATH_START Mission_Command cmd; if (read_cmd_from_storage(index, cmd)) { if (cmd.id == MAV_CMD_DO_LAND_START) { _flags.in_landing_sequence = true; } else if (cmd.id == MAV_CMD_DO_RETURN_PATH_START) { _flags.in_return_path = true; } } // mission command has been set, don't track history. reset_wp_history(); // sanity check index and that we have a mission if (index >= (unsigned)_cmd_total || _cmd_total == 1) { return false; } // stop the current running do command _do_cmd.index = AP_MISSION_CMD_INDEX_NONE; _flags.do_cmd_loaded = false; _flags.do_cmd_all_done = false; // stop current nav cmd _flags.nav_cmd_loaded = false; // if index is zero then the user wants to completely restart the mission if (index == 0 || _flags.state == MISSION_COMPLETE) { _prev_nav_cmd_id = AP_MISSION_CMD_ID_NONE; _prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE; _prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE; // reset the jump tracking to zero init_jump_tracking(); if (index == 0) { index = 1; } } // if the mission is stopped or completed move the nav_cmd index to the specified point and set the state to stopped // so that if the user resumes the mission it will begin at the specified index if (_flags.state != MISSION_RUNNING) { // search until we find next nav command or reach end of command list while (!_flags.nav_cmd_loaded) { // get next command if (!get_next_cmd(index, cmd, true)) { _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE; return false; } // check if navigation or "do" command if (is_nav_cmd(cmd)) { // set current navigation command _nav_cmd = cmd; _flags.nav_cmd_loaded = true; } else { // set current do command if (!_flags.do_cmd_loaded) { _do_cmd = cmd; _flags.do_cmd_loaded = true; } } // move onto next command index = cmd.index+1; } // if we have not found a do command then set flag to show there are no do-commands to be run before nav command completes if (!_flags.do_cmd_loaded) { _flags.do_cmd_all_done = true; } // if we got this far then the mission can safely be "resumed" from the specified index so we set the state to "stopped" _flags.state = MISSION_STOPPED; return true; } // the state must be MISSION_RUNNING, allow advance_current_nav_cmd() to manage starting the item if (!advance_current_nav_cmd(index)) { // on failure set mission complete complete(); return false; } // if we got this far we must have successfully advanced the nav command return true; } // restart current navigation command. Used to handle external changes to mission // returns true on success, false if mission is not running or current nav command is invalid bool AP_Mission::restart_current_nav_cmd() { // return immediately if mission is not running if (_flags.state != MISSION_RUNNING) { return false; } // return immediately if nav command index is invalid const uint16_t nav_cmd_index = get_current_nav_index(); if ((nav_cmd_index == 0) || (nav_cmd_index >= num_commands())) { return false; } return set_current_cmd(_nav_cmd.index); } // returns false on any issue at all. bool AP_Mission::set_item(uint16_t index, mavlink_mission_item_int_t& src_packet) { // this is the on-storage format AP_Mission::Mission_Command cmd {}; // can't handle request for anything bigger than the mission size+1... if (index > num_commands()) { return false; } // convert from mavlink-ish format to storage format, if we can. if (mavlink_int_to_mission_cmd(src_packet, cmd) != MAV_MISSION_ACCEPTED) { return false; } // A request to set the 'next' item after the end is how we add an extra // item to the list, thus allowing us to write entire missions if needed. if (index == num_commands()) { return add_cmd(cmd); } // replacing an existing mission item... return AP_Mission::replace_cmd(index, cmd); } bool AP_Mission::get_item(uint16_t index, mavlink_mission_item_int_t& ret_packet) const { // setting ret_packet.command = -1 and/or returning false // means it contains invalid data after it leaves here. // this is the on-storage format AP_Mission::Mission_Command cmd {}; // can't handle request for anything bigger than the mission size... if (index >= num_commands()) { ret_packet.command = -1; return false; } // minimal placeholder values during read-from-storage ret_packet.target_system = 1; // unused sysid ret_packet.target_component = 1; // unused compid // 0=home, higher number/s = mission item number. ret_packet.seq = index; // retrieve mission from eeprom if (!read_cmd_from_storage(ret_packet.seq, cmd)) { ret_packet.command = -1; return false; } // convert into mavlink-ish format for lua and friends. if (!mission_cmd_to_mavlink_int(cmd, ret_packet)) { ret_packet.command = -1; return false; } // set packet's current field to 1 if this is the command being executed if (cmd.id == (uint16_t)get_current_nav_cmd().index) { ret_packet.current = 1; } else { ret_packet.current = 0; } // set auto continue to 1, becasue that's what's done elsewhere. ret_packet.autocontinue = 1; // 1 (true), 0 (false) ret_packet.command = cmd.id; return true; } struct PACKED Packed_Location_Option_Flags { uint8_t relative_alt : 1; // 1 if altitude is relative to home uint8_t unused1 : 1; // unused flag (defined so that loiter_ccw uses the correct bit) uint8_t loiter_ccw : 1; // 0 if clockwise, 1 if counter clockwise uint8_t terrain_alt : 1; // this altitude is above terrain uint8_t origin_alt : 1; // this altitude is above ekf origin uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location uint8_t type_specific_bit_0 : 1; // each mission item type can use this for storing 1 bit of extra data uint8_t type_specific_bit_1 : 1; // each mission item type can use this for storing 1 bit of extra data }; struct PACKED PackedLocation { union { Packed_Location_Option_Flags flags; ///< options bitmask (1<<0 = relative altitude) uint8_t options; /// allows writing all flags to eeprom as one byte }; // by making alt 24 bit we can make p1 in a command 16 bit, // allowing an accurate angle in centi-degrees. This keeps the // storage cost per mission item at 15 bytes, and allows mission // altitudes of up to +/- 83km int32_t alt:24; ///< param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M int32_t lat; ///< param 3 - Latitude * 10**7 int32_t lng; ///< param 4 - Longitude * 10**7 }; union PackedContent { // location PackedLocation location; // Waypoint location // raw bytes, for reading/writing to eeprom. Note that only 10 // bytes are available if a 16 bit command ID is used uint8_t bytes[12]; }; /// load_cmd_from_storage - load command from storage /// true is return if successful bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const { ASSERT_STORAGE_SIZE(PackedContent, 12); WITH_SEMAPHORE(_rsem); // special handling for command #0 which is home if (index == 0) { cmd = {}; cmd.id = MAV_CMD_NAV_WAYPOINT; cmd.content.location = AP::ahrs().get_home(); return true; } if (index >= (unsigned)_cmd_total || index >= _commands_max) { return false; } // ensure all bytes of cmd are zeroed cmd = {}; // Find out proper location in memory by using the start_byte position + the index // we can load a command, we don't process it yet // read WP position const uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE); PackedContent packed_content {}; const uint8_t b1 = _storage.read_byte(pos_in_storage); if (b1 == 0 || b1 == 1) { cmd.id = _storage.read_uint16(pos_in_storage+1); cmd.p1 = _storage.read_uint16(pos_in_storage+3); _storage.read_block(packed_content.bytes, pos_in_storage+5, 10); format_conversion(b1, cmd, packed_content); } else { cmd.id = b1; cmd.p1 = _storage.read_uint16(pos_in_storage+1); _storage.read_block(packed_content.bytes, pos_in_storage+3, 12); } if (stored_in_location(cmd.id)) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL // NOTE! no 16-bit command may be stored_in_location as only // 10 bytes are available for storage and lat/lon/alt required // 4*sizeof(float) == 12 bytes of storage. if (b1 == 0) { AP_HAL::panic("May not store location for 16-bit commands"); } #endif // Location is not PACKED; field-wise copy it: cmd.content.location.relative_alt = packed_content.location.flags.relative_alt; cmd.content.location.loiter_ccw = packed_content.location.flags.loiter_ccw; cmd.content.location.terrain_alt = packed_content.location.flags.terrain_alt; cmd.content.location.origin_alt = packed_content.location.flags.origin_alt; cmd.content.location.loiter_xtrack = packed_content.location.flags.loiter_xtrack; cmd.content.location.alt = packed_content.location.alt; cmd.content.location.lat = packed_content.location.lat; cmd.content.location.lng = packed_content.location.lng; if (packed_content.location.flags.type_specific_bit_0) { cmd.type_specific_bits |= 1U << 0; } if (packed_content.location.flags.type_specific_bit_1) { cmd.type_specific_bits |= 1U << 1; } } else { // all other options in Content are assumed to be packed: static_assert(sizeof(cmd.content) >= 12, "content is big enough to take bytes"); // (void *) cast to specify gcc that we know that we are copy byte into a non trivial type and leaving 4 bytes untouched memcpy((void *)&cmd.content, packed_content.bytes, 12); } // set command's index to it's position in eeprom cmd.index = index; // return success return true; } bool AP_Mission::stored_in_location(uint16_t id) { switch (id) { case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_NAV_LOITER_TIME: case MAV_CMD_NAV_LAND: case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: case MAV_CMD_NAV_LOITER_TO_ALT: case MAV_CMD_NAV_SPLINE_WAYPOINT: case MAV_CMD_NAV_GUIDED_ENABLE: case MAV_CMD_DO_SET_HOME: case MAV_CMD_DO_RETURN_PATH_START: case MAV_CMD_DO_LAND_START: case MAV_CMD_DO_GO_AROUND: case MAV_CMD_DO_SET_ROI: case MAV_CMD_NAV_VTOL_TAKEOFF: case MAV_CMD_NAV_VTOL_LAND: case MAV_CMD_NAV_PAYLOAD_PLACE: return true; default: return false; } } /// write_cmd_to_storage - write a command to storage /// index is used to calculate the storage location /// true is returned if successful bool AP_Mission::write_cmd_to_storage(uint16_t index, const Mission_Command& cmd) { WITH_SEMAPHORE(_rsem); // range check cmd's index if (index >= num_commands_max()) { return false; } PackedContent packed {}; if (stored_in_location(cmd.id)) { // Location is not PACKED; field-wise copy it: packed.location.flags.relative_alt = cmd.content.location.relative_alt; packed.location.flags.loiter_ccw = cmd.content.location.loiter_ccw; packed.location.flags.terrain_alt = cmd.content.location.terrain_alt; packed.location.flags.origin_alt = cmd.content.location.origin_alt; packed.location.flags.loiter_xtrack = cmd.content.location.loiter_xtrack; packed.location.flags.type_specific_bit_0 = (cmd.type_specific_bits & (1U<<0)) >> 0; packed.location.flags.type_specific_bit_1 = (cmd.type_specific_bits & (1U<<1)) >> 1; packed.location.alt = cmd.content.location.alt; packed.location.lat = cmd.content.location.lat; packed.location.lng = cmd.content.location.lng; } else { // all other options in Content are assumed to be packed: static_assert(sizeof(packed.bytes) >= 12, "packed.bytes is big enough to take content"); memcpy(packed.bytes, &cmd.content, 12); } // calculate where in storage the command should be placed uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE); if (cmd.id < 256) { // for commands below 256 we store up to 12 bytes _storage.write_byte(pos_in_storage, cmd.id); _storage.write_uint16(pos_in_storage+1, cmd.p1); _storage.write_block(pos_in_storage+3, packed.bytes, 12); } else { // if the command ID is above 256 we store a tag byte followed // by the 16 bit command ID. The tag byte is 1 for commands // where we have changed the storage format (see // format_conversion), 0 otherwise uint8_t tag_byte = 0; // currently the only converted structure is NAV_SCRIPT_TIME if (cmd.id == MAV_CMD_NAV_SCRIPT_TIME) { tag_byte = 1; } _storage.write_byte(pos_in_storage, tag_byte); _storage.write_uint16(pos_in_storage+1, cmd.id); _storage.write_uint16(pos_in_storage+3, cmd.p1); _storage.write_block(pos_in_storage+5, packed.bytes, 10); } // remember when the mission last changed _last_change_time_ms = AP_HAL::millis(); // return success return true; } /// write_home_to_storage - writes the special purpose cmd 0 (home) to storage /// home is taken directly from ahrs void AP_Mission::write_home_to_storage() { Mission_Command home_cmd = {}; home_cmd.id = MAV_CMD_NAV_WAYPOINT; home_cmd.content.location = AP::ahrs().get_home(); write_cmd_to_storage(0,home_cmd); } MAV_MISSION_RESULT AP_Mission::sanity_check_params(const mavlink_mission_item_int_t& packet) { uint8_t nan_mask; switch (packet.command) { case MAV_CMD_NAV_WAYPOINT: nan_mask = ~(1 << 3); // param 4 can be nan break; case MAV_CMD_NAV_LOITER_UNLIM: nan_mask = ~(1 << 3); // param 4 can be nan break; case MAV_CMD_NAV_LAND: nan_mask = ~(1 << 3); // param 4 can be nan break; case MAV_CMD_NAV_TAKEOFF: nan_mask = ~(1 << 3); // param 4 can be nan break; case MAV_CMD_NAV_VTOL_TAKEOFF: nan_mask = ~(1 << 3); // param 4 can be nan break; case MAV_CMD_NAV_VTOL_LAND: nan_mask = ~((1 << 2) | (1 << 3)); // param 3 and 4 can be nan break; default: nan_mask = 0xff; break; } if (((nan_mask & (1 << 0)) && isnan(packet.param1)) || isinf(packet.param1)) { return MAV_MISSION_INVALID_PARAM1; } if (((nan_mask & (1 << 1)) && isnan(packet.param2)) || isinf(packet.param2)) { return MAV_MISSION_INVALID_PARAM2; } if (((nan_mask & (1 << 2)) && isnan(packet.param3)) || isinf(packet.param3)) { return MAV_MISSION_INVALID_PARAM3; } if (((nan_mask & (1 << 3)) && isnan(packet.param4)) || isinf(packet.param4)) { return MAV_MISSION_INVALID_PARAM4; } return MAV_MISSION_ACCEPTED; } // mavlink_int_to_mission_cmd - converts mavlink message to an AP_Mission::Mission_Command object which can be stored to eeprom // return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_item_int_t& packet, AP_Mission::Mission_Command& cmd) { cmd = {}; // command's position in mission list and mavlink id cmd.index = packet.seq; cmd.id = packet.command; cmd.content.location = {}; MAV_MISSION_RESULT param_check = sanity_check_params(packet); if (param_check != MAV_MISSION_ACCEPTED) { return param_check; } // command specific conversions from mavlink packet to mission command switch (cmd.id) { case 0 ... 1: // these are reserved for storing 16 bit command IDs return MAV_MISSION_INVALID; case MAV_CMD_NAV_WAYPOINT: { // MAV ID: 16 /* the 15 byte limit means we can't fit both delay and radius in the cmd structure. When we expand the mission structure we can do this properly */ #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) // acceptance radius in meters and pass by distance in meters uint16_t acp = packet.param2; // param 2 is acceptance radius in meters is held in low p1 uint16_t passby = packet.param3; // param 3 is pass by distance in meters is held in high p1 // limit to 255 so it does not wrap during the shift or mask operation passby = MIN(0xFF,passby); acp = MIN(0xFF,acp); cmd.p1 = (passby << 8) | (acp & 0x00FF); #else // delay at waypoint in seconds (this is for copters???) cmd.p1 = packet.param1; #endif } break; case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17 cmd.p1 = fabsf(packet.param3); // store radius as 16bit since no other params are competing for space cmd.content.location.loiter_ccw = (packet.param3 < 0); // -1 = counter clockwise, +1 = clockwise break; case MAV_CMD_NAV_LOITER_TURNS: { // MAV ID: 18 // number of turns is stored in the lowest bits. Number of // turns 0 < N < 1 are stored multiplied by 256 and a bit set // in storage so that on retrieval they are divided by 256. // Radii below 255m are stored in the top 8 bits as an 8-bit integer. // Radii above 255m are stored divided by 10 and a bit set in // storage so that on retrieval they are multiplied by 10 float param1_stored = packet.param1; if (param1_stored > 0 && param1_stored < 1) { param1_stored *= 256.0; cmd.type_specific_bits |= (1U << 1); } cmd.p1 = MIN(255, param1_stored); // store number of times to circle in low p1 uint8_t radius_m; const float abs_radius = fabsf(packet.param3); if (abs_radius <= 255) { radius_m = abs_radius; } else { radius_m = MIN(255, abs_radius * 0.1); cmd.type_specific_bits |= (1U << 0); } cmd.p1 |= (radius_m<<8); // store radius in high byte of p1 cmd.content.location.loiter_ccw = (packet.param3 < 0); cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location } break; case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19 cmd.p1 = packet.param1; // loiter time in seconds uses all 16 bits, 8bit seconds is too small. No room for radius. cmd.content.location.loiter_ccw = (packet.param3 < 0); cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20 break; case MAV_CMD_NAV_LAND: // MAV ID: 21 cmd.p1 = packet.param1; // abort target altitude(m) (plane only) if (!isnan(packet.param4)) { cmd.content.location.loiter_ccw = is_negative(packet.param4); // yaw direction, (plane deepstall only) } break; case MAV_CMD_NAV_TAKEOFF: // MAV ID: 22 cmd.p1 = packet.param1; // minimum pitch (plane only) break; case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: // MAV ID: 30 cmd.p1 = packet.param1; // Climb/Descend // 0 = Neutral, cmd complete at +/- 5 of indicated alt. // 1 = Climb, cmd complete at or above indicated alt. // 2 = Descend, cmd complete at or below indicated alt. break; case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31 cmd.p1 = fabsf(packet.param2); // param2 is radius in meters cmd.content.location.loiter_ccw = (packet.param2 < 0); cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82 #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) return MAV_MISSION_UNSUPPORTED; #else cmd.p1 = packet.param1; // delay at waypoint in seconds break; #endif case MAV_CMD_NAV_GUIDED_ENABLE: // MAV ID: 92 cmd.p1 = packet.param1; // on/off. >0.5 means "on", hand-over control to external controller break; case MAV_CMD_NAV_DELAY: // MAV ID: 93 cmd.content.nav_delay.seconds = packet.param1; // delay in seconds cmd.content.nav_delay.hour_utc = packet.param2;// absolute time's hour (utc) cmd.content.nav_delay.min_utc = packet.param3;// absolute time's min (utc) cmd.content.nav_delay.sec_utc = packet.param4; // absolute time's second (utc) break; case MAV_CMD_CONDITION_DELAY: // MAV ID: 112 cmd.content.delay.seconds = packet.param1; // delay in seconds break; case MAV_CMD_CONDITION_DISTANCE: // MAV ID: 114 cmd.content.distance.meters = packet.param1; // distance in meters from next waypoint break; case MAV_CMD_CONDITION_YAW: // MAV ID: 115 cmd.content.yaw.angle_deg = packet.param1; // target angle in degrees cmd.content.yaw.turn_rate_dps = packet.param2; // 0 = use default turn rate otherwise specific turn rate in deg/sec cmd.content.yaw.direction = packet.param3; // -1 = ccw, +1 = cw cmd.content.yaw.relative_angle = packet.param4; // lng=0: absolute angle provided, lng=1: relative angle provided break; case MAV_CMD_DO_JUMP: // MAV ID: 177 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 cmd.content.speed.throttle_pct = packet.param3; // throttle as a percentage from 1 ~ 100% break; case MAV_CMD_DO_SET_HOME: cmd.p1 = packet.param1; // p1=0 means use current location, p=1 means use provided location break; case MAV_CMD_DO_SET_RELAY: // MAV ID: 181 cmd.content.relay.num = packet.param1; // relay number cmd.content.relay.state = packet.param2; // 0:off, 1:on break; case MAV_CMD_DO_REPEAT_RELAY: // MAV ID: 182 cmd.content.repeat_relay.num = packet.param1; // relay number cmd.content.repeat_relay.repeat_count = packet.param2; // count cmd.content.repeat_relay.cycle_time = packet.param3; // time converted from seconds to milliseconds break; case MAV_CMD_DO_SET_SERVO: // MAV ID: 183 cmd.content.servo.channel = packet.param1; // channel cmd.content.servo.pwm = packet.param2; // PWM break; case MAV_CMD_DO_REPEAT_SERVO: // MAV ID: 184 cmd.content.repeat_servo.channel = packet.param1; // channel cmd.content.repeat_servo.pwm = packet.param2; // PWM cmd.content.repeat_servo.repeat_count = packet.param3; // count cmd.content.repeat_servo.cycle_time = packet.param4; // time in seconds break; case MAV_CMD_DO_RETURN_PATH_START: // MAV ID: 188 case MAV_CMD_DO_LAND_START: // MAV ID: 189 break; case MAV_CMD_DO_GO_AROUND: // MAV ID: 191 break; case MAV_CMD_DO_SET_ROI: // MAV ID: 201 cmd.p1 = packet.param1; // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported) break; case MAV_CMD_DO_DIGICAM_CONFIGURE: // MAV ID: 202 cmd.content.digicam_configure.shooting_mode = packet.param1; cmd.content.digicam_configure.shutter_speed = packet.param2; cmd.content.digicam_configure.aperture = packet.param3; cmd.content.digicam_configure.ISO = packet.param4; cmd.content.digicam_configure.exposure_type = packet.x; cmd.content.digicam_configure.cmd_id = packet.y; cmd.content.digicam_configure.engine_cutoff_time = packet.z; break; case MAV_CMD_DO_DIGICAM_CONTROL: // MAV ID: 203 cmd.content.digicam_control.session = packet.param1; cmd.content.digicam_control.zoom_pos = packet.param2; cmd.content.digicam_control.zoom_step = packet.param3; cmd.content.digicam_control.focus_lock = packet.param4; cmd.content.digicam_control.shooting_cmd = packet.x; cmd.content.digicam_control.cmd_id = packet.y; break; case MAV_CMD_DO_MOUNT_CONTROL: // MAV ID: 205 cmd.content.mount_control.pitch = packet.param1; cmd.content.mount_control.roll = packet.param2; cmd.content.mount_control.yaw = packet.param3; break; case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206 cmd.content.cam_trigg_dist.meters = packet.param1; // distance between camera shots in meters cmd.content.cam_trigg_dist.trigger = packet.param3; // when enabled, camera triggers once immediately break; case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207 cmd.p1 = packet.param1; // action 0=disable, 1=enable break; case MAV_CMD_DO_AUX_FUNCTION: cmd.content.auxfunction.function = packet.param1; cmd.content.auxfunction.switchpos = packet.param2; break; case MAV_CMD_DO_PARACHUTE: // MAV ID: 208 cmd.p1 = packet.param1; // action 0=disable, 1=enable, 2=release. See PARACHUTE_ACTION enum break; case MAV_CMD_DO_INVERTED_FLIGHT: // MAV ID: 210 cmd.p1 = packet.param1; // normal=0 inverted=1 break; #if AP_GRIPPER_ENABLED case MAV_CMD_DO_GRIPPER: // MAV ID: 211 cmd.content.gripper.num = packet.param1; // gripper number cmd.content.gripper.action = packet.param2; // action 0=release, 1=grab. See GRIPPER_ACTION enum break; #endif case MAV_CMD_DO_GUIDED_LIMITS: // MAV ID: 222 cmd.p1 = packet.param1; // max time in seconds the external controller will be allowed to control the vehicle cmd.content.guided_limits.alt_min = packet.param2; // min alt below which the command will be aborted. 0 for no lower alt limit cmd.content.guided_limits.alt_max = packet.param3; // max alt above which the command will be aborted. 0 for no upper alt limit cmd.content.guided_limits.horiz_max = packet.param4;// max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit break; case MAV_CMD_DO_AUTOTUNE_ENABLE: // MAV ID: 211 cmd.p1 = packet.param1; // disable=0 enable=1 break; case MAV_CMD_NAV_ALTITUDE_WAIT: // MAV ID: 83 cmd.content.altitude_wait.altitude = packet.param1; cmd.content.altitude_wait.descent_rate = packet.param2; cmd.content.altitude_wait.wiggle_time = packet.param3; break; case MAV_CMD_NAV_VTOL_TAKEOFF: break; case MAV_CMD_NAV_VTOL_LAND: cmd.p1 = (NAV_VTOL_LAND_OPTIONS)packet.param1; break; case MAV_CMD_DO_VTOL_TRANSITION: cmd.content.do_vtol_transition.target_state = packet.param1; break; case MAV_CMD_DO_SET_REVERSE: cmd.p1 = packet.param1; // 0 = forward, 1 = reverse break; case MAV_CMD_DO_ENGINE_CONTROL: cmd.content.do_engine_control.start_control = (packet.param1>0); cmd.content.do_engine_control.cold_start = (packet.param2>0); cmd.content.do_engine_control.height_delay_cm = packet.param3*100; cmd.content.do_engine_control.allow_disarmed_start = (((uint32_t)packet.param4) & ENGINE_CONTROL_OPTIONS_ALLOW_START_WHILE_DISARMED) != 0; break; #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: cmd.p1 = packet.param1*100; // copy max-descend parameter (m->cm) break; #endif case MAV_CMD_NAV_SET_YAW_SPEED: cmd.content.set_yaw_speed.angle_deg = packet.param1; // target angle in degrees cmd.content.set_yaw_speed.speed = packet.param2; // speed in meters/second cmd.content.set_yaw_speed.relative_angle = packet.param3; // 0 = absolute angle, 1 = relative angle break; case MAV_CMD_DO_WINCH: // MAV ID: 42600 cmd.content.winch.num = packet.param1; // winch number cmd.content.winch.action = packet.param2; // action (0 = relax, 1 = length control, 2 = rate control). See WINCH_ACTION enum cmd.content.winch.release_length = packet.param3; // cable distance to unwind in meters, negative numbers to wind in cable cmd.content.winch.release_rate = packet.param4; // release rate in meters/second break; case MAV_CMD_DO_SET_RESUME_REPEAT_DIST: cmd.p1 = packet.param1; // Resume repeat distance (m) break; case MAV_CMD_DO_SPRAYER: cmd.p1 = packet.param1; // action 0=disable, 1=enable break; case MAV_CMD_DO_SEND_SCRIPT_MESSAGE: cmd.p1 = packet.param1; cmd.content.scripting.p1 = packet.param2; cmd.content.scripting.p2 = packet.param3; cmd.content.scripting.p3 = packet.param4; break; #if AP_SCRIPTING_ENABLED case MAV_CMD_NAV_SCRIPT_TIME: cmd.content.nav_script_time.command = packet.param1; cmd.content.nav_script_time.timeout_s = packet.param2; cmd.content.nav_script_time.arg1.set(packet.param3); cmd.content.nav_script_time.arg2.set(packet.param4); cmd.content.nav_script_time.arg3 = int16_t(packet.x); cmd.content.nav_script_time.arg4 = int16_t(packet.y); break; #endif case MAV_CMD_NAV_ATTITUDE_TIME: cmd.content.nav_attitude_time.time_sec = constrain_float(packet.param1, 0, UINT16_MAX); cmd.content.nav_attitude_time.roll_deg = (fabsf(packet.param2) <= 180) ? packet.param2 : 0; cmd.content.nav_attitude_time.pitch_deg = (fabsf(packet.param3) <= 90) ? packet.param3 : 0; cmd.content.nav_attitude_time.yaw_deg = ((packet.param4 >= -180) && (packet.param4 <= 360)) ? packet.param4 : 0; cmd.content.nav_attitude_time.climb_rate = packet.x; break; case MAV_CMD_DO_PAUSE_CONTINUE: cmd.p1 = packet.param1; break; case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg = packet.param1; cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg = packet.param2; cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs = packet.param3; cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs = packet.param4; cmd.content.gimbal_manager_pitchyaw.flags = packet.x; cmd.content.gimbal_manager_pitchyaw.gimbal_id = packet.z; break; case MAV_CMD_IMAGE_START_CAPTURE: cmd.content.image_start_capture.instance = packet.param1; cmd.content.image_start_capture.interval_s = packet.param2; cmd.content.image_start_capture.total_num_images = packet.param3; cmd.content.image_start_capture.start_seq_number = packet.param4; break; case MAV_CMD_IMAGE_STOP_CAPTURE: cmd.p1 = packet.param1; break; case MAV_CMD_SET_CAMERA_ZOOM: cmd.content.set_camera_zoom.zoom_type = packet.param1; cmd.content.set_camera_zoom.zoom_value = packet.param2; break; case MAV_CMD_SET_CAMERA_FOCUS: cmd.content.set_camera_focus.focus_type = packet.param1; cmd.content.set_camera_focus.focus_value = packet.param2; break; case MAV_CMD_SET_CAMERA_SOURCE: cmd.content.set_camera_source.instance = packet.param1; cmd.content.set_camera_source.primary_source = packet.param2; cmd.content.set_camera_source.secondary_source = packet.param3; break; case MAV_CMD_VIDEO_START_CAPTURE: cmd.content.video_start_capture.video_stream_id = packet.param1; break; case MAV_CMD_VIDEO_STOP_CAPTURE: cmd.content.video_stop_capture.video_stream_id = packet.param1; break; default: // unrecognised command return MAV_MISSION_UNSUPPORTED; } // copy location from mavlink to command if (stored_in_location(cmd.id)) { // sanity check location if (!check_lat(packet.x)) { return MAV_MISSION_INVALID_PARAM5_X; } if (!check_lng(packet.y)) { return MAV_MISSION_INVALID_PARAM6_Y; } if (isnan(packet.z) || fabsf(packet.z) >= LOCATION_ALT_MAX_M) { return MAV_MISSION_INVALID_PARAM7; } cmd.content.location.lat = packet.x; cmd.content.location.lng = packet.y; cmd.content.location.alt = packet.z * 100.0f; // convert packet's alt (m) to cmd alt (cm) switch (packet.frame) { case MAV_FRAME_MISSION: case MAV_FRAME_GLOBAL: case MAV_FRAME_GLOBAL_INT: cmd.content.location.relative_alt = 0; break; case MAV_FRAME_GLOBAL_RELATIVE_ALT: case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: cmd.content.location.relative_alt = 1; break; #if AP_TERRAIN_AVAILABLE case MAV_FRAME_GLOBAL_TERRAIN_ALT: case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: // we mark it as a relative altitude, as it doesn't have // home alt added cmd.content.location.relative_alt = 1; // mark altitude as above terrain, not above home cmd.content.location.terrain_alt = 1; break; #endif default: return MAV_MISSION_UNSUPPORTED_FRAME; } } // if we got this far then it must have been successful return MAV_MISSION_ACCEPTED; } MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(const mavlink_mission_item_t &packet, mavlink_mission_item_int_t &mav_cmd) { // TODO: rename mav_cmd to mission_item_int // TODO: rename packet to mission_item mav_cmd.param1 = packet.param1; mav_cmd.param2 = packet.param2; mav_cmd.param3 = packet.param3; mav_cmd.param4 = packet.param4; mav_cmd.z = packet.z; mav_cmd.seq = packet.seq; mav_cmd.command = packet.command; mav_cmd.target_system = packet.target_system; mav_cmd.target_component = packet.target_component; mav_cmd.frame = packet.frame; mav_cmd.current = packet.current; mav_cmd.autocontinue = packet.autocontinue; mav_cmd.mission_type = packet.mission_type; /* the strategy for handling both MISSION_ITEM and MISSION_ITEM_INT is to pass the lat/lng in MISSION_ITEM_INT straight through, and for MISSION_ITEM multiply by 1e7 here. We need an exception for any commands which use the x and y fields not as latitude/longitude. */ if (!cmd_has_location(packet.command)) { mav_cmd.x = packet.x; mav_cmd.y = packet.y; } else { //these commands use x and y as lat/lon. We need to // multiply by 1e7 to convert to int32_t if (!check_lat(packet.x)) { return MAV_MISSION_INVALID_PARAM5_X; } if (!check_lng(packet.y)) { return MAV_MISSION_INVALID_PARAM6_Y; } mav_cmd.x = packet.x * 1.0e7f; mav_cmd.y = packet.y * 1.0e7f; } return MAV_MISSION_ACCEPTED; } MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(const mavlink_mission_item_int_t &item_int, mavlink_mission_item_t &item) { item.param1 = item_int.param1; item.param2 = item_int.param2; item.param3 = item_int.param3; item.param4 = item_int.param4; item.z = item_int.z; item.seq = item_int.seq; item.command = item_int.command; item.target_system = item_int.target_system; item.target_component = item_int.target_component; item.frame = item_int.frame; item.current = item_int.current; item.autocontinue = item_int.autocontinue; item.mission_type = item_int.mission_type; if (!cmd_has_location(item_int.command)) { item.x = item_int.x; item.y = item_int.y; } else { // These commands use x and y as lat/lon. We need to // multiply by 1e-7 to convert to float item.x = item_int.x * 1.0e-7f; item.y = item_int.y * 1.0e-7f; if (!check_lat(item.x)) { return MAV_MISSION_INVALID_PARAM5_X; } if (!check_lng(item.y)) { return MAV_MISSION_INVALID_PARAM6_Y; } } return MAV_MISSION_ACCEPTED; } // mission_cmd_to_mavlink_int - converts an AP_Mission::Mission_Command object to a mavlink message which can be sent to the GCS // return true on success, false on failure // NOTE: callers to this method current fill parts of "packet" in before calling this method, so do NOT attempt to zero the entire packet in here bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& cmd, mavlink_mission_item_int_t& packet) { // command's position in mission list and mavlink id packet.seq = cmd.index; packet.command = cmd.id; // set defaults packet.current = 0; // 1 if we are passing back the mission command that is currently being executed packet.param1 = 0; packet.param2 = 0; packet.param3 = 0; packet.param4 = 0; packet.frame = 0; packet.autocontinue = 1; // command specific conversions from mission command to mavlink packet switch (cmd.id) { case 0: // this is reserved for 16 bit command IDs return false; case MAV_CMD_NAV_WAYPOINT: // MAV ID: 16 #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) // acceptance radius in meters packet.param2 = LOWBYTE(cmd.p1); // param 2 is acceptance radius in meters is held in low p1 packet.param3 = HIGHBYTE(cmd.p1); // param 3 is pass by distance in meters is held in high p1 #else // delay at waypoint in seconds packet.param1 = cmd.p1; #endif break; case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17 packet.param3 = (float)cmd.p1; if (cmd.content.location.loiter_ccw) { packet.param3 *= -1; } break; case MAV_CMD_NAV_LOITER_TURNS: // MAV ID: 18 packet.param1 = LOWBYTE(cmd.p1); // number of times to circle is held in low byte of p1 packet.param3 = HIGHBYTE(cmd.p1); // radius is held in high byte of p1 if (cmd.content.location.loiter_ccw) { packet.param3 = -packet.param3; } if (cmd.type_specific_bits & (1U<<0)) { packet.param3 *= 10; } if (cmd.type_specific_bits & (1U<<1)) { packet.param1 /= 256.0; } packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19 packet.param1 = cmd.p1; // loiter time in seconds if (cmd.content.location.loiter_ccw) { packet.param3 = -1; } else { packet.param3 = 1; } packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20 break; case MAV_CMD_NAV_LAND: // MAV ID: 21 packet.param1 = cmd.p1; // abort target altitude(m) (plane only) packet.param4 = cmd.content.location.loiter_ccw ? -1 : 1; // yaw direction, (plane deepstall only) break; case MAV_CMD_NAV_TAKEOFF: // MAV ID: 22 packet.param1 = cmd.p1; // minimum pitch (plane only) break; case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: // MAV ID: 30 packet.param1 = cmd.p1; // Climb/Descend // 0 = Neutral, cmd complete at +/- 5 of indicated alt. // 1 = Climb, cmd complete at or above indicated alt. // 2 = Descend, cmd complete at or below indicated alt. break; case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31 packet.param2 = cmd.p1; // loiter radius(m) if (cmd.content.location.loiter_ccw) { packet.param2 = -packet.param2; } packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82 packet.param1 = cmd.p1; // delay at waypoint in seconds break; case MAV_CMD_NAV_GUIDED_ENABLE: // MAV ID: 92 packet.param1 = cmd.p1; // on/off. >0.5 means "on", hand-over control to external controller break; case MAV_CMD_NAV_DELAY: // MAV ID: 93 packet.param1 = cmd.content.nav_delay.seconds; // delay in seconds packet.param2 = cmd.content.nav_delay.hour_utc; // absolute time's day of week (utc) packet.param3 = cmd.content.nav_delay.min_utc; // absolute time's hour (utc) packet.param4 = cmd.content.nav_delay.sec_utc; // absolute time's min (utc) break; case MAV_CMD_CONDITION_DELAY: // MAV ID: 112 packet.param1 = cmd.content.delay.seconds; // delay in seconds break; case MAV_CMD_CONDITION_DISTANCE: // MAV ID: 114 packet.param1 = cmd.content.distance.meters; // distance in meters from next waypoint break; case MAV_CMD_CONDITION_YAW: // MAV ID: 115 packet.param1 = cmd.content.yaw.angle_deg; // target angle in degrees packet.param2 = cmd.content.yaw.turn_rate_dps; // 0 = use default turn rate otherwise specific turn rate in deg/sec packet.param3 = cmd.content.yaw.direction; // -1 = ccw, +1 = cw packet.param4 = cmd.content.yaw.relative_angle; // 0 = absolute angle provided, 1 = relative angle provided break; case MAV_CMD_DO_JUMP: // MAV ID: 177 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 packet.param3 = cmd.content.speed.throttle_pct; // throttle as a percentage from 1 ~ 100% break; case MAV_CMD_DO_SET_HOME: // MAV ID: 179 packet.param1 = cmd.p1; // p1=0 means use current location, p=1 means use provided location break; case MAV_CMD_DO_SET_RELAY: // MAV ID: 181 packet.param1 = cmd.content.relay.num; // relay number packet.param2 = cmd.content.relay.state; // 0:off, 1:on break; case MAV_CMD_DO_REPEAT_RELAY: // MAV ID: 182 packet.param1 = cmd.content.repeat_relay.num; // relay number packet.param2 = cmd.content.repeat_relay.repeat_count; // count packet.param3 = cmd.content.repeat_relay.cycle_time; // time in seconds break; case MAV_CMD_DO_SET_SERVO: // MAV ID: 183 packet.param1 = cmd.content.servo.channel; // channel packet.param2 = cmd.content.servo.pwm; // PWM break; case MAV_CMD_DO_REPEAT_SERVO: // MAV ID: 184 packet.param1 = cmd.content.repeat_servo.channel; // channel packet.param2 = cmd.content.repeat_servo.pwm; // PWM packet.param3 = cmd.content.repeat_servo.repeat_count; // count packet.param4 = cmd.content.repeat_servo.cycle_time; // time in milliseconds converted to seconds break; case MAV_CMD_DO_RETURN_PATH_START: // MAV ID: 188 case MAV_CMD_DO_LAND_START: // MAV ID: 189 break; case MAV_CMD_DO_GO_AROUND: // MAV ID: 191 break; case MAV_CMD_DO_SET_ROI: // MAV ID: 201 packet.param1 = cmd.p1; // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported) break; case MAV_CMD_DO_DIGICAM_CONFIGURE: // MAV ID: 202 packet.param1 = cmd.content.digicam_configure.shooting_mode; packet.param2 = cmd.content.digicam_configure.shutter_speed; packet.param3 = cmd.content.digicam_configure.aperture; packet.param4 = cmd.content.digicam_configure.ISO; packet.x = cmd.content.digicam_configure.exposure_type; packet.y = cmd.content.digicam_configure.cmd_id; packet.z = cmd.content.digicam_configure.engine_cutoff_time; break; case MAV_CMD_DO_DIGICAM_CONTROL: // MAV ID: 203 packet.param1 = cmd.content.digicam_control.session; packet.param2 = cmd.content.digicam_control.zoom_pos; packet.param3 = cmd.content.digicam_control.zoom_step; packet.param4 = cmd.content.digicam_control.focus_lock; packet.x = cmd.content.digicam_control.shooting_cmd; packet.y = cmd.content.digicam_control.cmd_id; break; case MAV_CMD_DO_MOUNT_CONTROL: // MAV ID: 205 packet.param1 = cmd.content.mount_control.pitch; packet.param2 = cmd.content.mount_control.roll; packet.param3 = cmd.content.mount_control.yaw; break; case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206 packet.param1 = cmd.content.cam_trigg_dist.meters; // distance between camera shots in meters packet.param3 = cmd.content.cam_trigg_dist.trigger; // when enabled, camera triggers once immediately break; case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207 packet.param1 = cmd.p1; // action 0=disable, 1=enable break; case MAV_CMD_DO_PARACHUTE: // MAV ID: 208 packet.param1 = cmd.p1; // action 0=disable, 1=enable, 2=release. See PARACHUTE_ACTION enum break; case MAV_CMD_DO_SPRAYER: packet.param1 = cmd.p1; // action 0=disable, 1=enable break; case MAV_CMD_DO_AUX_FUNCTION: packet.param1 = cmd.content.auxfunction.function; packet.param2 = cmd.content.auxfunction.switchpos; break; case MAV_CMD_DO_INVERTED_FLIGHT: // MAV ID: 210 packet.param1 = cmd.p1; // normal=0 inverted=1 break; #if AP_GRIPPER_ENABLED case MAV_CMD_DO_GRIPPER: // MAV ID: 211 packet.param1 = cmd.content.gripper.num; // gripper number packet.param2 = cmd.content.gripper.action; // action 0=release, 1=grab. See GRIPPER_ACTION enum break; #endif case MAV_CMD_DO_GUIDED_LIMITS: // MAV ID: 222 packet.param1 = cmd.p1; // max time in seconds the external controller will be allowed to control the vehicle packet.param2 = cmd.content.guided_limits.alt_min; // min alt below which the command will be aborted. 0 for no lower alt limit packet.param3 = cmd.content.guided_limits.alt_max; // max alt above which the command will be aborted. 0 for no upper alt limit packet.param4 = cmd.content.guided_limits.horiz_max;// max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit break; case MAV_CMD_DO_AUTOTUNE_ENABLE: packet.param1 = cmd.p1; // disable=0 enable=1 break; case MAV_CMD_DO_SET_REVERSE: packet.param1 = cmd.p1; // 0 = forward, 1 = reverse break; case MAV_CMD_NAV_ALTITUDE_WAIT: // MAV ID: 83 packet.param1 = cmd.content.altitude_wait.altitude; packet.param2 = cmd.content.altitude_wait.descent_rate; packet.param3 = cmd.content.altitude_wait.wiggle_time; break; case MAV_CMD_NAV_VTOL_TAKEOFF: break; case MAV_CMD_NAV_VTOL_LAND: packet.param1 = cmd.p1; break; case MAV_CMD_DO_VTOL_TRANSITION: packet.param1 = cmd.content.do_vtol_transition.target_state; break; case MAV_CMD_DO_ENGINE_CONTROL: packet.param1 = cmd.content.do_engine_control.start_control?1:0; packet.param2 = cmd.content.do_engine_control.cold_start?1:0; packet.param3 = cmd.content.do_engine_control.height_delay_cm*0.01f; if (cmd.content.do_engine_control.allow_disarmed_start) { packet.param4 = ENGINE_CONTROL_OPTIONS_ALLOW_START_WHILE_DISARMED; } break; #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: packet.param1 = cmd.p1/100.0f; // copy max-descend parameter (cm->m) break; #endif case MAV_CMD_NAV_SET_YAW_SPEED: packet.param1 = cmd.content.set_yaw_speed.angle_deg; // target angle in degrees packet.param2 = cmd.content.set_yaw_speed.speed; // speed in meters/second packet.param3 = cmd.content.set_yaw_speed.relative_angle; // 0 = absolute angle, 1 = relative angle break; case MAV_CMD_DO_WINCH: packet.param1 = cmd.content.winch.num; // winch number packet.param2 = cmd.content.winch.action; // action (0 = relax, 1 = length control, 2 = rate control). See WINCH_ACTION enum packet.param3 = cmd.content.winch.release_length; // cable distance to unwind in meters, negative numbers to wind in cable packet.param4 = cmd.content.winch.release_rate; // release rate in meters/second break; case MAV_CMD_DO_SET_RESUME_REPEAT_DIST: packet.param1 = cmd.p1; // Resume repeat distance (m) break; case MAV_CMD_DO_SEND_SCRIPT_MESSAGE: packet.param1 = cmd.p1; packet.param2 = cmd.content.scripting.p1; packet.param3 = cmd.content.scripting.p2; packet.param4 = cmd.content.scripting.p3; break; #if AP_SCRIPTING_ENABLED case MAV_CMD_NAV_SCRIPT_TIME: packet.param1 = cmd.content.nav_script_time.command; packet.param2 = cmd.content.nav_script_time.timeout_s; packet.param3 = cmd.content.nav_script_time.arg1.get(); packet.param4 = cmd.content.nav_script_time.arg2.get(); packet.x = cmd.content.nav_script_time.arg3; packet.y = cmd.content.nav_script_time.arg4; break; #endif case MAV_CMD_NAV_ATTITUDE_TIME: packet.param1 = cmd.content.nav_attitude_time.time_sec; packet.param2 = cmd.content.nav_attitude_time.roll_deg; packet.param3 = cmd.content.nav_attitude_time.pitch_deg; packet.param4 = cmd.content.nav_attitude_time.yaw_deg; packet.x = cmd.content.nav_attitude_time.climb_rate; break; case MAV_CMD_DO_PAUSE_CONTINUE: packet.param1 = cmd.p1; break; case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: packet.param1 = cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg; packet.param2 = cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg; packet.param3 = cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs; packet.param4 = cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs; packet.x = cmd.content.gimbal_manager_pitchyaw.flags; packet.z = cmd.content.gimbal_manager_pitchyaw.gimbal_id; break; case MAV_CMD_IMAGE_START_CAPTURE: packet.param1 = cmd.content.image_start_capture.instance; packet.param2 = cmd.content.image_start_capture.interval_s; packet.param3 = cmd.content.image_start_capture.total_num_images; packet.param4 = cmd.content.image_start_capture.start_seq_number; break; case MAV_CMD_IMAGE_STOP_CAPTURE: packet.param1 = cmd.p1; break; case MAV_CMD_SET_CAMERA_ZOOM: packet.param1 = cmd.content.set_camera_zoom.zoom_type; packet.param2 = cmd.content.set_camera_zoom.zoom_value; break; case MAV_CMD_SET_CAMERA_FOCUS: packet.param1 = cmd.content.set_camera_focus.focus_type; packet.param2 = cmd.content.set_camera_focus.focus_value; break; case MAV_CMD_SET_CAMERA_SOURCE: packet.param1 = cmd.content.set_camera_source.instance; packet.param2 = cmd.content.set_camera_source.primary_source; packet.param3 = cmd.content.set_camera_source.secondary_source; break; case MAV_CMD_VIDEO_START_CAPTURE: packet.param1 = cmd.content.video_start_capture.video_stream_id; break; case MAV_CMD_VIDEO_STOP_CAPTURE: packet.param1 = cmd.content.video_stop_capture.video_stream_id; break; default: // unrecognised command return false; } // copy location from mavlink to command if (stored_in_location(cmd.id)) { packet.x = cmd.content.location.lat; packet.y = cmd.content.location.lng; packet.z = cmd.content.location.alt * 0.01f; // cmd alt in cm to m if (cmd.content.location.relative_alt) { packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } else { packet.frame = MAV_FRAME_GLOBAL; } #if AP_TERRAIN_AVAILABLE if (cmd.content.location.terrain_alt) { // this is a above-terrain altitude if (!cmd.content.location.relative_alt) { // refuse to return non-relative terrain mission // items. Internally we do have these, and they // have home.alt added, but we should never be // returning them to the GCS, as the GCS doesn't know // our home.alt, so it would have no way to properly // interpret it return false; } packet.z = cmd.content.location.alt * 0.01f; packet.frame = MAV_FRAME_GLOBAL_TERRAIN_ALT; } #else // don't ever return terrain mission items if no terrain support if (cmd.content.location.terrain_alt) { return false; } #endif } // if we got this far then it must have been successful return true; } /// /// private methods /// /// complete - mission is marked complete and clean-up performed including calling the mission_complete_fn void AP_Mission::complete() { // flag mission as complete _flags.state = MISSION_COMPLETE; _flags.in_landing_sequence = false; _flags.in_return_path = false; // callback to main program's mission complete function _mission_complete_fn(); } /// advance_current_nav_cmd - moves current nav command forward /// do command will also be loaded /// accounts for do-jump commands // returns true if command is advanced, false if failed (i.e. mission completed) bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index) { // exit immediately if we're not running if (_flags.state != MISSION_RUNNING) { return false; } // exit immediately if current nav command has not completed if (_flags.nav_cmd_loaded) { return false; } // stop the current running do command _do_cmd.index = AP_MISSION_CMD_INDEX_NONE; _flags.do_cmd_loaded = false; _flags.do_cmd_all_done = false; // get starting point for search uint16_t cmd_index = starting_index > 0 ? starting_index - 1 : _nav_cmd.index; if (cmd_index == AP_MISSION_CMD_INDEX_NONE) { // start from beginning of the mission command list cmd_index = AP_MISSION_FIRST_REAL_COMMAND; } else { // start from one position past the current nav command cmd_index++; } // avoid endless loops uint8_t max_loops = 255; // search until we find next nav command or reach end of command list while (!_flags.nav_cmd_loaded && max_loops-- > 0) { // get next command Mission_Command cmd; if (!get_next_cmd(cmd_index, cmd, true)) { return false; } // check if navigation or "do" command if (is_nav_cmd(cmd)) { // 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 (!(cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) { _prev_nav_cmd_wp_index = _nav_cmd.index; } // set current navigation command and start it _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 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= (unsigned)_cmd_total) || (temp_cmd.content.jump.target == 0)) { // To-Do: log an error? return false; } // check for endless loops if (!increment_jump_num_times_if_found && jump_index == cmd_index) { // we have somehow reached this jump command twice and there is no chance it will complete // To-Do: log an error? return false; } // record this command so we can check for endless loops if (jump_index == AP_MISSION_CMD_INDEX_NONE) { jump_index = cmd_index; } // get number of times jump command has already been run if (temp_cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER || get_jump_times_run(temp_cmd) < temp_cmd.content.jump.num_times) { // update the record of the number of times run if (increment_jump_num_times_if_found && !_flags.resuming_mission) { increment_jump_times_run(temp_cmd, send_gcs_msg); } // continue searching from jump target cmd_index = temp_cmd.content.jump.target; } else { // jump has been run specified number of times so move search to next command in mission cmd_index++; } } else { // this is a non-jump command so return it cmd = temp_cmd; return true; } } // if we got this far we did not find a navigation command return false; } /// get_next_do_cmd - gets next "do" or "conditional" command after start_index /// returns true if found, false if not found /// stops and returns false if it hits another navigation command before it finds the first do or conditional command /// accounts for do_jump commands but never increments the jump's num_times_run (advance_current_nav_cmd is responsible for this) bool AP_Mission::get_next_do_cmd(uint16_t start_index, Mission_Command& cmd) { Mission_Command temp_cmd; // check we have not passed the end of the mission list if (start_index >= (unsigned)_cmd_total) { return false; } // get next command if (!get_next_cmd(start_index, temp_cmd, false)) { // no more commands so return failure return false; } else if (is_nav_cmd(temp_cmd)) { // if it's a "navigation" command then return false because we do not progress past nav commands return false; } else { // this must be a "do" or "conditional" and is not a do-jump command so return it cmd = temp_cmd; return true; } } /// /// 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++) { if (get_command_id(i) != uint16_t(MAV_CMD_JUMP_TAG)) { continue; } 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; } #if AP_SCRIPTING_ENABLED bool AP_Mission::get_last_jump_tag(uint16_t &tag, uint16_t &age) const { if (_jump_tag.age == 0) { return false; } tag = _jump_tag.tag; age = _jump_tag.age; return true; } #endif // init_jump_tracking - initialise jump_tracking variables void AP_Mission::init_jump_tracking() { for (uint8_t i=0; i= (unsigned)_cmd_total) || (cmd.content.jump.target == 0)) { // To-Do: log an error? return AP_MISSION_JUMP_TIMES_MAX; } // search through jump_tracking array for this cmd for (uint8_t i=0; i= dist_continue_to_land) { // then the mission should carry on uninterrupted as that is the shorter distance GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Rejecting RTL: closer land if mis continued"); return true; } else { // allow failsafes to interrupt the current mission return false; } } // Approximate the distance travelled to get to a landing. DO_JUMP commands are observed in look forward. bool AP_Mission::distance_to_landing(uint16_t index, float &tot_distance, Location prev_loc) { Mission_Command temp_cmd; tot_distance = 0.0f; bool ret = false; // reached end of loop without getting to a landing // back up jump tracking to reset after distance calculation jump_tracking_struct _jump_tracking_backup[AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS]; for (uint8_t i=0; i=0; i--) { // to get this far there has to be at least one 'passed wp' stored in history. This is to check incase // of history array no being completely filled with valid waypoints upon resume. if (_wp_index_history[i] == AP_MISSION_CMD_INDEX_NONE) { // no more stored history resume_index = i+1; break; } if (!read_cmd_from_storage(_wp_index_history[i], temp_cmd)) { // if read from storage failed then don't use rewind return false; } // calculate distance float disttemp = prev_loc.get_distance(temp_cmd.content.location); //(m) rewind_distance -= disttemp; resume_index = i; if (rewind_distance <= 0.0f) { // history rewound enough distance to meet _repeat_dist requirement rewind_cmd = temp_cmd; break; } // store wp location as previous prev_loc = temp_cmd.content.location; } if (rewind_distance > 0.0f) { // then the history array was rewound all of the way without finding a wp distance > _repeat_dist distance. // the last read temp_cmd will be the furthest cmd back in the history array so resume to that. rewind_cmd = temp_cmd; return true; } // if we have got this far the desired rewind distance lies between two waypoints stored in history array. // calculate the location for the mission to resume // the last wp read from storage is the wp that is before the resume wp in the mission order Location passed_wp_loc = temp_cmd.content.location; // fetch next destination wp if (!read_cmd_from_storage(_wp_index_history[resume_index+1], temp_cmd)) { // if read from storage failed then don't use rewind return false; } // determine the length of the mission leg that the resume wp lies in float leg_length = passed_wp_loc.get_distance(temp_cmd.content.location); //(m) // calculate the percentage along the leg that resume wp will be positioned float leg_percent = fabsf(rewind_distance)/leg_length; // calculate difference vector of mission leg Vector3f dist_vec = passed_wp_loc.get_distance_NED(temp_cmd.content.location); // calculate the resume wp position rewind_cmd.content.location.offset(dist_vec.x * leg_percent, dist_vec.y * leg_percent); rewind_cmd.content.location.alt -= dist_vec.z * leg_percent * 100; //(cm) // The rewind_cmd.index has the index of the 'last passed wp' from the history array. This ensures that the mission order // continues as planned without further intervention. The resume wp is not written to memory so will not perminantely change the mission. // if we got this far then mission rewind position was successfully calculated return true; } /* handle format conversion of storage format to allow us to update format to take advantage of new packing. This is particularly useful for conversion to Float16 to get extra parameter space */ void AP_Mission::format_conversion(uint8_t tag_byte, const Mission_Command &cmd, PackedContent &packed_content) const { // currently only one conversion needed, more can be added #if AP_SCRIPTING_ENABLED if (tag_byte == 0 && cmd.id == MAV_CMD_NAV_SCRIPT_TIME) { // PARAMETER_CONVERSION: conversion code added Oct 2022 struct nav_script_time_Command_tag0 old_fmt; struct nav_script_time_Command new_fmt; memcpy((void*)&old_fmt, packed_content.bytes, sizeof(old_fmt)); new_fmt.command = old_fmt.command; new_fmt.timeout_s = old_fmt.timeout_s; new_fmt.arg1.set(old_fmt.arg1); new_fmt.arg2.set(old_fmt.arg2); new_fmt.arg3 = 0; new_fmt.arg4 = 0; memcpy(packed_content.bytes, (void*)&new_fmt, sizeof(new_fmt)); } #endif } // Helpers to fill in location for scripting #if AP_SCRIPTING_ENABLED bool AP_Mission::jump_to_landing_sequence(void) { Location loc; if (AP::ahrs().get_location(loc)) { return jump_to_landing_sequence(loc); } GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start landing sequence"); return false; } bool AP_Mission::jump_to_abort_landing_sequence(void) { Location loc; if (AP::ahrs().get_location(loc)) { return jump_to_abort_landing_sequence(loc); } GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start find a landing abort sequence"); return false; } #endif // AP_SCRIPTING_ENABLED // singleton instance AP_Mission *AP_Mission::_singleton; namespace AP { AP_Mission *mission() { return AP_Mission::get_singleton(); } } #endif // AP_MISSION_ENABLED