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

Logging updates 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2250 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-12 21:37:38 +00:00
parent 07bd6d151f
commit 0629bb01b8
3 changed files with 53 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
ArduCopterMega/Log.pde
View File

@ -319,25 +319,6 @@ int find_last_log_page(int bottom_page)
return top_page;
}
// Write a command processing packet. Total length : 19 bytes
//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng)
void Log_Write_Cmd(byte num, struct Location *wp)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CMD_MSG);
DataFlash.WriteByte(num);
DataFlash.WriteByte(wp->id);
DataFlash.WriteByte(wp->p1);
DataFlash.WriteByte(wp->options);
DataFlash.WriteLong(wp->alt);
DataFlash.WriteLong(wp->lat);
DataFlash.WriteLong(wp->lng);
DataFlash.WriteByte(END_BYTE);
}
void Log_Write_Nav_Tuning()
{
Matrix3f tempmat = dcm.get_dcm_matrix();
@ -436,7 +417,7 @@ void Log_Write_GPS()
DataFlash.WriteLong(g_gps->altitude); // 6
DataFlash.WriteLong(current_loc.alt); // 7
DataFlash.WriteInt(g_gps->ground_speed); // 8
DataFlash.WriteInt(g_gps->ground_speed/100); // 8
DataFlash.WriteInt((int)(g_gps->ground_course/100)); // 9
DataFlash.WriteByte(END_BYTE);
@ -444,10 +425,10 @@ void Log_Write_GPS()
// Read a GPS packet
void Log_Read_GPS()
{ // 1 2 3 4 5 6 7 8 9
//GPS, 77361250, 1, 9, 40.0584750, -105.2034500, 166.2600, 2.8100, 0.0600, 266.0000
// 1 2 3 4 5 6 7 8 9
Serial.printf_P(PSTR("GPS, %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
{
Serial.printf_P(PSTR("GPS, %ld, %d, %d, "
"%4.7f, %4.7f, %4.4f, %4.4f, "
"%4.4f, %d\n"),
DataFlash.ReadLong(), // 1 time
(int)DataFlash.ReadByte(), // 2 fix
@ -458,8 +439,8 @@ void Log_Read_GPS()
(float)DataFlash.ReadLong() / 100.0, // 6 gps alt
(float)DataFlash.ReadLong() / 100.0, // 7 sensor alt
(float)DataFlash.ReadInt() / 100.0, // 8 ground speed
(float)DataFlash.ReadInt()); // 9 ground course
DataFlash.ReadInt(), // 8 ground speed
DataFlash.ReadInt()); // 9 ground course
}
@ -624,24 +605,47 @@ void Log_Read_Performance()
Serial.println(" ");
}
// Write a command processing packet.
//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng)
void Log_Write_Cmd(byte num, struct Location *wp)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CMD_MSG);
DataFlash.WriteByte(g.waypoint_total);
DataFlash.WriteByte(num);
DataFlash.WriteByte(wp->id);
DataFlash.WriteByte(wp->p1);
DataFlash.WriteByte(wp->options);
DataFlash.WriteLong(wp->alt);
DataFlash.WriteLong(wp->lat);
DataFlash.WriteLong(wp->lng);
DataFlash.WriteByte(END_BYTE);
}
// Read a command processing packet
void Log_Read_Cmd()
{
byte logvarb;
long logvarl;
Serial.printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"),
Serial.printf_P(PSTR("CMD:"));
for(int i = 1; i <= 4; i++) {
logvarb = DataFlash.ReadByte();
Serial.print(logvarb, DEC);
Serial.print(comma);
}
for(int i = 1; i <= 3; i++) {
logvarl = DataFlash.ReadLong();
Serial.print(logvarl, DEC);
Serial.print(comma);
}
Serial.println(" ");
// WP total
DataFlash.ReadByte(),
// num, id, p1, options
DataFlash.ReadByte(),
DataFlash.ReadByte(),
DataFlash.ReadByte(),
DataFlash.ReadByte(),
// Alt, lat long
DataFlash.ReadLong(),
DataFlash.ReadLong(),
DataFlash.ReadLong());
}
// Write an attitude packet. Total length : 10 bytes

1
ArduCopterMega/commands_logic.pde
View File

@ -423,6 +423,7 @@ bool verify_nav_wp()
char message[30];
sprintf(message,"Reached Command #%i",command_must_index);
gcs.send_text(SEVERITY_LOW,message);
wp_verify_byte = 0;
return true;
}else{
return false;

2
ArduCopterMega/commands_process.pde
View File

@ -6,7 +6,7 @@ void change_command(uint8_t index)
{
struct Location temp = get_command_with_index(index);
if (next_command.id > MAV_CMD_NAV_LAST ){
if (temp.id > MAV_CMD_NAV_LAST ){
gcs.send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
} else {
command_must_index = NO_COMMAND;