From 87c7d19f830baee06732508607d34db4cf42efa3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 28 Feb 2014 11:58:27 +0900 Subject: [PATCH] AP_Mission: special handling for cmd --- libraries/AP_Mission/AP_Mission.cpp | 47 +++++++++++++++++------------ libraries/AP_Mission/AP_Mission.h | 6 +++- 2 files changed, 32 insertions(+), 21 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 75c6663949..e2bf759f25 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -212,34 +212,41 @@ bool AP_Mission::read_cmd_from_storage(uint8_t index, Mission_Command& cmd) cons return false; } - // 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 - pos_in_storage = (AP_MISSION_EEPROM_START_BYTE) + (index * AP_MISSION_EEPROM_COMMAND_SIZE); + // special handling for command #0 which is home + if (index == 0) { + cmd.index = 0; + cmd.id = MAV_CMD_NAV_WAYPOINT; + cmd.content.location = _ahrs.get_home(); + }else{ + // 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 + pos_in_storage = (AP_MISSION_EEPROM_START_BYTE) + (index * AP_MISSION_EEPROM_COMMAND_SIZE); - cmd.content.location.id = hal.storage->read_byte(pos_in_storage); - pos_in_storage++; + cmd.content.location.id = hal.storage->read_byte(pos_in_storage); + pos_in_storage++; - cmd.content.location.options = hal.storage->read_byte(pos_in_storage); - pos_in_storage++; + cmd.content.location.options = hal.storage->read_byte(pos_in_storage); + pos_in_storage++; - cmd.content.location.p1 = hal.storage->read_byte(pos_in_storage); - pos_in_storage++; + cmd.content.location.p1 = hal.storage->read_byte(pos_in_storage); + pos_in_storage++; - cmd.content.location.alt = hal.storage->read_dword(pos_in_storage); - pos_in_storage += 4; + cmd.content.location.alt = hal.storage->read_dword(pos_in_storage); + pos_in_storage += 4; - cmd.content.location.lat = hal.storage->read_dword(pos_in_storage); - pos_in_storage += 4; + cmd.content.location.lat = hal.storage->read_dword(pos_in_storage); + pos_in_storage += 4; - cmd.content.location.lng = hal.storage->read_dword(pos_in_storage); + cmd.content.location.lng = hal.storage->read_dword(pos_in_storage); - // set command's index to it's position in eeprom - cmd.index = index; + // set command's index to it's position in eeprom + cmd.index = index; - // set command from location's command - // To-Do: remove id (and p1?) from Location structure - cmd.id = cmd.content.location.id; + // set command from location's command + // To-Do: remove id (and p1?) from Location structure + cmd.id = cmd.content.location.id; + } // return success return true; diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 2162054748..b1f8e32685 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -79,7 +79,8 @@ public: }; /// constructor - AP_Mission(mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn, mission_complete_fn_t mission_complete_fn) : + AP_Mission(AP_AHRS &ahrs, mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn, mission_complete_fn_t mission_complete_fn) : + _ahrs(ahrs), _cmd_start_fn(cmd_start_fn), _cmd_verify_fn(cmd_verify_fn), _mission_complete_fn(mission_complete_fn), @@ -220,6 +221,9 @@ private: /// increment_jump_times_run - increments the recorded number of times the jump command has been run void increment_jump_times_run(Mission_Command& cmd); + // references to external libraries + const AP_AHRS& _ahrs; // used only for home position + // parameters AP_Int16 _cmd_total; // total number of commands in the mission