ardupilot/Arducopter/Log.pde

268 lines
6.4 KiB
Plaintext

// Test code to Write and Read packets from DataFlash log memory
// Packets : Attitude, Raw sensor data, Radio and GPS
#define HEAD_BYTE1 0xA3
#define HEAD_BYTE2 0x95
#define END_BYTE 0xBA
#define LOG_ATTITUDE_MSG 0x01
#define LOG_GPS_MSG 0x02
#define LOG_RADIO_MSG 0x03
#define LOG_SENSOR_MSG 0x04
#define LOG_PID_MSG 0x05
// 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);
}
// Write an attitude packet. Total length : 10 bytes
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);
}
// 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 an GPS packet. Total length : 30 bytes
void Log_Write_GPS(long log_Time, long log_Lattitude, long log_Longitude, long log_Altitude,
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_Altitude);
DataFlash.WriteLong(log_Ground_Speed);
DataFlash.WriteLong(log_Ground_Course);
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);
}
// **** READ ROUTINES ****
// Read a Sensor raw data packet
void Log_Read_Sensor()
{
SerPri("SENSOR:");
SerPri(DataFlash.ReadInt()); // GX
SerPri(",");
SerPri(DataFlash.ReadInt()); // GY
SerPri(",");
SerPri(DataFlash.ReadInt()); // GZ
SerPri(",");
SerPri(DataFlash.ReadInt()); // ACCX
SerPri(",");
SerPri(DataFlash.ReadInt()); // ACCY
SerPri(",");
SerPri(DataFlash.ReadInt()); // ACCZ
SerPri(",");
SerPri(DataFlash.ReadInt()); // AUX
SerPriln();
}
// 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();
SerPri("ATT:");
SerPri(log_roll);
SerPri(",");
SerPri(log_pitch);
SerPri(",");
SerPri(log_yaw);
SerPriln();
}
// Read a Sensor raw data packet
void Log_Read_PID()
{
SerPri("PID:");
SerPri((int)DataFlash.ReadByte()); // NUM_PID
SerPri(",");
SerPri(DataFlash.ReadInt()); // P
SerPri(",");
SerPri(DataFlash.ReadInt()); // I
SerPri(",");
SerPri(DataFlash.ReadInt()); // D
SerPri(",");
SerPri(DataFlash.ReadInt()); // output
SerPriln();
}
// 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_Altitude;
long log_Ground_Speed;
long log_Ground_Course;
log_Time = DataFlash.ReadLong();
log_Fix = DataFlash.ReadByte();
log_NumSats = DataFlash.ReadByte();
log_Lattitude = DataFlash.ReadLong();
log_Longitude = DataFlash.ReadLong();
log_Altitude = DataFlash.ReadLong();
log_Ground_Speed = DataFlash.ReadLong();
log_Ground_Course = DataFlash.ReadLong();
SerPri("GPS:");
SerPri(log_Time);
SerPri(",");
SerPri((int)log_Fix);
SerPri(",");
SerPri((int)log_NumSats);
SerPri(",");
SerPri(log_Lattitude);
SerPri(",");
SerPri(log_Longitude);
SerPri(",");
SerPri(log_Altitude);
SerPri(",");
SerPri(log_Ground_Speed);
SerPri(",");
SerPri(log_Ground_Course);
SerPriln();
}
// Read an Radio packet
void Log_Read_Radio()
{
SerPri("RADIO:");
SerPri(DataFlash.ReadInt());
SerPri(",");
SerPri(DataFlash.ReadInt());
SerPri(",");
SerPri(DataFlash.ReadInt());
SerPri(",");
SerPri(DataFlash.ReadInt());
SerPri(",");
SerPri(DataFlash.ReadInt());
SerPri(",");
SerPri(DataFlash.ReadInt());
SerPriln();
}
// Read the DataFlash log memory : Packet Parser
void Log_Read(int start_page, int end_page)
{
byte data;
byte log_step=0;
long packet_count=0;
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:
switch (data)
{
case LOG_ATTITUDE_MSG:
Log_Read_Attitude();
log_step++;
break;
case LOG_GPS_MSG:
Log_Read_GPS();
log_step++;
break;
case LOG_RADIO_MSG:
Log_Read_Radio();
log_step++;
break;
case LOG_SENSOR_MSG:
Log_Read_Sensor();
log_step++;
break;
case LOG_PID_MSG:
Log_Read_PID();
log_step++;
break;
default:
SerPri("Error Reading Packet: ");
SerPri(packet_count);
log_step=0; // Restart, we have a problem...
}
break;
case 3:
if(data==END_BYTE)
packet_count++;
else
SerPriln("Error Reading END_BYTE");
log_step=0; // Restart sequence: new packet...
}
}
SerPri("Number of packets read: ");
SerPriln(packet_count);
}