Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM: Update for DataFlash changes

This commit is contained in:
Andrew Tridgell 2011-12-28 15:53:14 +11:00
parent 73e6d5ee11
commit 8d87aa41b9
3 changed files with 54 additions and 257 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

258
ArduPlane/Log.pde
View File

@ -45,17 +45,15 @@ static const struct Menu::command log_menu_commands[] PROGMEM = {
// A Macro to create the Menu // A Macro to create the Menu
MENU2(log_menu, "Log", log_menu_commands, print_log_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 static bool
print_log_menu(void) print_log_menu(void)
{ {
int log_start; int log_start;
int log_end; int log_end;
int temp; 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: ")); Serial.printf_P(PSTR("logs enabled: "));
@ -90,7 +88,7 @@ print_log_menu(void)
for(int i=num_logs;i>=1;i--) { for(int i=num_logs;i>=1;i--) {
int last_log_start = log_start, last_log_end = log_end; int last_log_start = log_start, last_log_end = log_end;
temp = last_log_num-i+1; 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); 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) { if (last_log_start == log_start && last_log_end == log_end) {
// we are printing bogus logs // 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 // check that the requested log number can be read
dump_log = argv[1].i; dump_log = argv[1].i;
last_log_num = find_last_log(); last_log_num = DataFlash.find_last_log();
if (dump_log == -2) { if (dump_log == -2) {
for(uint16_t count=1; count<=DataFlash.df_NumPages; count++) { 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")); Serial.printf_P(PSTR("dumping all\n"));
Log_Read(1, DataFlash.df_NumPages); Log_Read(1, DataFlash.df_NumPages);
return(-1); 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")); Serial.printf_P(PSTR("bad log number\n"));
return(-1); 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"), Serial.printf_P(PSTR("Dumping Log %d, start pg %d, end pg %d\n"),
dump_log, dump_log,
dump_log_start, dump_log_start,
@ -142,26 +140,27 @@ dump_log(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
static void
do_erase_logs(void (*delay_cb)(unsigned long)) void erase_callback(unsigned long t) {
{ mavlink_delay(t);
Serial.printf_P(PSTR("\nErasing log...\n")); if (DataFlash.GetWritePage() % 128 == 0) {
DataFlash.SetFileNumber(0xFFFF); Serial.printf_P(PSTR("+"));
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);
} }
static void do_erase_logs(void)
{
Serial.printf_P(PSTR("\nErasing log...\n"));
DataFlash.EraseAll(erase_callback);
Serial.printf_P(PSTR("\nLog erased.\n")); Serial.printf_P(PSTR("\nLog erased.\n"));
DataFlash.FinishWrite();
} }
static int8_t static int8_t
erase_logs(uint8_t argc, const Menu::arg *argv) 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; 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 // 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) {} int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats) {}
static void Log_Write_Performance() {} static void Log_Write_Performance() {}
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } 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_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw) {}
static void Log_Write_Control_Tuning() {} static void Log_Write_Control_Tuning() {}
static void Log_Write_Raw() {} static void Log_Write_Raw() {}

27
ArduPlane/system.pde
View File

@ -58,8 +58,6 @@ static void run_cli(void)
static void init_ardupilot() static void init_ardupilot()
{ {
bool need_log_erase = false;
#if USB_MUX_PIN > 0 #if USB_MUX_PIN > 0
// on the APM2 board we have a mux thet switches UART0 between // on the APM2 board we have a mux thet switches UART0 between
// USB and the board header. If the right ArduPPM firmware is // 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 delay(100); // wait for serial send
AP_Var::erase_all(); 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 // save the current format version
g.format_version.set_and_save(Parameters::k_format_version); g.format_version.set_and_save(Parameters::k_format_version);
Serial.println_P(PSTR("done.")); Serial.println_P(PSTR("done."));
@ -178,9 +170,16 @@ static void init_ardupilot()
mavlink_system.sysid = g.sysid_this_mav; mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED #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")); 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 #endif
@ -204,10 +203,6 @@ static void init_ardupilot()
} }
#endif #endif
#if LOGGING_ENABLED == ENABLED
DataFlash.Init(); // DataFlash log initialization
#endif
// Do GPS init // Do GPS init
g_gps = &g_gps_driver; g_gps = &g_gps_driver;
g_gps->init(); // GPS Initialization 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")); Serial.printf_P(PSTR("\nPress ENTER 3 times to start interactive setup\n\n"));
#endif // CLI_ENABLED #endif // CLI_ENABLED
if(g.log_bitmask != 0){
start_new_log();
}
// read in the flight switches // read in the flight switches
update_servo_switches(); update_servo_switches();

26
ArduPlane/test.pde
View File

@ -27,6 +27,7 @@ static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2 #if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv); static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv);
#endif #endif
static int8_t test_logging(uint8_t argc, const Menu::arg *argv);
// Creates a constant array of structs representing menu options // Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM. // and stores them in Flash memory, not RAM.
@ -67,6 +68,7 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
{"compass", test_mag}, {"compass", test_mag},
#elif HIL_MODE == HIL_MODE_ATTITUDE #elif HIL_MODE == HIL_MODE_ATTITUDE
#endif #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 #if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
static int8_t static int8_t
test_dipswitches(uint8_t argc, const Menu::arg *argv) test_dipswitches(uint8_t argc, const Menu::arg *argv)