diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 14365ddf29..e81b3a4c91 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -36,6 +36,7 @@ #include #include #include +#include #include // ArduPilot GPS library #include // ArduPilot barometer library #include // ArduPilot Mega Magnetometer Library @@ -317,7 +318,7 @@ static AP_Camera camera(&relay); #endif //Rally Ponints -AP_Rally rally(ahrs, MAX_RALLYPOINTS, RALLY_START_BYTE); +AP_Rally rally(ahrs); //////////////////////////////////////////////////////////////////////////////// // Global variables @@ -589,8 +590,7 @@ static bool start_command_callback(const AP_Mission::Mission_Command &cmd); AP_Mission mission(ahrs, &start_command_callback, &verify_command_callback, - &exit_mission_callback, - MISSION_START_BYTE, MISSION_END_BYTE); + &exit_mission_callback); //////////////////////////////////////////////////////////////////////////////// // terrain handling @@ -792,7 +792,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { }; // setup the var_info table -AP_Param param_loader(var_info, MISSION_START_BYTE); +AP_Param param_loader(var_info); void setup() { cliSerial = hal.console; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 4ab4878e79..81c7c6def2 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1290,7 +1290,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/ArduPlane/defines.h b/ArduPlane/defines.h index cf4c64b025..224bb286f5 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -177,19 +177,6 @@ enum log_messages { // which a groundstart will be // triggered -// fence points are stored at the end of the EEPROM -#define MAX_FENCEPOINTS 20 -#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 10 -#define RALLY_START_BYTE (FENCE_START_BYTE-(MAX_RALLYPOINTS*AC_RALLY_WP_SIZE)) - -// parameters get the first 1280 bytes of EEPROM, mission commands are stored between these params and the rally points -#define MISSION_START_BYTE 0x500 -#define MISSION_END_BYTE (RALLY_START_BYTE-1) - // convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps // to -1) #define BOOL_TO_SIGN(bvalue) ((bvalue) ? -1 : 1) diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde index c71f1c03d4..539b96067f 100644 --- a/ArduPlane/geofence.pde +++ b/ArduPlane/geofence.pde @@ -35,27 +35,34 @@ static struct GeofenceState { int32_t guided_lat; int32_t guided_lng; /* point 0 is the return point */ - Vector2l boundary[MAX_FENCEPOINTS]; + Vector2l *boundary; } *geofence_state; +static const StorageAccess fence_storage(StorageManager::StorageFence); + +/* + maximum number of fencepoints + */ +static uint8_t max_fencepoints(void) +{ + return min(255, fence_storage.size() / sizeof(Vector2l)); +} + /* * fence boundaries fetch/store */ static Vector2l get_fence_point_with_index(unsigned i) { - uint16_t mem; Vector2l ret; - if (i > (unsigned)g.fence_total) { + if (i > (unsigned)g.fence_total || i >= max_fencepoints()) { return Vector2l(0,0); } // read fence point - mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE); - ret.x = hal.storage->read_dword(mem); - mem += sizeof(uint32_t); - ret.y = hal.storage->read_dword(mem); + ret.x = fence_storage.read_uint32(i * sizeof(Vector2l)); + ret.y = fence_storage.read_uint32(i * sizeof(Vector2l) + 4); return ret; } @@ -63,18 +70,13 @@ static Vector2l get_fence_point_with_index(unsigned i) // save a fence point static void set_fence_point_with_index(Vector2l &point, unsigned i) { - uint16_t mem; - - if (i >= (unsigned)g.fence_total.get()) { + if (i >= (unsigned)g.fence_total.get() || i >= max_fencepoints()) { // not allowed return; } - mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE); - - hal.storage->write_dword(mem, point.x); - mem += sizeof(uint32_t); - hal.storage->write_dword(mem, point.y); + fence_storage.write_uint32(i * sizeof(Vector2l), point.x); + fence_storage.write_uint32(i * sizeof(Vector2l)+4, point.y); if (geofence_state != NULL) { geofence_state->boundary_uptodate = false; @@ -98,6 +100,14 @@ static void geofence_load(void) // not much we can do here except disable it goto failed; } + + geofence_state->boundary = (Vector2l *)calloc(sizeof(Vector2l), max_fencepoints()); + if (geofence_state->boundary == NULL) { + free(geofence_state); + geofence_state = NULL; + goto failed; + } + geofence_state->old_switch_position = 254; } diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 6415cb9d01..6ec26b0b7b 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -448,11 +448,8 @@ print_divider(void) static void zero_eeprom(void) { - uint8_t b = 0; cliSerial->printf_P(PSTR("\nErasing EEPROM\n")); - for (uint16_t i = 0; i < HAL_STORAGE_SIZE_AVAILABLE; i++) { - hal.storage->write_byte(i, b); - } + StorageManager::erase(); cliSerial->printf_P(PSTR("done\n")); } diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 66d026226a..7dc7c22f69 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -19,7 +19,6 @@ static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv); static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); static int8_t test_mag(uint8_t argc, const Menu::arg *argv); static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); -static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv); static int8_t test_logging(uint8_t argc, const Menu::arg *argv); #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN @@ -41,7 +40,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { {"relay", test_relay}, {"waypoints", test_wp}, {"xbee", test_xbee}, - {"eedump", test_eedump}, {"modeswitch", test_modeswitch}, // Tests below here are for hardware sensors only present @@ -83,21 +81,6 @@ static void print_hit_enter() cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n")); } -static int8_t -test_eedump(uint8_t argc, const Menu::arg *argv) -{ - uint16_t i, j; - - // hexdump the EEPROM - for (i = 0; i < HAL_STORAGE_SIZE_AVAILABLE; i += 16) { - cliSerial->printf_P(PSTR("%04x:"), i); - for (j = 0; j < 16; j++) - cliSerial->printf_P(PSTR(" %02x"), hal.storage->read_byte(i + j)); - cliSerial->println(); - } - return(0); -} - static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv) {