Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

Browse Source
This commit is contained in:
Chris Anderson 2011-12-27 21:15:36 -08:00
commit 04c73fbe72
20 changed files with 448 additions and 557 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

253
ArduCopter/Log.pde
View File

@ -46,17 +46,15 @@ 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: "));
@ -86,7 +84,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
@ -108,7 +106,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(int count=1; count<=DataFlash.df_NumPages; count++) {
@ -122,12 +120,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 number %d, start %d, end %d\n"),
dump_log,
dump_log_start,
@ -138,26 +136,26 @@ 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(int 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,220 +214,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 == 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;
}
// print_latlon - prints an latitude or longitude value held in an int32_t
// probably this should be moved to AP_Common
@ -1136,7 +920,6 @@ static void Log_Read_Startup() {}
static void Log_Read(int start_page, int end_page) {}
static void Log_Write_Cmd(byte num, struct Location *wp) {}
static void Log_Write_Mode(byte mode) {}
static void start_new_log() {}
static void Log_Write_Raw() {}
static void Log_Write_GPS() {}
static void Log_Write_Current() {}

37
ArduCopter/system.pde
View File

@ -55,8 +55,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
@ -174,13 +172,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();
#endif
need_log_erase = true;
// save the new format version
g.format_version.set_and_save(Parameters::k_format_version);
@ -212,13 +203,19 @@ static void init_ardupilot()
// identify ourselves correctly with the ground station
mavlink_system.sysid = g.sysid_this_mav;
if (need_log_erase) {
#if LOGGING_ENABLED == ENABLED
DataFlash.Init();
if (!DataFlash.CardInserted()) {
gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash inserted"));
g.log_bitmask.set(0);
} else if (DataFlash.NeedErase()) {
gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
#if LOGGING_ENABLED == ENABLED
do_erase_logs(mavlink_delay);
#endif
do_erase_logs();
}
if (g.log_bitmask != 0){
DataFlash.start_new_log();
}
#endif
#ifdef RADIO_OVERRIDE_DEFAULTS
{
@ -270,10 +267,6 @@ static void init_ardupilot()
USERHOOK_INIT
#endif
#if LOGGING_ENABLED == ENABLED
DataFlash.Init();
#endif
#if CLI_ENABLED == ENABLED && CLI_SLIDER_ENABLED == ENABLED
// If the switch is in 'menu' mode, run the main menu.
//
@ -290,14 +283,6 @@ static void init_ardupilot()
Serial.printf_P(PSTR("\nPress ENTER 3 times for CLI\n\n"));
#endif // CLI_ENABLED
#if LOGGING_ENABLED == ENABLED
if(g.log_bitmask != 0){
// TODO - Here we will check on the length of the last log
// We don't want to create a bunch of little logs due to powering on and off
start_new_log();
}
#endif
GPS_enabled = false;
#if HIL_MODE == HIL_MODE_DISABLED

27
ArduCopter/test.pde
View File

@ -30,6 +30,7 @@ static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
#endif
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
static int8_t test_optflow(uint8_t argc, const Menu::arg *argv);
static int8_t test_logging(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_rawgps(uint8_t argc, const Menu::arg *argv);
@ -80,6 +81,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"optflow", test_optflow},
//{"xbee", test_xbee},
{"eedump", test_eedump},
{"logging", test_logging},
// {"rawgps", test_rawgps},
// {"mission", test_mission},
//{"reverse", test_reverse},
@ -1030,6 +1032,31 @@ test_optflow(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;
}
/*
static int8_t
//test_mission(uint8_t argc, const Menu::arg *argv)

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
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() {}

27
ArduPlane/system.pde
View File

@ -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();

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
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)

6
Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml
View File

@ -4,7 +4,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-2560-2.hex</url2560-2>
<name>ArduPlane V2.27 Alpha </name>
<name>ArduPlane V2.27 </name>
<desc></desc>
<format_version>12</format_version>
</Firmware>
@ -12,7 +12,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-2560-2.hex</url2560-2>
<name>ArduPlane V2.27 Alpha HIL</name>
<name>ArduPlane V2.27 HIL</name>
<desc>
#define FLIGHT_MODE_CHANNEL 8
#define FLIGHT_MODE_1 AUTO
@ -47,7 +47,7 @@
<url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-trunk-1280.hex</url>
<url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-trunk-2560.hex</url2560>
<url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-trunk-2560-2.hex</url2560-2>
<name>ArduPlane V2.27 Alpha APM trunk</name>
<name>ArduPlane V2.27 APM trunk</name>
<desc></desc>
<format_version>12</format_version>
</Firmware>

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -64,7 +64,7 @@
#define BIT_I2C_IF_DIS 0x10
#define BIT_INT_STATUS_DATA 0x01
int AP_InertialSensor_MPU6000::_cs_pin;
uint8_t AP_InertialSensor_MPU6000::_cs_pin;
/* pch: by the data sheet, the gyro scale should be 16.4LSB per DPS
* Given the radians conversion factor (0.174532), the gyro scale factor
@ -90,7 +90,7 @@ const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3;
static volatile uint8_t _new_data;
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000( int cs_pin )
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000( uint8_t cs_pin )
{
_cs_pin = cs_pin; /* can't use initializer list, is static */
_gyro.x = 0;
@ -100,10 +100,13 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000( int cs_pin )
_accel.y = 0;
_accel.z = 0;
_temp = 0;
_initialised = 0;
}
void AP_InertialSensor_MPU6000::init( AP_PeriodicProcess * scheduler )
{
if (_initialised) return;
_initialised = 1;
hardware_init();
scheduler->register_process( &AP_InertialSensor_MPU6000::read );
}

11
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -14,7 +14,7 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor
{
public:
AP_InertialSensor_MPU6000( int cs_pin );
AP_InertialSensor_MPU6000( uint8_t cs_pin );
void init( AP_PeriodicProcess * scheduler );
@ -33,14 +33,14 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor
uint32_t sample_time();
void reset_sample_time();
private:
static void read(uint32_t);
static void data_interrupt(void);
static uint8_t register_read( uint8_t reg );
static void register_write( uint8_t reg, uint8_t val );
static void hardware_init();
private:
Vector3f _gyro;
Vector3f _accel;
float _temp;
@ -63,7 +63,10 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor
static int16_t _data[7];
/* TODO deprecate _cs_pin */
static int _cs_pin;
static uint8_t _cs_pin;
// ensure we can't initialise twice
unsigned _initialised:1;
};
#endif // __AP_INERTIAL_SENSOR_MPU6000_H__

2
libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.cpp
View File

@ -1,6 +1,6 @@
#include "AP_PeriodicProcessStub.h"
AP_PeriodicProcessStub::AP_PeriodicProcessStub(int period) {}
AP_PeriodicProcessStub::AP_PeriodicProcessStub(uint8_t period) {}
void AP_PeriodicProcessStub::init( Arduino_Mega_ISR_Registry * isr_reg ){}
void AP_PeriodicProcessStub::register_process(ap_procedure proc) {}
void AP_PeriodicProcessStub::set_failsafe(ap_procedure proc) {}

4
libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.h
View File

@ -9,13 +9,13 @@
class AP_PeriodicProcessStub : public AP_PeriodicProcess
{
public:
AP_PeriodicProcessStub(int period = 0);
AP_PeriodicProcessStub(uint8_t period = 0);
void init( Arduino_Mega_ISR_Registry * isr_reg );
void register_process(ap_procedure proc);
void set_failsafe(ap_procedure proc);
static void run(void);
protected:
static int _period;
static uint8_t _period;
static void (*_proc)(void);
static void (*_failsafe)(void);
};

19
libraries/AP_PeriodicProcess/AP_TimerProcess.cpp
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "AP_TimerProcess.h"
@ -8,13 +9,13 @@ extern "C" {
#include <avr/interrupt.h>
}
int AP_TimerProcess::_period;
uint8_t AP_TimerProcess::_period;
ap_procedure AP_TimerProcess::_proc[AP_TIMERPROCESS_MAX_PROCS];
ap_procedure AP_TimerProcess::_failsafe;
bool AP_TimerProcess::_in_timer_call;
int AP_TimerProcess::_pidx = 0;
uint8_t AP_TimerProcess::_pidx = 0;
AP_TimerProcess::AP_TimerProcess(int period)
AP_TimerProcess::AP_TimerProcess(uint8_t period)
{
_period = period;
}
@ -32,16 +33,26 @@ void AP_TimerProcess::init( Arduino_Mega_ISR_Registry * isr_reg )
_failsafe = NULL;
_in_timer_call = false;
for (int i = 0; i < AP_TIMERPROCESS_MAX_PROCS; i++)
for (uint8_t i = 0; i < AP_TIMERPROCESS_MAX_PROCS; i++)
_proc[i] = NULL;
isr_reg->register_signal( ISR_REGISTRY_TIMER2_OVF, AP_TimerProcess::run);
}
/*
register a process to be called at the timer interrupt rate
*/
void AP_TimerProcess::register_process(ap_procedure proc)
{
// see if its already registered (due to double initialisation
// of a driver)
for (uint8_t i=0; i<_pidx; i++) {
if (_proc[i] == proc) return;
}
cli();
if (_pidx < AP_TIMERPROCESS_MAX_PROCS)
_proc[_pidx++] = proc;
sei();
}
void AP_TimerProcess::set_failsafe(ap_procedure proc)

6
libraries/AP_PeriodicProcess/AP_TimerProcess.h
View File

@ -13,16 +13,16 @@
class AP_TimerProcess : public AP_PeriodicProcess
{
public:
AP_TimerProcess(int period = TIMERPROCESS_PER_DEFAULT);
AP_TimerProcess(uint8_t period = TIMERPROCESS_PER_DEFAULT);
void init( Arduino_Mega_ISR_Registry * isr_reg );
void register_process(ap_procedure proc);
void set_failsafe(ap_procedure proc);
static void run(void);
protected:
static int _period;
static uint8_t _period;
static ap_procedure _proc[AP_TIMERPROCESS_MAX_PROCS];
static ap_procedure _failsafe;
static int _pidx;
static uint8_t _pidx;
static bool _in_timer_call;
};

243
libraries/DataFlash/DataFlash.cpp
View File

@ -185,3 +185,246 @@ uint16_t DataFlash_Class::GetFilePage()
{
return df_FilePage;
}
void DataFlash_Class::EraseAll(void (*delay_cb)(unsigned long))
{
SetFileNumber(0xFFFF);
for(uint16_t j = 1; j <= df_NumPages; j++) {
PageErase(j);
StartWrite(j);
delay_cb(1);
}
// write the logging format in the last page
StartWrite(df_NumPages+1);
WriteLong(DF_LOGGING_FORMAT);
FinishWrite();
}
/*
we need to erase if the logging format has changed
*/
bool DataFlash_Class::NeedErase(void)
{
StartRead(df_NumPages+1);
return ReadLong() != DF_LOGGING_FORMAT;
}
// This function determines the number of whole or partial log files in the DataFlash
// Wholly overwritten files are (of course) lost.
uint8_t DataFlash_Class::get_num_logs(void)
{
uint16_t lastpage;
uint16_t last;
uint16_t first;
if(find_last_page() == 1) return 0;
StartRead(1);
if(GetFileNumber() == 0XFFFF) return 0;
lastpage = find_last_page();
StartRead(lastpage);
last = GetFileNumber();
StartRead(lastpage + 2);
first = GetFileNumber();
if(first > last) {
StartRead(1);
first = GetFileNumber();
}
if(last == first)
{
return 1;
} else {
return (last - first + 1);
}
}
// This function starts a new log file in the DataFlash
void DataFlash_Class::start_new_log(void)
{
uint16_t last_page = find_last_page();
StartRead(last_page);
//Serial.print("last page: "); Serial.println(last_page);
//Serial.print("file #: "); Serial.println(GetFileNumber());
//Serial.print("file page: "); Serial.println(GetFilePage());
if(find_last_log() == 0 || GetFileNumber() == 0xFFFF) {
SetFileNumber(1);
StartWrite(1);
//Serial.println("start log from 0");
return;
}
// Check for log of length 1 page and suppress
if(GetFilePage() <= 1) {
SetFileNumber(GetFileNumber()); // Last log too short, reuse its number
StartWrite(last_page); // and overwrite it
//Serial.println("start log from short");
} else {
if(last_page == 0xFFFF) last_page=0;
SetFileNumber(GetFileNumber()+1);
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.
void DataFlash_Class::get_log_boundaries(uint8_t log_num, int & start_page, int & end_page)
{
int num = get_num_logs();
int look;
if(num == 1)
{
StartRead(df_NumPages);
if(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) {
StartRead(df_NumPages);
if(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)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;
}
bool DataFlash_Class::check_wrapped(void)
{
StartRead(df_NumPages);
if(GetFileNumber() == 0xFFFF)
return 0;
else
return 1;
}
// This funciton finds the last log number
int DataFlash_Class::find_last_log(void)
{
int last_page = find_last_page();
StartRead(last_page);
return GetFileNumber();
}
// This function finds the last page of the last file
int DataFlash_Class::find_last_page(void)
{
uint16_t look;
uint16_t bottom = 1;
uint16_t top = df_NumPages;
uint32_t look_hash;
uint32_t bottom_hash;
uint32_t top_hash;
StartRead(bottom);
bottom_hash = ((long)GetFileNumber()<<16) | GetFilePage();
while(top-bottom > 1) {
look = (top+bottom)/2;
StartRead(look);
look_hash = (long)GetFileNumber()<<16 | 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;
}
}
StartRead(top);
top_hash = ((long)GetFileNumber()<<16) | GetFilePage();
if (top_hash >= 0xFFFF0000) {
top_hash = 0;
}
if (top_hash > bottom_hash) {
return top;
}
return bottom;
}
// This function finds the last page of a particular log file
int DataFlash_Class::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())
{
StartRead(1);
bottom = GetFileNumber();
if (bottom > log_number)
{
bottom = find_last_page();
top = 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;
StartRead(look);
look_hash = (long)GetFileNumber()<<16 | GetFilePage();
if (look_hash >= 0xFFFF0000) look_hash = 0;
if(look_hash > check_hash) {
// move down
top = look;
} else {
// move up
bottom = look;
}
}
StartRead(top);
if (GetFileNumber() == log_number) return top;
StartRead(bottom);
if (GetFileNumber() == log_number) return bottom;
return -1;
}

33
libraries/DataFlash/DataFlash.h
View File

@ -8,6 +8,10 @@
#define DF_OVERWRITE_DATA 1 // 0: When reach the end page stop, 1: Start overwriting from page 1
// the last page holds the log format in first 4 bytes. Please change
// this if (and only if!) the low level format changes
#define DF_LOGGING_FORMAT 0x28122011
class DataFlash_Class
{
private:
@ -27,20 +31,31 @@ class DataFlash_Class
virtual void BufferToPage (unsigned char BufferNum, uint16_t PageAdr, unsigned char wait) = 0;
virtual void PageToBuffer(unsigned char BufferNum, uint16_t PageAdr) = 0;
virtual unsigned char BufferRead (unsigned char BufferNum, uint16_t IntPageAdr) = 0;
virtual void PageErase(uint16_t PageAdr) = 0;
virtual void ChipErase(void) = 0;
// internal high level functions
int find_last_page(void);
int find_last_page_of_log(uint16_t log_number);
bool check_wrapped(void);
public:
unsigned char df_manufacturer;
unsigned char df_device_0;
unsigned char df_device_1;
uint16_t df_device;
uint16_t df_PageSize;
DataFlash_Class() {} // Constructor
virtual void Init(void) = 0;
virtual void ReadManufacturerID() = 0;
virtual bool CardInserted(void) = 0;
int16_t GetPage(void);
int16_t GetWritePage(void);
virtual void PageErase(uint16_t PageAdr) = 0;
virtual void ChipErase(void) = 0;
// erase handling
void EraseAll(void (*delay_cb)(unsigned long));
bool NeedErase(void);
// Write methods
void StartWrite(int16_t PageAdr);
@ -55,12 +70,20 @@ class DataFlash_Class
int16_t ReadInt();
int32_t ReadLong();
// file numbers
void SetFileNumber(uint16_t FileNumber);
uint16_t GetFileNumber();
uint16_t GetFilePage();
uint16_t df_PageSize;
// page handling
uint16_t df_NumPages;
// high level interface
int find_last_log(void);
void get_log_boundaries(uint8_t log_num, int & start_page, int & end_page);
uint8_t get_num_logs(void);
void start_new_log(void);
};
#include "DataFlash_APM1.h"

14
libraries/DataFlash/DataFlash_APM1.cpp
View File

@ -115,7 +115,9 @@ void DataFlash_APM1::Init(void)
// get page size: 512 or 528
df_PageSize=PageSize();
df_NumPages = DF_LAST_PAGE;
// the last page is reserved for config information
df_NumPages = DF_LAST_PAGE - 1;
}
// This function is mainly to test the device
@ -127,13 +129,19 @@ void DataFlash_APM1::ReadManufacturerID()
SPI.transfer(DF_READ_MANUFACTURER_AND_DEVICE_ID);
df_manufacturer = SPI.transfer(0xff);
df_device_0 = SPI.transfer(0xff);
df_device_1 = SPI.transfer(0xff);
df_device = SPI.transfer(0xff);
df_device = (df_device<<8) | SPI.transfer(0xff);
SPI.transfer(0xff);
dataflash_CS_inactive(); // Reset dataflash command decoder
}
bool DataFlash_APM1::CardInserted(void)
{
return true;
}
// Read the status register
byte DataFlash_APM1::ReadStatusReg()
{

5
libraries/DataFlash/DataFlash_APM1.h
View File

@ -18,14 +18,15 @@ class DataFlash_APM1 : public DataFlash_Class
unsigned char ReadStatusReg();
unsigned char ReadStatus();
uint16_t PageSize();
void PageErase (uint16_t PageAdr);
void ChipErase ();
public:
DataFlash_APM1(); // Constructor
void Init();
void ReadManufacturerID();
void PageErase (uint16_t PageAdr);
void ChipErase ();
bool CardInserted(void);
};
#endif // __DATAFLASH_APM1_H__

10
libraries/DataFlash/DataFlash_APM2.cpp
View File

@ -132,7 +132,9 @@ void DataFlash_APM2::Init(void)
// get page size: 512 or 528 (by default: 528)
df_PageSize=PageSize();
df_NumPages = DF_LAST_PAGE;
// the last page is reserved for config information
df_NumPages = DF_LAST_PAGE - 1;
}
// This function is mainly to test the device
@ -145,15 +147,15 @@ void DataFlash_APM2::ReadManufacturerID()
SPI_transfer(DF_READ_MANUFACTURER_AND_DEVICE_ID);
df_manufacturer = SPI_transfer(0xff);
df_device_0 = SPI_transfer(0xff);
df_device_1 = SPI_transfer(0xff);
df_device = SPI_transfer(0xff);
df_device = (df_device<<8) | SPI_transfer(0xff);
SPI_transfer(0xff);
}
// This function return 1 if Card is inserted on SD slot
bool DataFlash_APM2::CardInserted()
{
return (digitalRead(DF_CARDDETECT) != 0);
return (digitalRead(DF_CARDDETECT) == 0);
}
// Read the status register

5
libraries/DataFlash/DataFlash_APM2.h
View File

@ -22,15 +22,14 @@ class DataFlash_APM2 : public DataFlash_Class
unsigned char SPI_transfer(unsigned char data);
void CS_inactive();
void CS_active();
void PageErase (uint16_t PageAdr);
void ChipErase ();
public:
DataFlash_APM2(); // Constructor
void Init();
void ReadManufacturerID();
bool CardInserted();
void PageErase (uint16_t PageAdr);
void ChipErase ();
};
#endif

12
libraries/Desktop/support/DataFlash_APM1.cpp
View File

@ -32,15 +32,21 @@ void DataFlash_APM1::Init(void)
}
}
df_PageSize = DF_PAGE_SIZE;
df_NumPages = DF_NUM_PAGES;
// reserve last page for config information
df_NumPages = DF_NUM_PAGES - 1;
}
// This function is mainly to test the device
void DataFlash_APM1::ReadManufacturerID()
{
df_manufacturer = 1;
df_device_0 = 2;
df_device_1 = 3;
df_device = 0x0203;
}
bool DataFlash_APM1::CardInserted(void)
{
return true;
}
// Read the status register