telemetry.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 telemetry.pde
 *  @brief Functions for sending telemetry data over Serial3 to the Xbee
 *  @author Michael Posner (CIS, MEAM '12)
 *  @author Timothy Hennelly (ESE '12)
 *  @author Jacob Orloff (MEAM '12)
 *  
 */


void transmit_imu(void) {
  
  Serial3.println("IM");
  Serial3.println(millis());
  Serial3.println(accel.x);
  Serial3.println(accel.y);
  Serial3.println(accel.z);
  Serial3.println(gyro.x);
  Serial3.println(gyro.y);
  Serial3.println(gyro.z);

}

void transmit_gps(void) {

  //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;
  }
  
  Serial3.println("GP");
  Serial3.println(millis());
  Serial3.println(day);
  Serial3.println(hour);
  Serial3.println(minute);
  Serial3.println(second);

  Serial3.println(gps->latitude);  //these are *10^7, will correct in MATLAB
  Serial3.println(gps->longitude);  //these are *10^7, will correct in MATLAB
  Serial3.println(gps->altitude/100.0);
  Serial3.println(gps->ground_speed/100.0);
  Serial3.println(gps->ground_course/100.0);


}

void transmit_barometer(void) {

  Serial3.println("BA");
  Serial3.println(millis());
  Serial3.println(barometer.Temp*0.1 + 273.15);  //barometer returns in 0.1 deg C, print K
  Serial3.println(barometer.Press/1000.0);  //barometer returns in Pa, print kPa
  
}

void transmit_rc(void) {

  Serial3.println("RC");
  Serial3.println(millis());
  Serial3.println(rc_elevator);
  Serial3.println(rc_rudder);
  Serial3.println(rc_throttle);
  Serial3.println(rc_4);
  Serial3.println(rc_5);
  Serial3.println(rc_6);
  Serial3.println(rc_7);
  Serial3.println(rc_8);
  
}

void transmit_airspeed(void) {

  Serial3.println("AR");
  Serial3.println(millis());
  Serial3.println(pitot_pressure_offset);
  Serial3.println(airspeed);

}

void transmit_magnetometer(void) {

  Serial3.println("MG");
  Serial3.println(millis());
  Serial3.println(RAD2DEG(compass.heading));
  Serial3.println(dcm.roll_sensor/100.0);
  Serial3.println(dcm.pitch_sensor/100.0);
  Serial3.println(dcm.yaw_sensor/100.0);

}

void transmit_controls_rudder(void) {
  
  Serial3.println("CR");
  Serial3.println(millis());
  
  Serial3.println(KP_RUD);
  Serial3.println(kp_rud_gm);
  Serial3.println(KD_RUD);
  Serial3.println(kd_rud_gm);
  Serial3.println(KI_RUD);
  Serial3.println(ki_rud_gm);
  Serial3.println(I_RUD);
  Serial3.println(del_RUD);
  
}

void transmit_controls_elevator(void) {
  
  Serial3.println("CE");
  Serial3.println(millis());
  
  Serial3.println(KP_ELE);
  Serial3.println(kp_ele_gm);
  Serial3.println(KP_RP);
  Serial3.println(KI_ELE);
  Serial3.println(ki_ele_gm);
  Serial3.println(I_ELE);
  Serial3.println(del_ELE);
  
}

void transmit_controls_throttle(void) {
  
  Serial3.println("CT");
  Serial3.println(millis());
  
  Serial3.println(KP_THR);
  Serial3.println(kp_thr_gm);
  Serial3.println(KI_THR);
  Serial3.println(ki_thr_gm);
  Serial3.println(I_THR);
  Serial3.println(del_ALT);
  Serial3.println(del_THR);
  
}

void transmit_state(void) {
  
  Serial3.println("ST");
  Serial3.println(millis());
  
  if(drop_state) {
    Serial3.println(1);
  }
  else {
    Serial3.println(0);
  }
  
  Serial3.println(logging_sw);
  
  if(get_wind_profile) {
    Serial3.println(1);
  }
  else {
    Serial3.println(0);
  }
  Serial3.println(avg_wind_ns);
  Serial3.println(avg_wind_ew);
  Serial3.println(drift_ns);
  Serial3.println(drift_ew);
  
}

void transmit_navigation(void) {
  
  Serial3.println("NA");
  Serial3.println(millis());
  
  Serial3.println(LAT_C);
  Serial3.println(LON_C);
  Serial3.println(del_LAT);
  Serial3.println(del_LON);
  Serial3.println(psi_c);
  Serial3.println(psi_error);
  Serial3.println(phi_c);
  Serial3.println(DROP_THRESH);
  
}


void transmit_data(void) {

  transmit_imu();
  
  if(use_gps == true) {
    transmit_gps();
  }
  
  transmit_barometer();
  transmit_rc();
  transmit_airspeed();
  transmit_magnetometer();
  transmit_controls_rudder();
  transmit_controls_elevator();
  transmit_controls_throttle();
  transmit_state();
  transmit_navigation();
  
}