controls.h

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 controls.h
*  @brief Controls header file, contains all controls variables, gains, constants, etc.
*  @author Michael Posner (CIS, MEAM '12)
*  @author Timothy Hennelly (ESE '12)
*  @author Jacob Orloff (MEAM '12)
*  
*/

// Definitions --------------------------------------------

// Navigation
#define KP_PSI2PHI 0.25  ///< 45deg of bank / 180deg of error

// Target: North-eastern baseball diamond:
#define target_LAT 399878890  // Commanded latitude (x10^7)
#define target_LON -752113880 // Commanded longitude (x10^7)

//point out by GM, tree circle
//#define target_LAT 399521260
//#define target_LON -751914850

//southwest corner of hill field
//#define target_LAT 399535800
//#define target_LON -751918300

// A Little further out if we wanna change things up:
//#define LAT_C 399882430  // Commanded latitude (x10^7)
//#define LON_C -752103750 // Commanded longitude (x10^7)

// Tolerance radius squared
//#define TRS (DELT_M2GPS_LON(10)*DELT_M2GPS_LON(10))  ///< tolerance radius around drop location (10m)^2 
//#define TRS 9000000  // ~ 30 m?
#define DROP_THRESH 400

// Throttle Control
#define KP_THR 50.0
#define KI_THR 10.0

// Rudder Control
#define KP_RUD 5.0
#define KD_RUD 3.0
#define KI_RUD 1.0

// Elevator Control
#define KP_RP 0.05
#define KP_ELE 40.0//120.0//60.0
#define KI_ELE 10.0//100.0

//Variables ----------------------------------------------
int throttle_mix_result = 0;   ///< Additional rc control to add to the rudder based on throttle

unsigned long ptimer = 0;  ///< Perturbation timer for step input perturbation routine

// Controls switch variables to detect when they go low to high:
int sw1 = 0; // Throttle
int sw2 = 0; // Rudder

// Navigation
long dist2drop = 10000000;
long LAT_C = target_LAT;
long LON_C = target_LON;
long del_LAT = 1000000;
long del_LON = 1000000;
float psi_c = 0.0;
float psi_error = 0;

// Elevator Control
int del_ELE = 0;
float I_ELE = 0;
int rc_ele_trim = 1500;
float kp_ele_gm = 1.0;
float ki_ele_gm = 1.0;
float q_c = 0.0;    // commanded
float q = 0.0;
float alpha_q = 0.5;//0.0385;// 0.0196; 
float q_filtered = 0.0;

// Throttle Control
int updated_ALT = 0;
float old_ALT = 0;
float new_ALT = 0;
float del_ALT = 0;
int del_THR = 0;
int I_THR = 0;
int rc_thr_trim = 1500;
float kp_thr_gm = 1.0;   // gain multiplier determined by rc6
float ki_thr_gm = 1.0;

// Rudder Control
int del_RUD = 0;
int I_RUD = 0;
int rc_rud_trim = 1500;
float kp_rud_gm = 1.0;   // gain multiplier determined by rc6
float kd_rud_gm = 1.0;
float ki_rud_gm = 1.0;
int phi_c = 0;             // commanded phi based on rudder stick rc input