storc.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 storc.pde
 *  @brief Main file, contains Arduino setup() and loop() functions
 *  @author Michael Posner (CIS, MEAM '12)
 *  @author Timothy Hennelly (ESE '12)
 *  @author Jacob Orloff (MEAM '12)
 *  
 */

// Libraries  --------------------------------------------------------
// (they have to be included by this main storc.pde file, not storc.h)

//ArduPilot Mega headers
#include <FastSerial.h>  //ArduPilot Mega library for better serial communication (used by common and GPS library, MUST BE FIRST #include )
#include <AP_Common.h>  //ArduPilot Mega common library
#include <DataFlash.h>  //ArduPilot Mega flash memory library
#include <APM_RC.h>  //ArduPilot Mega library for reading RC inputs and generating PWM pulses on output channels
#include <AP_GPS.h>  //ArduPilot Mega library for reading GPS data from GPS
#include <APM_BMP085.h>  //ArduPilot Mega library for reading the barometer (abs. pressure + temp)
#include <AP_ADC.h>  //ArduPilot Mega library for using ADC
#include <AP_IMU.h>  //ArduPilot Mega general header for including all relevant IMU headers
#include <AP_Compass.h> //ArduPilot Mega magnetometer library
#include <AP_DCM.h>  //ArduPilot Mega dcm library

//AVR headers
#include <stdio.h>

//Arduino headers
#include <SPI.h>  // Arduino SPI lib  (for flash memory communication)
#include <Wire.h>  //Arduino I2C library (used by barometer)

//Project Headers  --------------------------------------------------
#include "storc.h"  //Main storc header file
#include "controls.h"  //Controls header file

//Functions  --------------------------------------------------------

/**
 * Function that runs every so often, as determined by {@link SLOW_LOOP_CLOCK}
 * @see loop
 */
void slow_loop(void) {

  //GPS
  if(use_gps == true) {

    gps->update();

    //debug_gps();

    if(LOG && (logging_sw > 512)) {
      if(gps->new_data) { //If the GPS has new data
        //adjust the GPS time:  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;
        }

        //log gps data in flash memory (these values are sometimes *10^7 or *100, but we fix that in the read function)
        Log_Write_GPS(day,hour,minute,second,gps->latitude,gps->longitude,gps->altitude,gps->ground_speed,gps->ground_course);  

        gps->new_data = 0; // We have read the data

        new_ALT = gps->altitude/100.0; // New altitude reading
        updated_ALT = 1;               // Updated altitude is true
      }
    }



  }

  //Barometer
  if(barometer.Read() == 1) { //if the barometer has new data
    //debug_barometer();
    if(LOG && (logging_sw > 512)) {
      Log_Write_Barometer((float)(barometer.Temp)*0.1 + 273.15, (float)(barometer.Press)/1000.0);  //convert temp to K, press to kPa
    }

  }

  //Log state
  if(LOG) {
    Log_Write_State(drop_state, logging_sw, get_wind_profile, avg_wind_ns, avg_wind_ew, drift_ns, drift_ew);
  }

  //Controls ----------------------------------------------------------
  if(CONTROLS){

    if (rc_5 > 1500){

      //Navigation update
      if(NAVIGATION){

        /*
        // Figure out where to go
         if(drop_state == false){ // If still have not delivered payload, go to target
         LAT_C = target_LAT;
         LON_C = target_LON;
         }else{ // If already delivered, go to start
         LAT_C = gps_start_lat;b
         LON_C = gps_start_lon;
         }
         */
         

        // Check if within Tolerance Radius Squared of the drop site
        //dist2drop = (del_LAT*del_LAT)+(del_LON*del_LON);
        //if(dist2drop < TRS){
        if((abs(del_LAT) < DROP_THRESH) && (abs(del_LON) < DROP_THRESH)) {
          drop_payload();
          drop_state = true;
          LAT_C = gps_start_lat;
          LON_C = gps_start_lon;
          Serial.println("----IN THE SACK----");
        }
        else{
          reset_drop();
          Serial.println("----OUTTA THE SACK----");
        }

        //calculate desired heading to align with destination
        del_LAT = LAT_C - (gps->latitude);
        del_LON = LON_C - (gps->longitude);
        psi_c = atan2(del_LAT,del_LON);

        //convert psi_c to same reference frame as GPS Heading is in (atan2 returns in the range [-pi,pi])
        if(psi_c < 0) {
          psi_c += 2.0*PI;
        }

        psi_c = 2.0*PI-(psi_c-PI/2.0);

        psi_c = RAD2DEG(psi_c); //convert to degrees

        //calculate difference in desired from current heading
        psi_error = psi_c - gps->ground_course/100.0;

        //wrap if error is over 180 or less than -180
        if(psi_error > 180) {
          psi_error -= 360;
        }
        else if (psi_error < -180) {
          psi_error += 360;
        }      

        //calculate bank command based on difference
        if( psi_error < 0 && psi_error > -180 ) { //psi_c is to the left of the plane, turn left

          phi_c = -KP_PSI2PHI*abs(psi_error);
        }
        else {  //psi_c is to the right of the plane, turn right

          phi_c = KP_PSI2PHI*abs(psi_error);
        }

        phi_c = constrain(phi_c,-45,45);

        /*
        Serial.print("H: "); 
        Serial.print(gps->ground_course/100.0);
        Serial.print(", Psi_c: ");  
        Serial.print(psi_c);
        Serial.print(", Phi_c: "); 
        Serial.print(phi_c);
        Serial.print(", d: ");
        if(drop_state) {
          Serial.print("1");
        }
        else {
          Serial.print("0");
        }
        
        Serial.print(", LAT_C: ");
        Serial.print(LAT_C);
        Serial.print(", LON_C: ");
        Serial.print(LON_C);
        Serial.print(", real lat: ");
        Serial.print(gps->latitude);
        Serial.print(", real lon: ");
        Serial.print(gps->longitude);
        Serial.print(", DROP_THRESH: ");
        Serial.print(DROP_THRESH);
        Serial.print(", del_LAT: ");
        Serial.print(del_LAT);
        Serial.print(", del_LON: ");
        Serial.println(del_LON);
        */

        if(LOG && (logging_sw > 512)) {
          Log_Write_Navigation(LAT_C, LON_C, del_LAT, del_LON, psi_c, psi_error, phi_c, DROP_THRESH);
        }
      }

    }

    if ((rc_4 > 1500) && (updated_ALT)){  // Throttle Control

      // If just switched on 
      if(sw1 == 0){
        I_THR = 0;                  // Reset integrator to zero 
        rc_thr_trim = rc_throttle;  // Remember throttle starting position
      }

      sw1 = 1;                                      // Set switch variable to high

      del_THR= 0;                                  // Reset to zero
      THR_control(1.0/(float)(SLOW_LOOP_CLOCK));   // Run the throttle controls algorithm. Use slow_loop timer since that's the sampling rate.
      updated_ALT = 0;

    }
    else{
      sw1 = 0;     // Keep switch variable low
    }

  }

}

/**
 * Function that runs every so often, as determined by {@link MED_LOOP_CLOCK}
 * @see loop
 */
void medium_loop(void) {

  //Read logging switch
  logging_sw = analogRead(LOGGING_SWITCH);  //analogRead returns 0-1023

  //test slide switch
  //slide_sw = digitalRead(SLIDE_SWITCH);
  //Serial.print("slide sw: ");  Serial.println(slide_sw);

  //IMU ---------------------------------------------------------------
  imu.update();
  accel = imu.get_accel();
  gyro = imu.get_gyro();

  //DCM ---------------------------------------------------------------
  //debug_dcm();

  //Magnetometer ------------------------------------------------------
  compass.read();
  compass.calculate(dcm.get_dcm_matrix());  //use the current dcm matrix to adjust calculations
  compass.null_offsets(dcm.get_dcm_matrix());  //re-evaluate offsets

  //debug_magnetometer();

  //Airspeed ----------------------------------------------------------
  update_airspeed();
  //debug_airspeed();

  if(gps->altitude/100.0 > 38+20) {  //if we get high enough, profile over
    get_wind_profile = false;
  }
  
  if(get_wind_profile) {
    //update average wind stuff while we're ascending
    //get current wind speed, split into directions
    float curr_wind_ns = (gps->ground_speed/100.0 - airspeed)*sin(DEG2RAD(gps->ground_course/100.0));
    float curr_wind_ew = (gps->ground_speed/100.0 - airspeed)*cos(DEG2RAD(gps->ground_course/100.0));
  
    //add to running sum
    sum_wind_ns += curr_wind_ns;
    sum_wind_ew += curr_wind_ew;
  
    //increment total number of samples
    total_ns++;
    total_ew++;
  
    //calculate new average
    avg_wind_ns = sum_wind_ns/(float)(total_ns);
    avg_wind_ew = sum_wind_ew/(float)(total_ew);
  }
  else {  //do offset calculations
    
    int s_ns = 1;  //sign of ns wind
    int s_ew = -1;  //sign of ew wind
    
    if(avg_wind_ns < 0) {
      s_ns = -1;
    }
    if(avg_wind_ew < 0) {
      s_ew = -1;
    }
    
    drift_ns = K_DRIFT*(gps->altitude/100.0)*avg_wind_ns+(s_ns*OFFSET_DRIFT);  //drift [m] positive means will drift north  
    drift_ew = K_DRIFT*(gps->altitude/100.0)*avg_wind_ew+(s_ew*OFFSET_DRIFT);  //drift [m] positive means will drift east
    
    //drift is in meters, constrain to not be huge
    drift_ns = constrain(drift_ns, -500,500);
    drift_ew = constrain(drift_ew, -500,500);
    
    //convert drift to GPS
    float drift_ns_gps = DELT_M2GPS_LAT(drift_ns);
    float drift_ew_gps = DELT_M2GPS_LON(drift_ew);
    
    //adjust target
    LAT_C = target_LAT - drift_ns_gps*10000000;
    LON_C = target_LON - drift_ew_gps*10000000;
  
  }

  //RC ----------------------------------------------------------------
  //debug_rc();


  //Controls ----------------------------------------------------------
  if(CONTROLS){

    /* No Longer Doing Elevator Control
     
     if (rc_4 > 1500){  // Elevator Control
     
     // If just switched on 
     if(sw1 == 0){
     I_ELE = 0;                  // Reset integrator to zero 
     rc_ele_trim = rc_elevator;  // Remember servo neutral position
     }
     
     sw1 = 1;                                    // Set switch variable to high
     del_ELE = 0;                                // Reset to zero
     q_c = (float)(KP_RP*(abs(phi_c)));                   // Commanded pitch rate based on rc input
     ELE_control(1.0/(float)(MED_LOOP_CLOCK));   // Run the elevator controls algorithm
     
     }
     else{
     sw1 = 0;     // Keep switch variable low
     }
     */

    if (rc_5 > 1500){  // Rudder Control

      // If just switched on 
      if(sw2 == 0){
        I_RUD = 0;                // Reset integrator to zero
        rc_rud_trim = rc_rudder;  // Remember servo neutral position
      }

      sw2 = 1;                                    // Set switch variable to high
      del_RUD = 0;                                // Reset to zero
      // Get phi_c from navigation (outer loop)
      phi_c = (rc_rudder - rc_rud_trim)/10.0f;    // Commanded bank based on rc input (inner loop)
      RUD_control(1.0/(float)(MED_LOOP_CLOCK));   // Run the rudder controls algorithm

    }
    else{
      sw2 = 0;    // Keep switch variable low
      phi_c = 0.0;
    }

    //debug_controls();

  }
  else{  //PAYLOAD DROP ----------------------------------------------------------------

    //re-zero controls variables incase we are switching mid-flight
    del_THR = 0;
    del_RUD = 0;
    del_ELE = 0;

    //alternate conditions for dropping (auto-based on GPS, or manual) based on whether GPS is active
    boolean condition = false;
    if(use_gps == true) { //if GPS is on
      condition = rc_4 > 1500;
      //condition = gps->latitude > gps_start_lat + 0.0008*10000000; //farther than about 90 m north of start point  //bug, lose rc control
      //condition = gps->latitude < 399518950;
    }
    else { //else do manual
      condition = rc_4 > 1500;
    }

    //drop condition
    if( condition ) {
      drop_payload();
      drop_state = true;
    }
    else {
      reset_drop();
    }

  }

}

/**
 * Function that runs every so often, as determined by {@link FAST_LOOP_CLOCK}
 * @see loop
 */
void fast_loop(void) {

  //update DCM
  dcm.update_DCM(GYRO_INT_TIME);

}


//Setup and Loop  ---------------------------------------------------

/**
 * Arduino setup() function.  This gets called first when the board is powered
 * on.  It sets up the timers and initializes the serial connection as well as 
 * any library initialization.
 */
void setup(void){

  //Setup timers for fast, medium, slow loops (Arduino millis() command returns
  //  current time in milliseconds in an unsigned long)
  fast_timer = millis();
  medium_timer = millis();
  slow_timer = millis();
  telemetry_timer = millis();

  //Initialize LED pins, set for output
  pinMode(A_LED,OUTPUT);
  pinMode(B_LED,OUTPUT);
  pinMode(C_LED,OUTPUT);

  //start LEDs ON, then turn them off as initialization progresses
  digitalWrite(A_LED,HIGH);
  digitalWrite(B_LED,HIGH);
  digitalWrite(C_LED,HIGH);


  //Initialize Serial ---------------------------------------
  Serial.begin(115200);  // Initialize Serial Communication (FTDI on Serial 0)
  Serial1.begin(38400, 128, 16);  // standard gps running
  Serial3.begin(115200);  // Initialize Serial3 for telemetry communication
  Serial.println("---------- StoRC Debug ----------");

  //Initialize Slide switch for input
  pinMode(SLIDE_SWITCH,INPUT);
  slide_sw = digitalRead(SLIDE_SWITCH);

  //slide switch determines whether to use gps or not
  if(slide_sw == 1) {
    use_gps = true;
  }
  else {
    use_gps = false;
  }

  Serial.print("SLIDE SWITCH: ");
  Serial.println(slide_sw);

  //Initialize sensors and libraries ---------------------------------------

  adc.Init(); //initialize adc

  //Only initialize GPS if slide switch has determined we should
  if(use_gps == true) {
    //Set up GPS
    gps = &gps_driver;
    gps->init();
    gps->update();  //force the driver to detect the GPS before checking its status

    //Wait for full GPS lock before proceeding
    Serial.println("GPS Init...");
    while(gps->status() < 2) {
      gps->update();
    }
    Serial.println(" GPS Locked, waiting for altitude to settle...");
    long count = 0;
    while(gps->altitude/100.0 < 20 || gps->altitude/100.0 > 50 ) {
      if(count == 100000) { //(int)(gps->altitude) % 10 == 0)
        Serial.print("   ");  
        Serial.println(gps->altitude/100.0);
        count = 0;
      }
      count++;

      gps->update();
    }
    Serial.println(" Altitude between 30 and 50 meters.");
  }
  else {
    Serial.println("Screw GPS, man...");
  }

  digitalWrite(A_LED,LOW);  //done with GPS intialization

  barometer.Init();  //initialize the barometer

  zero_pitot();  //initialize the airspeed measurement (must come after adc init!)

  imu.init(IMU::COLD_START, delay); //intialize IMU on ground, stationary and level
  Serial.println();

  digitalWrite(B_LED,LOW);  //done with IMU initialization

  //Initialize Magnetometer stuff
  Wire.begin();  //used to communicate with the magnetometer pins

  boolean result = compass.init();
  if(result == true) {
    Serial.print("Compass found: ");
  }
  else {
    Serial.println("Compass initialization failed");
    while (1) {
    };
  }

  compass.set_orientation(AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD);  // set compass's orientation on aircraft.
  compass.set_offsets(0,0,0);  // set offsets to account for surrounding interference
  compass.set_declination(0.213221057);  // set local difference between magnetic north and true north (Philadelphia: 12 deg, 13 min)

  switch( compass.product_id ) {
  case AP_COMPASS_TYPE_HIL:
    Serial.println("HIL");
    break;
  case AP_COMPASS_TYPE_HMC5843:
    Serial.println("HMC5843");
    break;
  case AP_COMPASS_TYPE_HMC5883L:
    Serial.println("HMC5883L");
    break;
  default:
    Serial.println("unknown");
    break;
  }

  //DCM
  dcm.set_compass(&compass);
  dcm.set_centripetal(1);

  //initialize the flash memory
  DataFlash.Init();

  if(LOG) {
    start_new_log(get_num_logs());  //start a fresh log if logging is turned on
  }

  APM_RC.Init();  // Initialize RC library

  //initialize code parameters -----------------------------------

  //Initialize the drop timer
  //drop_timer = -1;  
  reset_drop();


  //Initialize accel and gyro readings
  imu.update();
  accel = imu.get_accel();
  gyro = imu.get_gyro();

  //save starting GPS location
  if(use_gps == true) {
    gps->update();
    gps_start_lat = gps->latitude;
    gps_start_lon = gps->longitude;
  }

  digitalWrite(C_LED,LOW);  //done with initialization
}

/**
 * Arduino loop() function.  This is called immediately after {@link setup} and
 * it loops as long as the board is powered.  This function calls everything else
 * that runs.  Using timers and periods set by {@link SLOW_LOOP_CLOCK}, 
 * {@link MED_LOOP_CLOCK}, and {@link FAST_LOOP_CLOCK}, it determines when to call
 * each subfunction and also calls what needs to run at full speed.
 */
void loop(void){

  //Full speed loop (things that are done every iteration of loop() )

  read_RC();  //Get RC commands from controller

  // Slow Loop check
  if( millis() - slow_timer >= 1000/SLOW_LOOP_CLOCK ) {
    slow_timer = millis();
    slow_loop();
  }

  // Medium Loop check
  if( millis() - medium_timer >= 1000/MED_LOOP_CLOCK ) {
    medium_timer = millis();
    medium_loop();
  }

  // Fast Loop check
  if( millis() - fast_timer >= 1000/FAST_LOOP_CLOCK ) {
    fast_timer = millis();
    fast_loop();
  }

  //telemetry
  if(TELEMETRY) {
    if( millis() - telemetry_timer >= 1000/TELEMETRY_CLOCK ) {
      telemetry_timer = millis();
      transmit_data();
    }
  }

  //important, higher-speed logging
  if(LOG) {
    if( millis() - logging_timer >= 1000/LOGGING_CLOCK ) {
      logging_timer = millis();

      if (logging_sw > 512) {        
        Log_Write_IMU(gyro.x, gyro.y, gyro.z, accel.x, accel.y, accel.z);
        Log_Write_Magnetometer(RAD2DEG(compass.heading), dcm.roll_sensor/100.0, dcm.pitch_sensor/100.0, dcm.yaw_sensor/100.0);
        Log_Write_Airspeed(airspeed);
        Log_Write_RC(rc_elevator,rc_rudder,rc_throttle, rc_4, rc_5, rc_6, rc_7, rc_8);
        Log_Write_Controls_Rudder(KP_RUD, kp_rud_gm, KD_RUD, kd_rud_gm, KI_RUD, ki_rud_gm, I_RUD, del_RUD);
        //Log_Write_Controls_Elevator(KP_ELE, kp_ele_gm, KP_RP, KI_ELE, ki_ele_gm, I_ELE, del_ELE);
        Log_Write_Controls_Throttle(KP_THR, kp_thr_gm, KI_THR, ki_thr_gm, I_THR, del_ALT, del_THR);
      }

    }
  }

  //decide whether to add controls
  if(CONTROLS){

    if (rc_5 > 1500){  // Rudder Control
      rc_rudder = rc_rud_trim + del_RUD;
    }
    if (rc_4 > 1500) {  // Throttle Control
      rc_throttle = rc_thr_trim + del_THR;
    }

  }

  //debug_controls();
  send_RC();  //Send RC commands to outputs

}