From e260f1c5691736bec761ab9635aa25f58b81ce92 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Aug 2014 14:44:31 +1000 Subject: [PATCH] Copter: convert to using StorageManager --- ArduCopter/ArduCopter.pde | 10 +++++++--- ArduCopter/GCS_Mavlink.pde | 2 +- ArduCopter/defines.h | 19 ------------------- 3 files changed, 8 insertions(+), 23 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index add094d007..c2e324bfef 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -87,6 +87,7 @@ #include #include #include +#include // AP_HAL #include #include @@ -323,7 +324,7 @@ SITL sitl; static bool start_command(const AP_Mission::Mission_Command& cmd); static bool verify_command(const AP_Mission::Mission_Command& cmd); static void exit_mission(); -AP_Mission mission(ahrs, &start_command, &verify_command, &exit_mission, MISSION_START_BYTE, MISSION_END_BYTE); +AP_Mission mission(ahrs, &start_command, &verify_command, &exit_mission); //////////////////////////////////////////////////////////////////////////////// // Optical flow sensor @@ -716,7 +717,7 @@ AC_Fence fence(&inertial_nav); // Rally library //////////////////////////////////////////////////////////////////////////////// #if AC_RALLY == ENABLED -AP_Rally rally(ahrs, MAX_RALLYPOINTS, RALLY_START_BYTE); +AP_Rally rally(ahrs); #endif //////////////////////////////////////////////////////////////////////////////// @@ -766,7 +767,7 @@ static void pre_arm_checks(bool display_failure); //////////////////////////////////////////////////////////////////////////////// // setup the var_info table -AP_Param param_loader(var_info, MISSION_START_BYTE); +AP_Param param_loader(var_info); #if MAIN_LOOP_RATE == 400 /* @@ -912,6 +913,9 @@ void setup() // Load the default values of variables listed in var_info[]s AP_Param::setup_sketch_defaults(); + // setup storage layout for copter + StorageManager::set_layout_copter(); + init_ardupilot(); // initialise the main loop scheduler diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index ae8a879568..db6354f7dc 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1329,7 +1329,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; if (packet.idx >= rally.get_rally_total() || - packet.idx >= MAX_RALLYPOINTS) { + packet.idx >= rally.get_rally_max()) { send_text_P(SEVERITY_LOW,PSTR("bad rally point message ID")); break; } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index f92df07884..8a19fa343b 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -310,25 +310,6 @@ enum FlipState { // Centi-degrees to radians #define DEGX100 5729.57795f -// fence points are stored at the end of the EEPROM -#define MAX_FENCEPOINTS 6 -#define FENCE_WP_SIZE sizeof(Vector2l) -#define FENCE_START_BYTE (HAL_STORAGE_SIZE_AVAILABLE-(MAX_FENCEPOINTS*FENCE_WP_SIZE)) - -// rally points shoehorned between fence points and waypoints -#define MAX_RALLYPOINTS 6 -#define RALLY_START_BYTE (FENCE_START_BYTE-(MAX_RALLYPOINTS*AC_RALLY_WP_SIZE)) -#define RALLY_LIMIT_KM_DEFAULT 2.0 // we'll set a per-vehicle default for this - -// parameters get the first 1536 bytes of EEPROM -// mission commands are stored between these params and the rally points, or fence points if rally disabled -#define MISSION_START_BYTE 0x600 -#if AC_RALLY == ENABLED - #define MISSION_END_BYTE (RALLY_START_BYTE-1) -#else - #define MISSION_END_BYTE (FENCE_START_BYTE-1) -#endif - // mark a function as not to be inlined #define NOINLINE __attribute__((noinline))