log.pde

Go to the documentation of this file.

/***********************************************************
 * StoRC - Search to Rescue Craft
 * MEAM Senior Design 2011-2012
 * 
 * Authors:  Michael Posner (CIS, MEAM '12)
 * Timothy Hennelly (ESE '12)
 * Jacob Orloff (MEAM '12)
 * 
 * (some code from ArduPilot Mega project, found here:
 * http://code.google.com/p/ardupilot-mega/ )
 * 
 * Licensing:
 * 
 * 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/>.
 * 
 ***********************************************************/

/**
*  @file log.pde
*  @brief Flash memory logging functions
*  @author Michael Posner (CIS, MEAM '12)
*  @author Timothy Hennelly (ESE '12)
*  @author Jacob Orloff (MEAM '12)
*  
*  logging file, mostly copied from ArduPilot.  this allows us to write to
 the flash memory.  when a logging Write() function is changed here, make sure its
 appropriate Read() counterpart is changed in the default code you use to
 download the log. (it will give an Error Reading END_BYTE error if the 
 read() formate doesnt match the format that the data was written in below)
 Note:  Instead of modifying the DataFlash library to write floats (kinda hard), 
 instead a hack is to multiply the float value by 10^5 or whatever, then cast to long
 in the Write() function and write it using the long writing function.  Then, in the
 Read() function, cast back to float and divide by 10^5.
*/


/**
*  ArduPilot function.  figures out how many logs are currently in flash memory.
*  @return the number of logs currently in flash memory
*/
byte get_num_logs(void)
{
        int page = 1;
        byte data;
        byte log_step = 0;

        DataFlash.StartRead(1);

        while (page == 1) {
                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++;
                                else
                                        log_step = 0;
                                break;

                        case 2:
                                if(data==LOG_INDEX_MSG){
                                        byte num_logs = DataFlash.ReadByte();
                                        return num_logs;
                                }else{
                                                log_step=0;      // Restart, we have a problem...
                                }
                                break;
                        }
                page = DataFlash.GetPage();
        }
        return 0;
}

/**
*  ArduPilot Mega function.  starts a new log file.
*  @param num_existing_logs  the number of logs already in memory, as returned
*    by @link{get_num_logs}
*  @see get_num_logs
*/
void start_new_log(byte num_existing_logs)
{
        int start_pages[50] = {0,0,0};
        int end_pages[50]       = {0,0,0};

        if(num_existing_logs > 0) {
                for(int i=0;i<num_existing_logs;i++) {
                        get_log_boundaries(i+1,start_pages[i],end_pages[i]);
                }
                end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]);
        }

        if(end_pages[num_existing_logs - 1] < 4095 && num_existing_logs < MAX_NUM_LOGS) {
                if(num_existing_logs > 0)
                        start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1;
                else
                        start_pages[0] = 2;
                num_existing_logs++;
                DataFlash.StartWrite(1);
                DataFlash.WriteByte(HEAD_BYTE1);
                DataFlash.WriteByte(HEAD_BYTE2);
                DataFlash.WriteByte(LOG_INDEX_MSG);
                DataFlash.WriteByte(num_existing_logs);
                for(int i=0;i<MAX_NUM_LOGS;i++) {
                        DataFlash.WriteInt(start_pages[i]);
                        DataFlash.WriteInt(end_pages[i]);
                }
                DataFlash.WriteByte(END_BYTE);
                DataFlash.FinishWrite();
                DataFlash.StartWrite(start_pages[num_existing_logs-1]);
        }
        else{ //logs are full
          Serial.println("--LOGS ARE FULL--");
        }
}

/**
*  ArduPilot Mega function.  figures out the boundaries of the log.  
*/
void get_log_boundaries(byte log_num, int & start_page, int & end_page)
{
        int page                = 1;
        byte data;
        byte log_step   = 0;

        DataFlash.StartRead(1);
        while (page == 1) {
                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++;
                                else
                                        log_step = 0;
                                break;
                        case 2:
                                if(data==LOG_INDEX_MSG){
                                        byte num_logs = DataFlash.ReadByte();
                                        for(int i=0;i<log_num;i++) {
                                                start_page = DataFlash.ReadInt();
                                                end_page = DataFlash.ReadInt();
                                        }
                                        if(log_num==num_logs)
                                                end_page = find_last_log_page(start_page);

                                        return;         // This is the normal exit point
                                }else{
                                                log_step=0;      // Restart, we have a problem...
                                }
                                break;
                        }
                page = DataFlash.GetPage();
        }
        //  Error condition if we reach here with page = 2   TO DO - report condition
}

/**
*  ArduPilot Mega function.  finds the last log page.
*/
int find_last_log_page(int bottom_page)
{
        int top_page = 4096;
        int look_page;
        long check;

        while((top_page - bottom_page) > 1) {
                look_page = (top_page + bottom_page) / 2;
                DataFlash.StartRead(look_page);
                check = DataFlash.ReadLong();
                if(check == (long)0xFFFFFFFF)
                        top_page = look_page;
                else
                        bottom_page = look_page;
        }
        return top_page;
}



/**
*  modified function from ArduPilot Mega.  Logs IMU data.  Be careful when casting from float to long!
*  long is an integer type and cannot store decimals.  so this function takes in floats, but since the 
*  flash library can't write floats (I looked into modifying it, but this hack is easier), I multiply
*  the float by 10000 and then cast to a long.  Then the corresponding Read function divides by 10000
*  before writing to the text file.
*/
void Log_Write_IMU(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float accel_z)
{
        
        DataFlash.WriteByte(HEAD_BYTE1);
        DataFlash.WriteByte(HEAD_BYTE2);
        DataFlash.WriteByte(LOG_IMU_MSG);
        DataFlash.WriteLong(millis());
        DataFlash.WriteLong((long)(gyro_x*10000.0));
        DataFlash.WriteLong((long)(gyro_y*10000.0));
        DataFlash.WriteLong((long)(gyro_z*10000.0));
        DataFlash.WriteLong((long)(accel_x*10000.0));
        DataFlash.WriteLong((long)(accel_y*10000.0));
        DataFlash.WriteLong((long)(accel_z*10000.0));
        DataFlash.WriteByte(END_BYTE);
        
}

/**
* modified function from ArduPilot Mega.  Logs GPS data.  Note that the GPS library already returns
* longitude and latitude as their actual values * 10^7, as well as the altitude, ground speed, and ground course 
* as their actual values * 100, so there's no need to multiply before saving as longs.
* @param log_day the number of the day of the week (0=Sun, 1=Mon, ... 6=Sat)
* @param log_hour the hour of the current day (0 - 23)
* @param log_minute the minute of the current hour (0 - 59)
* @param log_second the second of the current minute (0-59)
* @param log_latitude the GPS latitude, multiplied by 10^7 (this is necessary to log in long format while keeping significant digits)
* @param log_longitude the GPS longitude, multiplied by 10^7 (this is necessary to log in long format while keeping significant digits)
* @param log_altitude the altitude in cm (m * 100)
* @param log_ground_speed the ground speed in cm/s (m/s * 100)
* @param log_ground_course the ground course in hundredths of a degree (deg * 100)
*/
void Log_Write_GPS(     long log_day, long log_hour, long log_minute, long log_second, long log_latitude, long log_longitude, long log_altitude,
                            long log_ground_speed, long log_ground_course)
{
        DataFlash.WriteByte(HEAD_BYTE1);
        DataFlash.WriteByte(HEAD_BYTE2);
        DataFlash.WriteByte(LOG_GPS_MSG);
        DataFlash.WriteLong(millis());
        DataFlash.WriteLong(log_day);
        DataFlash.WriteLong(log_hour);
        DataFlash.WriteLong(log_minute);
        DataFlash.WriteLong(log_second);
        DataFlash.WriteLong(log_latitude);
        DataFlash.WriteLong(log_longitude);
        DataFlash.WriteLong(log_altitude);
        DataFlash.WriteLong(log_ground_speed);
        DataFlash.WriteLong(log_ground_course);
        DataFlash.WriteByte(END_BYTE);
}

/**
*  Logs barometer data.
*/
void Log_Write_Barometer(float temp, float press)
{
        DataFlash.WriteByte(HEAD_BYTE1);
        DataFlash.WriteByte(HEAD_BYTE2);
        DataFlash.WriteByte(LOG_BAROMETER_MSG);
        DataFlash.WriteLong(millis());
        DataFlash.WriteLong((long)(temp*10000.0));
        DataFlash.WriteLong((long)(press*10000.0));
        DataFlash.WriteByte(END_BYTE);
}

/**
*  Logs RC servo values.
*/
void Log_Write_RC(int elevator, int rudder, int throttle, int ch4, int ch5, int ch6, int ch7, int ch8)
{
        DataFlash.WriteByte(HEAD_BYTE1);
        DataFlash.WriteByte(HEAD_BYTE2);
        DataFlash.WriteByte(LOG_RC_MSG);
        DataFlash.WriteLong(millis());
        DataFlash.WriteLong((long)(elevator));
        DataFlash.WriteLong((long)(rudder));
        DataFlash.WriteLong((long)(throttle));
        DataFlash.WriteLong((long)(ch4));
        DataFlash.WriteLong((long)(ch5));
        DataFlash.WriteLong((long)(ch6));
        DataFlash.WriteLong((long)(ch7));
        DataFlash.WriteLong((long)(ch8));
        DataFlash.WriteByte(END_BYTE);
}

/**
*  Logs Airspeed value
*/
void Log_Write_Airspeed(float a)
{
        DataFlash.WriteByte(HEAD_BYTE1);
        DataFlash.WriteByte(HEAD_BYTE2);
        DataFlash.WriteByte(LOG_AIRSPEED_MSG);
        DataFlash.WriteLong(millis());
        DataFlash.WriteLong((long)(a*10000.0));
        DataFlash.WriteByte(END_BYTE);
}


/**
* Logs magnetometer values
*/
void Log_Write_Magnetometer(float h, float r, float p, float y)
{
        DataFlash.WriteByte(HEAD_BYTE1);
        DataFlash.WriteByte(HEAD_BYTE2);
        DataFlash.WriteByte(LOG_MAG_MSG);
        DataFlash.WriteLong(millis());
        DataFlash.WriteLong((long)(h*10000.0));
        DataFlash.WriteLong((long)(r*10000.0));
        DataFlash.WriteLong((long)(p*10000.0));
        DataFlash.WriteLong((long)(y*10000.0));
        DataFlash.WriteByte(END_BYTE);
}


/**
*  Logs rudder controls values
*/
void Log_Write_Controls_Rudder(float kpRud, float kpRudGm, float kdRud, float kdRudGm, float kiRud, float kiRudGm, float iRud, float delta)
{
        DataFlash.WriteByte(HEAD_BYTE1);
        DataFlash.WriteByte(HEAD_BYTE2);
        DataFlash.WriteByte(LOG_CONTROLS_RUDDER_MSG);
        DataFlash.WriteLong(millis());
        DataFlash.WriteLong((long)(kpRud*10000.0));
        DataFlash.WriteLong((long)(kpRudGm*10000.0));
        DataFlash.WriteLong((long)(kdRud*10000.0));
        DataFlash.WriteLong((long)(kdRudGm*10000.0));
        DataFlash.WriteLong((long)(kiRud*10000.0));
        DataFlash.WriteLong((long)(kiRudGm*10000.0));
        DataFlash.WriteLong((long)(iRud*10000.0));
        DataFlash.WriteLong((long)(delta*10000.0));
        DataFlash.WriteByte(END_BYTE);
}

/**
*  Logs elevator controls values
*/
void Log_Write_Controls_Elevator(float kpEle, float kpEleGm, float kpRp, float kiEle, float kiEleGm, float iEle, float delta)
{
        DataFlash.WriteByte(HEAD_BYTE1);
        DataFlash.WriteByte(HEAD_BYTE2);
        DataFlash.WriteByte(LOG_CONTROLS_ELEVATOR_MSG);
        DataFlash.WriteLong(millis());
        DataFlash.WriteLong((long)(kpEle*10000.0));
        DataFlash.WriteLong((long)(kpEleGm*10000.0));
        DataFlash.WriteLong((long)(kpRp*10000.0));
        DataFlash.WriteLong((long)(kiEle*10000.0));
        DataFlash.WriteLong((long)(kiEleGm*10000.0));
        DataFlash.WriteLong((long)(iEle*10000.0));
        DataFlash.WriteLong((long)(delta*10000.0));
        DataFlash.WriteByte(END_BYTE);
}

/**
*  Logs throttle controls values
*/
void Log_Write_Controls_Throttle(float kpThr, float kpThrGm, float kiThr, float kiThrGm, float iThr, float delAlt, float delta)
{
        DataFlash.WriteByte(HEAD_BYTE1);
        DataFlash.WriteByte(HEAD_BYTE2);
        DataFlash.WriteByte(LOG_CONTROLS_THROTTLE_MSG);
        DataFlash.WriteLong(millis());
        DataFlash.WriteLong((long)(kpThr*10000.0));
        DataFlash.WriteLong((long)(kpThrGm*10000.0));
        DataFlash.WriteLong((long)(kiThr*10000.0));
        DataFlash.WriteLong((long)(kiThrGm*10000.0));
        DataFlash.WriteLong((long)(iThr*10000.0));
        DataFlash.WriteLong((long)(delAlt*10000.0));
        DataFlash.WriteLong((long)(delta*10000.0));
        DataFlash.WriteByte(END_BYTE);
}

/**
*  Logs code state (drop status, logging switch)
*/
void Log_Write_State(boolean drop_status, int logging_switch, boolean windProfile, float avgWindNS, float avgWindEW, float driftNS, float driftEW)
{
        DataFlash.WriteByte(HEAD_BYTE1);
        DataFlash.WriteByte(HEAD_BYTE2);
        DataFlash.WriteByte(LOG_STATE_MSG);
        DataFlash.WriteLong(millis());
        
        if(drop_status) {
          DataFlash.WriteByte(1);
        }
        else {
          DataFlash.WriteByte(0);
        }
        DataFlash.WriteInt(logging_switch);
        if(windProfile) {
          DataFlash.WriteByte(1);
        }
        else {
          DataFlash.WriteByte(0);
        }
        DataFlash.WriteLong((long)(avgWindNS*10000.0));
        DataFlash.WriteLong((long)(avgWindEW*10000.0));
        DataFlash.WriteLong((long)(driftNS*10000.0));
        DataFlash.WriteLong((long)(driftEW*10000.0));
        DataFlash.WriteByte(END_BYTE);
}


/**
*  Logs navigation values
*/
void Log_Write_Navigation(long latc, long lonc, long delLat, long delLon, float psic, float psierror, float phic, long dropThresh)
{
        DataFlash.WriteByte(HEAD_BYTE1);
        DataFlash.WriteByte(HEAD_BYTE2);
        DataFlash.WriteByte(LOG_NAVIGATION_MSG);
        DataFlash.WriteLong(millis());
        DataFlash.WriteLong(latc);
        DataFlash.WriteLong(lonc);
        DataFlash.WriteLong(delLat);
        DataFlash.WriteLong(delLon);
        DataFlash.WriteLong((long)(psic*10000.0));
        DataFlash.WriteLong((long)(psierror*10000.0));
        DataFlash.WriteLong((long)(phic*10000.0));
        DataFlash.WriteLong(dropThresh);
        DataFlash.WriteByte(END_BYTE);
}