From b108b907e03b554390a799dd20a067cef20a7cca Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Dec 2011 15:53:14 +1100 Subject: [PATCH] APM: Update for DataFlash changes --- ArduPlane/Log.pde | 258 ++++--------------------------------------- ArduPlane/system.pde | 27 ++--- ArduPlane/test.pde | 26 +++++ 3 files changed, 54 insertions(+), 257 deletions(-) diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index b5bab748f7..6b9887c076 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -45,17 +45,15 @@ static const struct Menu::command log_menu_commands[] PROGMEM = { // A Macro to create the Menu MENU2(log_menu, "Log", log_menu_commands, print_log_menu); -static void get_log_boundaries(byte log_num, int & start_page, int & end_page); - static bool print_log_menu(void) { int log_start; int log_end; int temp; - int last_log_num = find_last_log(); + int last_log_num = DataFlash.find_last_log(); - uint16_t num_logs = get_num_logs(); + uint16_t num_logs = DataFlash.get_num_logs(); Serial.printf_P(PSTR("logs enabled: ")); @@ -90,7 +88,7 @@ print_log_menu(void) for(int i=num_logs;i>=1;i--) { int last_log_start = log_start, last_log_end = log_end; temp = last_log_num-i+1; - get_log_boundaries(temp, log_start, log_end); + DataFlash.get_log_boundaries(temp, log_start, log_end); Serial.printf_P(PSTR("Log %d, start %d, end %d\n"), temp, log_start, log_end); if (last_log_start == log_start && last_log_end == log_end) { // we are printing bogus logs @@ -112,7 +110,7 @@ dump_log(uint8_t argc, const Menu::arg *argv) // check that the requested log number can be read dump_log = argv[1].i; - last_log_num = find_last_log(); + last_log_num = DataFlash.find_last_log(); if (dump_log == -2) { for(uint16_t count=1; count<=DataFlash.df_NumPages; count++) { @@ -126,12 +124,12 @@ dump_log(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("dumping all\n")); Log_Read(1, DataFlash.df_NumPages); return(-1); - } else if ((argc != 2) || (dump_log <= (last_log_num - get_num_logs())) || (dump_log > last_log_num)) { + } else if ((argc != 2) || (dump_log <= (last_log_num - DataFlash.get_num_logs())) || (dump_log > last_log_num)) { Serial.printf_P(PSTR("bad log number\n")); return(-1); } - get_log_boundaries(dump_log, dump_log_start, dump_log_end); + DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end); Serial.printf_P(PSTR("Dumping Log %d, start pg %d, end pg %d\n"), dump_log, dump_log_start, @@ -142,26 +140,27 @@ dump_log(uint8_t argc, const Menu::arg *argv) return 0; } -static void -do_erase_logs(void (*delay_cb)(unsigned long)) + +void erase_callback(unsigned long t) { + mavlink_delay(t); + if (DataFlash.GetWritePage() % 128 == 0) { + Serial.printf_P(PSTR("+")); + } +} + +static void do_erase_logs(void) { Serial.printf_P(PSTR("\nErasing log...\n")); - DataFlash.SetFileNumber(0xFFFF); - for(uint16_t j = 1; j <= DataFlash.df_NumPages; j++) { - DataFlash.PageErase(j); - DataFlash.StartWrite(j); // We need this step to clean FileNumbers - if(j%128 == 0) Serial.printf_P(PSTR("+")); - delay_cb(1); - } - + DataFlash.EraseAll(erase_callback); Serial.printf_P(PSTR("\nLog erased.\n")); - DataFlash.FinishWrite(); } static int8_t erase_logs(uint8_t argc, const Menu::arg *argv) { - do_erase_logs(delay); + in_mavlink_delay = true; + do_erase_logs(); + in_mavlink_delay = false; return 0; } @@ -216,223 +215,6 @@ process_logs(uint8_t argc, const Menu::arg *argv) } -// This function determines the number of whole or partial log files in the DataFlash -// Wholly overwritten files are (of course) lost. -static byte get_num_logs(void) -{ - uint16_t lastpage; - uint16_t last; - uint16_t first; - - if(find_last_page() == 1) return 0; - - DataFlash.StartRead(1); - - if(DataFlash.GetFileNumber() == 0XFFFF) return 0; - - lastpage = find_last_page(); - DataFlash.StartRead(lastpage); - last = DataFlash.GetFileNumber(); - DataFlash.StartRead(lastpage + 2); - first = DataFlash.GetFileNumber(); - if(first > last) { - DataFlash.StartRead(1); - first = DataFlash.GetFileNumber(); - } - if(last == first) - { - return 1; - } else { - return (last - first + 1); - } -} - -// This function starts a new log file in the DataFlash -static void start_new_log() -{ - uint16_t last_page = find_last_page(); - - DataFlash.StartRead(last_page); - //Serial.print("last page: "); Serial.println(last_page); - //Serial.print("file #: "); Serial.println(DataFlash.GetFileNumber()); - //Serial.print("file page: "); Serial.println(DataFlash.GetFilePage()); - - if(find_last_log() == 0 || DataFlash.GetFileNumber() == 0xFFFF) { - DataFlash.SetFileNumber(1); - DataFlash.StartWrite(1); - //Serial.println("start log from 0"); - return; - } - - // Check for log of length 1 page and suppress - if(DataFlash.GetFilePage() <= 1) { - DataFlash.SetFileNumber(DataFlash.GetFileNumber()); // Last log too short, reuse its number - DataFlash.StartWrite(last_page); // and overwrite it - //Serial.println("start log from short"); - } else { - if(last_page == 0xFFFF) last_page=0; - DataFlash.SetFileNumber(DataFlash.GetFileNumber()+1); - DataFlash.StartWrite(last_page + 1); - //Serial.println("start log normal"); - } -} - -// This function finds the first and last pages of a log file -// The first page may be greater than the last page if the DataFlash has been filled and partially overwritten. -static void get_log_boundaries(byte log_num, int & start_page, int & end_page) -{ - int num = get_num_logs(); - int look; - - if(num == 1) - { - DataFlash.StartRead(DataFlash.df_NumPages); - if(DataFlash.GetFileNumber() == 0xFFFF) - { - start_page = 1; - end_page = find_last_page_of_log((uint16_t)log_num); - } else { - end_page = find_last_page_of_log((uint16_t)log_num); - start_page = end_page + 1; - } - - } else { - if(log_num==1) { - DataFlash.StartRead(DataFlash.df_NumPages); - if(DataFlash.GetFileNumber() == 0xFFFF) { - start_page = 1; - } else { - start_page = find_last_page() + 1; - } - } else { - if(log_num == find_last_log() - num + 1) { - start_page = find_last_page() + 1; - } else { - look = log_num-1; - do { - start_page = find_last_page_of_log(look) + 1; - look--; - } while (start_page <= 0 && look >=1); - } - } - } - if(start_page == (int)DataFlash.df_NumPages+1 || start_page == 0) start_page=1; - end_page = find_last_page_of_log((uint16_t)log_num); - if(end_page <= 0) end_page = start_page; -} - -static bool check_wrapped(void) -{ - DataFlash.StartRead(DataFlash.df_NumPages); - if(DataFlash.GetFileNumber() == 0xFFFF) - return 0; - else - return 1; -} - - -// This funciton finds the last log number -static int find_last_log(void) -{ - int last_page = find_last_page(); - DataFlash.StartRead(last_page); - return DataFlash.GetFileNumber(); -} - -// This function finds the last page of the last file -static int find_last_page(void) -{ -uint16_t look; -uint16_t bottom = 1; -uint16_t top = DataFlash.df_NumPages; -uint32_t look_hash; -uint32_t bottom_hash; -uint32_t top_hash; - - DataFlash.StartRead(bottom); - bottom_hash = ((long)DataFlash.GetFileNumber()<<16) | DataFlash.GetFilePage(); - - while(top-bottom > 1) - { - look = (top+bottom)/2; - DataFlash.StartRead(look); - look_hash = (long)DataFlash.GetFileNumber()<<16 | DataFlash.GetFilePage(); - if (look_hash >= 0xFFFF0000) look_hash = 0; - - if(look_hash < bottom_hash) { - // move down - top = look; - } else { - // move up - bottom = look; - bottom_hash = look_hash; - } - } - - DataFlash.StartRead(top); - top_hash = ((long)DataFlash.GetFileNumber()<<16) | DataFlash.GetFilePage(); - if (top_hash >= 0xFFFF0000) top_hash = 0; - if (top_hash > bottom_hash) - { - return top; - } else { - return bottom; - } -} - -// This function finds the last page of a particular log file -static int find_last_page_of_log(uint16_t log_number) -{ - -uint16_t look; -uint16_t bottom; -uint16_t top; -uint32_t look_hash; -uint32_t check_hash; - - if(check_wrapped()) - { - DataFlash.StartRead(1); - bottom = DataFlash.GetFileNumber(); - if (bottom > log_number) - { - bottom = find_last_page(); - top = DataFlash.df_NumPages; - } else { - bottom = 1; - top = find_last_page(); - } - } else { - bottom = 1; - top = find_last_page(); - } - - check_hash = (long)log_number<<16 | 0xFFFF; - - while(top-bottom > 1) - { - look = (top+bottom)/2; - DataFlash.StartRead(look); - look_hash = (long)DataFlash.GetFileNumber()<<16 | DataFlash.GetFilePage(); - if (look_hash >= 0xFFFF0000) look_hash = 0; - - if(look_hash > check_hash) { - // move down - top = look; - } else { - // move up - bottom = look; - } - } - - DataFlash.StartRead(top); - if (DataFlash.GetFileNumber() == log_number) return top; - - DataFlash.StartRead(bottom); - if (DataFlash.GetFileNumber() == log_number) return bottom; - - return -1; -} // Write an attitude packet. Total length : 10 bytes @@ -883,8 +665,6 @@ static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_ int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats) {} static void Log_Write_Performance() {} static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } -static byte get_num_logs(void) { return 0; } -static void start_new_log() {} static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw) {} static void Log_Write_Control_Tuning() {} static void Log_Write_Raw() {} diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 11af9abf41..e6513cfe8a 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -58,8 +58,6 @@ static void run_cli(void) static void init_ardupilot() { - bool need_log_erase = false; - #if USB_MUX_PIN > 0 // on the APM2 board we have a mux thet switches UART0 between // USB and the board header. If the right ArduPPM firmware is @@ -136,12 +134,6 @@ static void init_ardupilot() delay(100); // wait for serial send AP_Var::erase_all(); - // erase DataFlash on format version change - #if LOGGING_ENABLED == ENABLED - DataFlash.Init(); - need_log_erase = true; - #endif - // save the current format version g.format_version.set_and_save(Parameters::k_format_version); Serial.println_P(PSTR("done.")); @@ -178,10 +170,17 @@ static void init_ardupilot() mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED - if (need_log_erase) { + DataFlash.Init(); // DataFlash log initialization + if (!DataFlash.CardInserted()) { + gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash card inserted")); + g.log_bitmask.set(0); + } else if (DataFlash.NeedErase()) { gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS")); - do_erase_logs(mavlink_delay); + do_erase_logs(); } + if (g.log_bitmask != 0) { + DataFlash.start_new_log(); + } #endif #if HIL_MODE != HIL_MODE_ATTITUDE @@ -204,10 +203,6 @@ static void init_ardupilot() } #endif -#if LOGGING_ENABLED == ENABLED - DataFlash.Init(); // DataFlash log initialization -#endif - // Do GPS init g_gps = &g_gps_driver; g_gps->init(); // GPS Initialization @@ -270,10 +265,6 @@ static void init_ardupilot() Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n")); #endif // CLI_ENABLED - if(g.log_bitmask != 0){ - start_new_log(); - } - // read in the flight switches update_servo_switches(); diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index e4ebfdcfa1..11e1fc0a34 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -27,6 +27,7 @@ static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv); #if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2 static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv); #endif +static int8_t test_logging(uint8_t argc, const Menu::arg *argv); // Creates a constant array of structs representing menu options // and stores them in Flash memory, not RAM. @@ -67,6 +68,7 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { {"compass", test_mag}, #elif HIL_MODE == HIL_MODE_ATTITUDE #endif + {"logging", test_logging}, }; @@ -407,6 +409,30 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv) } } +/* + test the dataflash is working + */ +static int8_t +test_logging(uint8_t argc, const Menu::arg *argv) +{ + Serial.println_P(PSTR("Testing dataflash logging")); + if (!DataFlash.CardInserted()) { + Serial.println_P(PSTR("ERR: No dataflash inserted")); + return 0; + } + DataFlash.ReadManufacturerID(); + Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"), + (unsigned)DataFlash.df_manufacturer, + (unsigned)DataFlash.df_device); + Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"), + (unsigned)DataFlash.df_NumPages+1, + (unsigned)DataFlash.df_PageSize); + DataFlash.StartRead(DataFlash.df_NumPages+1); + Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"), + (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT); + return 0; +} + #if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2 static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv)