debug.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 debug.pde
 *  @brief Useful debugging functions that print to Serial
 *  @author Michael Posner (CIS, MEAM '12)
 *  @author Timothy Hennelly (ESE '12)
 *  @author Jacob Orloff (MEAM '12)
 *  
 *  This file contains functions that print debugging information to Serial.  We don't
 *  want them cluttering up the main file, but they're definitely useful to have and to
 *  keep for sanity checks.
 */


/**
 * Print GPS data for testing / debugging
 */
void debug_gps(void) {

  Serial.print("gps->new_data = ");  
  Serial.println(gps->new_data);
  Serial.print("gps->status() = ");
  Serial.print(gps->status());
  Serial.println("  ( 0:No GPS  1:No FIX  2:GPS Lock )");
  Serial.print("Num Sats: ");
  Serial.println(gps->num_sats);
  Serial.print("time epoch: ");
  Serial.println(gps->epoch());

  if(gps->new_data) {

    //Print time stuff.  In theory the ArduPilot Library's epoch() command should return
    //  what time scale the GPS is currently using, but it's wrong.  this GPS returns
    //  number of milliseconds since the start of the current week.  this is used below.
    Serial.print(" Time: ");
    Serial.print(gps->time);
    Serial.println(" (GPS Millisecond time of the week)");

    //GPS Millisecond time of week -> HH:MM:SS
    long day = floor((gps->time/1000)/SEC_PER_DAY);
    long hour = floor(((gps->time/1000) - day*SEC_PER_DAY)/SEC_PER_HOUR);
    long minute = floor(((gps->time/1000) - day*SEC_PER_DAY - hour*SEC_PER_HOUR)/SEC_PER_MIN);
    long second = floor((gps->time/1000) - day*SEC_PER_DAY - hour*SEC_PER_HOUR - minute*SEC_PER_MIN);

    //convert to EST
    hour -= 5;
    if(hour < 0) {
      hour += 24;
    }

    Serial.print(" Time: ");

    switch(day) {
    case 0:
      Serial.print("Sun");
      break;
    case 1:
      Serial.print("Mon");
      break;
    case 2:
      Serial.print("Tues");
      break;
    case 3:
      Serial.print("Wed");
      break;
    case 4:
      Serial.print("Thurs");
      break;
    case 5:
      Serial.print("Fri");
      break;
    case 6:
      Serial.print("Sat");
      break;
    }

    Serial.print(" ");
    Serial.print(hour);
    Serial.print(":");
    Serial.print(minute);
    Serial.print(":");
    Serial.print(second);
    Serial.println(" (EST)");


    Serial.print(" Lat: ");
    Serial.print(gps->latitude);
    Serial.println(" (x10^7)");
    Serial.print(" Lon: ");
    Serial.print(gps->longitude);
    Serial.println(" (x10^7)");
    Serial.print(" Alt: ");
    Serial.print(gps->altitude/100.0);
    Serial.println(" (in m) ");
    Serial.print(" Ground Speed: ");
    Serial.print(gps->ground_speed/100.0);
    Serial.println(" (in m/s) ");
    Serial.print(" Ground Course: ");
    Serial.print(gps->ground_course/100.0);
    Serial.println(" (in deg)");
    Serial.println();

  }  //end if new_data

}

/**
 * Print barometer data to Serial
 */
void debug_barometer(void) {

  Serial.println("Barometer Data ------");
  Serial.print("Temp: ");
  Serial.print(barometer.Temp*0.1 + 273.15);  //barometer returns in 0.1 deg C, print K
  Serial.println(" K");
  Serial.print("Pressure: ");
  Serial.print(barometer.Press/1000.0);  //barometer returns in Pa, print kPa
  Serial.println(" kPa");

}

/**
 * Print gyro data to Serial
 */
void debug_gyro(void){
  Serial.print("Gyro x=");
  Serial.print(gyro.x);
  Serial.print("; y=");
  Serial.print(gyro.y);
  Serial.print("; z=");
  Serial.println(gyro.z);  
}

/**
 * Print acceleration data to Serial
 */
void debug_accel(void){
  Serial.print("Accel x=");
  Serial.print(accel.x);
  Serial.print("; y=");
  Serial.print(accel.y);
  Serial.print("; z=");
  Serial.println(accel.z);
}


/**
 * Print airspeed data to Serial
 */
void debug_airspeed(void) {

  Serial.print("Pitot Zero: ");
  Serial.print(pitot_pressure_offset);
  Serial.println(" kPa");

  Serial.print("Airspeed: ");
  Serial.print(airspeed);
  Serial.println(" m/s");

}

/**
 * Print RC data to Serial
 */
void debug_rc(void) {

  Serial.print("RC:  CH1(E): ");
  Serial.print(rc_elevator);
  Serial.print(", CH2(R): ");
  Serial.print(rc_rudder);
  Serial.print(", CH3(T): ");
  Serial.print(rc_throttle);
  Serial.print(", CH4: ");
  Serial.print(rc_4);
  Serial.print(", CH5: ");
  Serial.print(rc_5);
  Serial.print(", CH6: ");
  Serial.print(rc_6);
  Serial.print(", CH7: ");
  Serial.print(rc_7);
  Serial.print(", CH8: ");
  Serial.println(rc_8);
}

/**
 * Print magnetometer values to Serial
 */
void debug_magnetometer(void) {
  
  Serial.print("Magnetometer:  heading:");
  Serial.print(RAD2DEG(compass.heading));
  Serial.print(" deg   X: ");
  Serial.print(compass.mag_x);
  Serial.print(", Y: ");
  Serial.print(compass.mag_y);
  Serial.print(", Z: ");
  Serial.println(compass.mag_z);

}

/**
 * Print Controls variables and data to Serial
 */
void debug_controls(void){

  Serial.print(" phi= ");
  Serial.print(dcm.roll_sensor/100);
  Serial.print(" dr_roll_comp= ");
  Serial.print(del_RUD);
  Serial.print(" kd_roll_comp_gm= ");
  Serial.print(kd_rud_gm);
  Serial.print(" kp_roll_comp_gm= ");
  Serial.print(kp_rud_gm);
  Serial.print(" delta_rudder= ");
  Serial.print(del_RUD);
  Serial.print(" rc_rudder= ");
  Serial.println(rc_rudder);


}

/**
* Print data from DCM to Serial
*/
void debug_dcm(void) {
  
  Serial.print("DCM:  ");
  Serial.print("roll: ");
  Serial.print(dcm.roll_sensor/100);
  Serial.print(", pitch: ");
  Serial.print(dcm.pitch_sensor/100);
  Serial.print(", yaw: ");
  Serial.println(dcm.yaw_sensor/100);
  
}