You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
516 lines
12 KiB
516 lines
12 KiB
/* |
|
www.ArduCopter.com - www.DIYDrones.com |
|
Copyright (c) 2010. All rights reserved. |
|
An Open Source Arduino based multicopter. |
|
|
|
File : Log.pde |
|
Version : v1.0, Aug 27, 2010 |
|
Author(s): ArduCopter Team |
|
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, |
|
Jani Hirvinen, Ken McEwans, Roberto Navoni, |
|
Sandro Benigno, Chris Anderson |
|
|
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
|
|
* ************************************************************** * |
|
ChangeLog: |
|
|
|
|
|
* ************************************************************** * |
|
TODO: |
|
|
|
|
|
* ************************************************************** */ |
|
|
|
|
|
// Code to Write and Read packets from DataFlash log memory |
|
// Code to interact with the user to dump or erase logs |
|
|
|
#define HEAD_BYTE1 0xA3 // Decimal 163 |
|
#define HEAD_BYTE2 0x95 // Decimal 149 |
|
#define END_BYTE 0xBA // Decimal 186 |
|
|
|
#define LOG_ATTITUDE_MSG 0x01 |
|
#define LOG_GPS_MSG 0x02 |
|
#define LOG_MODE_MSG 0X03 |
|
#define LOG_CONTROL_TUNING_MSG 0X04 |
|
#define LOG_NAV_TUNING_MSG 0X05 |
|
#define LOG_PERFORMANCE_MSG 0X06 |
|
#define LOG_RAW_MSG 0x07 |
|
#define LOG_RADIO_MSG 0x08 |
|
#define LOG_SENSOR_MSG 0x09 |
|
|
|
|
|
// Function to display available logs and solicit action from the user. |
|
// -------------------------------------------------------------------- |
|
void Print_Log_Menu(void) |
|
{ |
|
int log_start; |
|
int log_end; |
|
eeprom_busy_wait(); |
|
byte last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); eeprom_busy_wait(); |
|
if (last_log_num == 0) { |
|
Serial.println("No logs available for download"); |
|
} else { |
|
Serial.print(last_log_num,DEC); |
|
Serial.println(" logs available for download"); |
|
for(int i=1;i<last_log_num+1;i++) { |
|
log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(i-1)*0x02)); eeprom_busy_wait(); |
|
log_end = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(i)*0x02))-1; eeprom_busy_wait(); |
|
if (i == last_log_num) { |
|
log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE); eeprom_busy_wait(); |
|
} |
|
Serial.print("Log number "); |
|
Serial.print(i); |
|
Serial.print(", start page "); |
|
Serial.print(log_start); |
|
Serial.print(", end page "); |
|
Serial.println(log_end); |
|
} |
|
} |
|
Serial.println(" "); |
|
Serial.println("Input 1 <enter> to dump a log"); |
|
Serial.println("Input 2 <enter> to erase all logs"); |
|
Serial.println("Input 3 <enter> to leave log mode"); |
|
Serial.println(" "); |
|
} |
|
|
|
|
|
// Function to get desired action from the user. |
|
// --------------------------------------------- |
|
byte Get_User_Log_Command(void) |
|
{ |
|
|
|
byte step=0; |
|
byte char_1; |
|
byte char_2; |
|
byte char_3; |
|
|
|
Serial.flush(); |
|
|
|
while(step<3) |
|
{ |
|
if(Serial.available()){ |
|
char_1 = Serial.read(); |
|
char_2 = Serial.read(); |
|
char_3 = Serial.read(); |
|
|
|
if (char_1<0x30 || char_1>0x39) { |
|
return 0; |
|
} else if (char_2 == 0xFF) { |
|
return (char_1 - 0x30); |
|
} else if (char_2<0x30 || char_2>0x39) { |
|
return 0; |
|
} else if (char_2 != 0xFF) { |
|
return 0; |
|
} else { |
|
return ((char_1 - 0x30)*10 + char_2 - 0x30); |
|
} |
|
|
|
} else { |
|
delay(20); |
|
} |
|
} |
|
} |
|
|
|
|
|
|
|
// Write an attitude packet. Total length : 10 bytes |
|
#if LOG_ATTITUDE |
|
void Log_Write_Attitude(int log_roll, int log_pitch, int log_yaw) |
|
{ |
|
DataFlash.WriteByte(HEAD_BYTE1); |
|
DataFlash.WriteByte(HEAD_BYTE2); |
|
DataFlash.WriteByte(LOG_ATTITUDE_MSG); |
|
DataFlash.WriteInt(log_roll); |
|
DataFlash.WriteInt(log_pitch); |
|
DataFlash.WriteInt(log_yaw); |
|
DataFlash.WriteByte(END_BYTE); |
|
} |
|
#endif |
|
|
|
|
|
#ifdef LOG_SEN |
|
// Write a Sensor Raw data packet |
|
void Log_Write_Sensor(int s1, int s2, int s3,int s4, int s5, int s6, int s7) |
|
{ |
|
DataFlash.WriteByte(HEAD_BYTE1); |
|
DataFlash.WriteByte(HEAD_BYTE2); |
|
DataFlash.WriteByte(LOG_SENSOR_MSG); |
|
DataFlash.WriteInt(s1); |
|
DataFlash.WriteInt(s2); |
|
DataFlash.WriteInt(s3); |
|
DataFlash.WriteInt(s4); |
|
DataFlash.WriteInt(s5); |
|
DataFlash.WriteInt(s6); |
|
DataFlash.WriteInt(s7); |
|
DataFlash.WriteByte(END_BYTE); |
|
} |
|
#endif |
|
|
|
#if LOG_PM |
|
// Write a performance monitoring packet. Total length : 19 bytes |
|
void Log_Write_Performance() |
|
{ |
|
DataFlash.WriteByte(HEAD_BYTE1); |
|
DataFlash.WriteByte(HEAD_BYTE2); |
|
DataFlash.WriteByte(LOG_PERFORMANCE_MSG); |
|
DataFlash.WriteLong(millis() - perf_mon_timer); |
|
DataFlash.WriteInt(mainLoop_count); |
|
DataFlash.WriteInt(G_Dt_max); |
|
DataFlash.WriteByte(gyro_sat_count); |
|
DataFlash.WriteByte(adc_constraints); |
|
DataFlash.WriteByte(renorm_sqrt_count); |
|
DataFlash.WriteByte(renorm_blowup_count); |
|
DataFlash.WriteByte(gps_fix_count); |
|
DataFlash.WriteInt((int)(imu_health*1000)); |
|
DataFlash.WriteByte(END_BYTE); |
|
} |
|
#endif |
|
|
|
#if LOG_CTUN |
|
// Write a control tuning packet. Total length : 22 bytes |
|
void Log_Write_Control_Tuning() |
|
{ |
|
DataFlash.WriteByte(HEAD_BYTE1); |
|
DataFlash.WriteByte(HEAD_BYTE2); |
|
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); |
|
DataFlash.WriteInt((int)(servo_out[CH_ROLL]*100)); |
|
DataFlash.WriteInt((int)nav_roll); |
|
DataFlash.WriteInt((int)roll_sensor); |
|
DataFlash.WriteInt((int)(servo_out[CH_PITCH]*100)); |
|
DataFlash.WriteInt((int)nav_pitch); |
|
DataFlash.WriteInt((int)pitch_sensor); |
|
DataFlash.WriteInt((int)(servo_out[CH_THROTTLE]*100)); |
|
DataFlash.WriteInt((int)(servo_out[CH_RUDDER]*100)); |
|
DataFlash.WriteInt((int)AN[5]); |
|
DataFlash.WriteByte(END_BYTE); |
|
} |
|
#endif |
|
|
|
#if LOG_NTUN |
|
// Write a navigation tuning packet. Total length : 18 bytes |
|
void Log_Write_Nav_Tuning() |
|
{ |
|
DataFlash.WriteByte(HEAD_BYTE1); |
|
DataFlash.WriteByte(HEAD_BYTE2); |
|
DataFlash.WriteByte(LOG_NAV_TUNING_MSG); |
|
DataFlash.WriteInt((int)yaw_sensor); |
|
DataFlash.WriteInt((int)wp_distance); |
|
DataFlash.WriteInt((int)target_bearing); |
|
DataFlash.WriteInt((int)nav_bearing); |
|
DataFlash.WriteInt(altitude_error); |
|
DataFlash.WriteInt((int)airspeed_error); |
|
DataFlash.WriteInt((int)(nav_gain_scaler*1000)); |
|
DataFlash.WriteByte(END_BYTE); |
|
} |
|
#endif |
|
|
|
#if LOG_MODE |
|
// Write a mode packet. Total length : 5 bytes |
|
void Log_Write_Mode(byte mode) |
|
{ |
|
DataFlash.WriteByte(HEAD_BYTE1); |
|
DataFlash.WriteByte(HEAD_BYTE2); |
|
DataFlash.WriteByte(LOG_MODE_MSG); |
|
DataFlash.WriteByte(mode); |
|
DataFlash.WriteByte(END_BYTE); |
|
} |
|
#endif |
|
|
|
#if LOG_GPS |
|
// Write an GPS packet. Total length : 30 bytes |
|
void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_mix_alt, long log_gps_alt, |
|
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) |
|
{ |
|
DataFlash.WriteByte(HEAD_BYTE1); |
|
DataFlash.WriteByte(HEAD_BYTE2); |
|
DataFlash.WriteByte(LOG_GPS_MSG); |
|
DataFlash.WriteLong(log_Time); |
|
DataFlash.WriteByte(log_Fix); |
|
DataFlash.WriteByte(log_NumSats); |
|
DataFlash.WriteLong(log_Lattitude); |
|
DataFlash.WriteLong(log_Longitude); |
|
DataFlash.WriteLong(log_mix_alt); |
|
DataFlash.WriteLong(log_gps_alt); |
|
DataFlash.WriteLong(log_Ground_Speed); |
|
DataFlash.WriteLong(log_Ground_Course); |
|
DataFlash.WriteByte(END_BYTE); |
|
DataFlash.WriteByte(END_BYTE); |
|
} |
|
#endif |
|
|
|
#if LOG_RAW |
|
// Write an raw accel/gyro data packet. Total length : 28 bytes |
|
void Log_Write_Raw() |
|
{ |
|
DataFlash.WriteByte(HEAD_BYTE1); |
|
DataFlash.WriteByte(HEAD_BYTE2); |
|
DataFlash.WriteByte(LOG_RAW_MSG); |
|
for(int i=0;i<6;i++) |
|
DataFlash.WriteLong((long)(AN[i]*1000.0)); |
|
DataFlash.WriteByte(END_BYTE); |
|
} |
|
#endif |
|
|
|
|
|
// Read an control tuning packet |
|
void Log_Read_Control_Tuning() |
|
{ |
|
float logvar; |
|
|
|
Serial.print("CTUN:"); |
|
for (int y=1;y<10;y++) { |
|
logvar = DataFlash.ReadInt(); |
|
if(y < 9) logvar = logvar/100.f; |
|
Serial.print(logvar); |
|
Serial.print(","); |
|
} |
|
Serial.println(" "); |
|
} |
|
|
|
// Read a nav tuning packet |
|
void Log_Read_Nav_Tuning() |
|
{ |
|
float logvar; |
|
|
|
Serial.print("NTUN:"); |
|
for (int y=1;y<8;y++) { |
|
logvar = DataFlash.ReadInt(); |
|
if(y > 6) logvar = logvar/1000.f; |
|
Serial.print(logvar); |
|
Serial.print(","); |
|
} |
|
Serial.println(" "); |
|
} |
|
|
|
// Read a performance packet |
|
void Log_Read_Performance() |
|
{ |
|
long pm_time; |
|
int logvar; |
|
|
|
Serial.print("PM:"); |
|
pm_time = DataFlash.ReadLong(); |
|
Serial.print(pm_time); |
|
Serial.print(","); |
|
for (int y=1;y<9;y++) { |
|
if(y<3 || y>7) logvar = DataFlash.ReadInt(); |
|
else logvar = DataFlash.ReadByte(); |
|
if(y > 7) logvar = logvar/1000.f; |
|
Serial.print(logvar); |
|
Serial.print(","); |
|
} |
|
Serial.println(" "); |
|
} |
|
|
|
// Read an attitude packet |
|
void Log_Read_Attitude() |
|
{ |
|
int log_roll; |
|
int log_pitch; |
|
int log_yaw; |
|
|
|
log_roll = DataFlash.ReadInt(); |
|
log_pitch = DataFlash.ReadInt(); |
|
log_yaw = DataFlash.ReadInt(); |
|
Serial.print("ATT:"); |
|
Serial.print(log_roll); |
|
Serial.print(","); |
|
Serial.print(log_pitch); |
|
Serial.print(","); |
|
Serial.print(log_yaw); |
|
Serial.println(); |
|
} |
|
|
|
// Read a mode packet |
|
void Log_Read_Mode() |
|
{ |
|
byte mode; |
|
|
|
mode = DataFlash.ReadByte(); |
|
Serial.print("MOD:"); |
|
switch (mode) { |
|
case 0: |
|
Serial.println("Manual mode"); |
|
break; |
|
case 1: |
|
Serial.println("Stabilize mode"); |
|
break; |
|
case 5: |
|
Serial.println("Fly By Wire A mode"); |
|
break; |
|
case 6: |
|
Serial.println("Fly By Wire B mode"); |
|
break; |
|
case 10: |
|
Serial.println("AUTO mode"); |
|
break; |
|
case 11: |
|
Serial.println("RTL mode"); |
|
break; |
|
case 12: |
|
Serial.println("Loiter mode"); |
|
break; |
|
case 98: |
|
Serial.println("Air Start Complete"); |
|
break; |
|
case 99: |
|
Serial.println("Ground Start Complete"); |
|
break; |
|
} |
|
} |
|
|
|
// Read a GPS packet |
|
void Log_Read_GPS() |
|
{ |
|
long log_Time; |
|
byte log_Fix; |
|
byte log_NumSats; |
|
long log_Lattitude; |
|
long log_Longitude; |
|
long log_gps_alt; |
|
long log_mix_alt; |
|
float log_Ground_Speed; |
|
float log_Ground_Course; |
|
|
|
log_Time = DataFlash.ReadLong(); |
|
log_Fix = DataFlash.ReadByte(); |
|
log_NumSats = DataFlash.ReadByte(); |
|
log_Lattitude = DataFlash.ReadLong(); |
|
log_Longitude = DataFlash.ReadLong(); |
|
log_mix_alt = DataFlash.ReadLong(); |
|
log_gps_alt = DataFlash.ReadLong(); |
|
log_Ground_Speed = DataFlash.ReadLong(); |
|
log_Ground_Course = DataFlash.ReadLong(); |
|
|
|
Serial.print("GPS:"); |
|
Serial.print(log_Time); |
|
Serial.print(","); |
|
Serial.print((int)log_Fix); |
|
Serial.print(","); |
|
Serial.print((int)log_NumSats); |
|
Serial.print(","); |
|
Serial.print(log_Lattitude); |
|
Serial.print(","); |
|
Serial.print(log_Longitude); |
|
Serial.print(","); |
|
Serial.print(log_mix_alt/100.0); |
|
Serial.print(","); |
|
Serial.print(log_gps_alt/100.0); |
|
Serial.print(","); |
|
Serial.print(log_Ground_Speed/100.0f); |
|
Serial.print(","); |
|
Serial.print(log_Ground_Course/100.0); // This is incorrect!!!!! |
|
Serial.println(); |
|
|
|
} |
|
|
|
// Read a raw accel/gyro packet |
|
void Log_Read_Raw() |
|
{ |
|
float logvar; |
|
|
|
Serial.print("RAW:"); |
|
for (int y=0;y<6;y++) { |
|
logvar = DataFlash.ReadLong()/1000.f; |
|
Serial.print(logvar); |
|
Serial.print(","); |
|
} |
|
Serial.println(" "); |
|
} |
|
|
|
// Read the DataFlash log memory : Packet Parser |
|
void Log_Read(int start_page, int end_page) |
|
{ |
|
byte data; |
|
byte log_step=0; |
|
int packet_count=0; |
|
int flash_led = 1; |
|
|
|
DataFlash.StartRead(start_page); |
|
while (DataFlash.GetPage() < end_page) |
|
{ |
|
data = DataFlash.ReadByte(); |
|
switch(log_step) //This is a state machine to read the packets |
|
{ |
|
case 0: |
|
if(data==HEAD_BYTE1) // Head byte 1 |
|
log_step++; |
|
break; |
|
case 1: |
|
if(data==HEAD_BYTE2) // Head byte 2 |
|
log_step++; |
|
break; |
|
case 2: |
|
if(data==LOG_ATTITUDE_MSG){ |
|
Log_Read_Attitude(); |
|
log_step++; |
|
|
|
}else if(data==LOG_MODE_MSG){ |
|
Log_Read_Mode(); |
|
log_step++; |
|
|
|
}else if(data==LOG_CONTROL_TUNING_MSG){ |
|
Log_Read_Control_Tuning(); |
|
log_step++; |
|
|
|
}else if(data==LOG_NAV_TUNING_MSG){ |
|
Log_Read_Nav_Tuning(); |
|
log_step++; |
|
|
|
}else if(data==LOG_PERFORMANCE_MSG){ |
|
Log_Read_Performance(); |
|
log_step++; |
|
|
|
}else if(data==LOG_RAW_MSG){ |
|
Log_Read_Raw(); |
|
log_step++; |
|
}else { |
|
if(data==LOG_GPS_MSG){ |
|
Log_Read_GPS(); |
|
log_step++; |
|
} else { |
|
Serial.print("Error Reading Packet: "); |
|
Serial.print(packet_count); |
|
log_step=0; // Restart, we have a problem... |
|
} |
|
} |
|
break; |
|
case 3: |
|
if(data==END_BYTE){ |
|
packet_count++; |
|
}else{ |
|
Serial.print("Error Reading END_BYTE "); |
|
Serial.println(data,DEC); |
|
} |
|
log_step=0; // Restart sequence: new packet... |
|
if(flash_led > 0) { |
|
digitalWrite(A_LED_PIN,HIGH); |
|
flash_led++; |
|
if(flash_led>100) flash_led=-1; |
|
} else { |
|
digitalWrite(A_LED_PIN,LOW); |
|
flash_led--; |
|
if(flash_led<-100) flash_led=1; |
|
} |
|
break; |
|
} |
|
} |
|
Serial.print("Number of packets read: "); |
|
Serial.println(packet_count); |
|
digitalWrite(A_LED_PIN,HIGH); |
|
} |
|
|
|
|
|
|