mirror of https://github.com/ArduPilot/ardupilot
583 lines
14 KiB
Plaintext
583 lines
14 KiB
Plaintext
/*
|
|
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
|
|
#define LOG_PID_MSG 0x0A
|
|
|
|
|
|
// 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
|
|
|
|
// Write a PID control info
|
|
void Log_Write_PID(byte num_PID, int P, int I,int D, int output)
|
|
{
|
|
DataFlash.WriteByte(HEAD_BYTE1);
|
|
DataFlash.WriteByte(HEAD_BYTE2);
|
|
DataFlash.WriteByte(LOG_PID_MSG);
|
|
DataFlash.WriteByte(num_PID);
|
|
DataFlash.WriteInt(P);
|
|
DataFlash.WriteInt(I);
|
|
DataFlash.WriteInt(D);
|
|
DataFlash.WriteInt(output);
|
|
DataFlash.WriteByte(END_BYTE);
|
|
}
|
|
|
|
// Write a Radio packet
|
|
void Log_Write_Radio(int ch1, int ch2, int ch3,int ch4, int ch5, int ch6)
|
|
{
|
|
DataFlash.WriteByte(HEAD_BYTE1);
|
|
DataFlash.WriteByte(HEAD_BYTE2);
|
|
DataFlash.WriteByte(LOG_RADIO_MSG);
|
|
DataFlash.WriteInt(ch1);
|
|
DataFlash.WriteInt(ch2);
|
|
DataFlash.WriteInt(ch3);
|
|
DataFlash.WriteInt(ch4);
|
|
DataFlash.WriteInt(ch5);
|
|
DataFlash.WriteInt(ch6);
|
|
DataFlash.WriteByte(END_BYTE);
|
|
}
|
|
|
|
#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);
|
|
}
|
|
#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 Sensor raw data packet
|
|
void Log_Read_PID()
|
|
{
|
|
Serial.print("PID:");
|
|
Serial.print((int)DataFlash.ReadByte()); // NUM_PID
|
|
Serial.print(",");
|
|
Serial.print(DataFlash.ReadInt()); // P
|
|
Serial.print(",");
|
|
Serial.print(DataFlash.ReadInt()); // I
|
|
Serial.print(",");
|
|
Serial.print(DataFlash.ReadInt()); // D
|
|
Serial.print(",");
|
|
Serial.print(DataFlash.ReadInt()); // output
|
|
Serial.println();
|
|
}
|
|
|
|
// Read an Radio packet
|
|
void Log_Read_Radio()
|
|
{
|
|
Serial.print("RADIO:");
|
|
Serial.print(DataFlash.ReadInt());
|
|
Serial.print(",");
|
|
Serial.print(DataFlash.ReadInt());
|
|
Serial.print(",");
|
|
Serial.print(DataFlash.ReadInt());
|
|
Serial.print(",");
|
|
Serial.print(DataFlash.ReadInt());
|
|
Serial.print(",");
|
|
Serial.print(DataFlash.ReadInt());
|
|
Serial.print(",");
|
|
Serial.print(DataFlash.ReadInt());
|
|
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_PID_MSG){
|
|
Log_Read_PID();
|
|
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);
|
|
}
|
|
|
|
|