zr-v4/Arducopter/Log.pde

// 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);
}