/************************************************************************
* initialize_controllers()
* setup of feedback controllers used in flight core
************************************************************************/
int initialize_controllers(){
cstate.roll_ctrl = generatePID(
config.Droll_KP,
config.Droll_KI,
config.Droll_KD,
.015,
DT);
cstate.pitch_ctrl = generatePID(
config.Dpitch_KP,
config.Dpitch_KI,
config.Dpitch_KD,
.015,
DT);
cstate.yaw_ctrl = generatePID(
config.yaw_KP,
config.yaw_KI,
config.yaw_KD,
.015,
DT);
zero_out_controller();
if(parse_mix_layout(config.rotors, config.rotor_layout)){
return -1;
}
return 0;
}
/************************************************************************
* arm_controller()
* - zero out the controller
* - set the setpoint.armed_state to ARMED
* - enable motors
************************************************************************/
int arm_controller(){
zero_out_controller();
setpoint.arm_state = ARMED;
setGRN(HIGH);
setRED(HIGH);
return 0;
}
/******************************************************************
* on_mode_release()
* toggle between position and angle modes if MiP is paused
*******************************************************************/
int on_mode_release(){
// store whether or not the controller was armed
int was_armed = setpoint.arm_state;
// disarm the controller if necessary
if(was_armed == ARMED){
disarm_controller();
}
// toggle between position and angle modes
if(setpoint.core_mode == POSITION){
setpoint.core_mode = ANGLE;
printf("using core_mode = ANGLE\n");
}
else {
setpoint.core_mode = POSITION;
printf("using core_mode = POSITION\n");
}
// arm the controller if it was armed before swapping modes
if(was_armed == ARMED){
zero_out_controller();
arm_controller();
}
blink_green();
return 0;
}
/************************************************************************
* arm_controller()
* - zero out the controller
* - set the setpoint.armed_state to ARMED
* - enable motors
************************************************************************/
int arm_controller(){
zero_out_controller();
setpoint.arm_state = ARMED;
set_led(GREEN,HIGH);
set_led(RED,HIGH);
return 0;
}
/************************************************************************
* flight_core()
* Hardware Interrupt-Driven Flight Control Loop
* - read sensor values
* - estimate system state
* - read setpoint from flight_stack
* - if is position mode, calculate a new attitude setpoint
* - otherwise use user attitude setpoint
* - calculate and send ESC commands
************************************************************************/
int flight_core(){
// remember previous arm mode to detect transition from DISARMED
static arm_state_t previous_arm_state;
int i; // general purpose
static float new_esc[8];
static float u[4]; // normalized roll, pitch, yaw, throttle, components
/************************************************************************
* Begin control loop if there was a valid interrupt with new IMU data
************************************************************************/
if (mpu9150_read(&mpu) != 0){
return -1;
}
/***********************************************************************
* STATE_ESTIMATION
* read sensors and compute the state regardless of if the controller
* is ARMED or DISARMED
************************************************************************/
// collect new IMU roll/pitch data
// positive roll right according to right hand rule
// MPU9150 driver has incorrect minus sign on Y axis, correct for it here
// positive pitch backwards according to right hand rule
cstate.roll = -(mpu.fusedEuler[VEC3_Y] - cstate.imu_roll_err);
cstate.pitch = mpu.fusedEuler[VEC3_X] - cstate.imu_pitch_err;
// current roll/pitch/yaw rates straight from gyro
// converted to rad/s with default FUll scale range
// raw gyro matches sign on MPU9150 coordinate system, unlike Euler angle
cstate.dRoll = mpu.rawGyro[VEC3_Y] * GYRO_FSR * DEGREE_TO_RAD / 32767.0;
cstate.dPitch = mpu.rawGyro[VEC3_X] * GYRO_FSR * DEGREE_TO_RAD / 32767.0;
cstate.dYaw = mpu.rawGyro[VEC3_Z] * GYRO_FSR * DEGREE_TO_RAD / 32767.0;
// if this is the first loop since being armed, reset yaw trim
if(previous_arm_state==DISARMED && setpoint.arm_state!=DISARMED){
cstate.imu_yaw_on_takeoff = mpu.fusedEuler[VEC3_Z];
cstate.num_yaw_spins = 0;
}
float new_yaw = -(mpu.fusedEuler[VEC3_Z] - cstate.imu_yaw_on_takeoff) \
+ (cstate.num_yaw_spins*2*PI);
// detect the crossover point at Z = +-PI
if(new_yaw - cstate.last_yaw > 6){
cstate.num_yaw_spins -= 1;
}
else if(new_yaw - cstate.last_yaw < -6){
cstate.num_yaw_spins += 1;
}
// also reset yaw if the throttle stick is down to prevent drift while on the ground
if (user_interface.throttle_stick < -0.95){
cstate.imu_yaw_on_takeoff = mpu.fusedEuler[VEC3_Z];
cstate.num_yaw_spins=0;
}
// record new yaw compensating for full rotation
cstate.last_yaw = cstate.yaw;
cstate.yaw = -(mpu.fusedEuler[VEC3_Z] - cstate.imu_yaw_on_takeoff) +
(cstate.num_yaw_spins*2*PI);
/***********************************************************************
* Control based on the robotics_library defined state variable
* PAUSED: make sure the controller stays DISARMED
* RUNNING: Normal operation of controller.
* - check for tipover
* - choose mode from setpoint.core_mode
* - evaluate difference equation and check saturation
* - actuate motors
************************************************************************/
switch (get_state()){
// make sure things are off if program is closing
case EXITING:
return 0;
// if the controller is somehow still ARMED, disarm it
case PAUSED:
if(setpoint.arm_state==ARMED){
disarm_controller();
}
break;
// normal operating mode
case RUNNING:
// exit if the controller was not armed properly
if(setpoint.arm_state==DISARMED){
/************************************************************************
* if disarmed, reset controllers and return
************************************************************************/
zero_out_controller();
goto END;
}
// check for a tipover
if (fabs(cstate.roll) >config.tip_angle ||
fabs(cstate.pitch)>config.tip_angle)
{
disarm_controller();
printf("\n TIPOVER DETECTED \n");
goto END;
}
/************************************************************************
* manage the setpoints based on attitude or position mode
************************************************************************/
switch(setpoint.core_mode){
/************************************************************************
* in Position control mode, evaluate an outer loop controller to
* change the attitude setpoint. Discard user attitude setpoints
************************************************************************/
case POSITION:
// TODO: outer loop position controller
break;
/************************************************************************
* in attitude control mode, user has direct control over throttle
* roll, and pitch angles. Absolute yaw setpoint gets updated at
* user-commanded yaw_rate
************************************************************************/
case ATTITUDE:
// only when flying, update the yaw setpoint
if(setpoint.throttle > YAW_CUTOFF_TH){
setpoint.yaw += DT*setpoint.yaw_rate;
}
else{
setpoint.yaw = cstate.yaw;
}
break;
default:
break; //should never get here
}
/************************************************************************
* Finally run the attitude feedback controllers
************************************************************************/
/************************************************************************
* Roll & Pitch Controllers
************************************************************************/
float dRoll_setpoint = (setpoint.roll - cstate.roll) *
config.roll_rate_per_rad;
float dPitch_setpoint = (setpoint.pitch - cstate.pitch) *
config.pitch_rate_per_rad;
cstate.dRoll_err = dRoll_setpoint - cstate.dRoll;
cstate.dPitch_err = dPitch_setpoint - cstate.dPitch;
// // if last state was DISARMED, then errors will all be 0.
// // make the previous error the same
// if(previous_core_mode == DISARMED){
// preFillFilter(&cstate.roll_ctrl, cstate.dRoll_err);
// preFillFilter(&cstate.pitch_ctrl, cstate.dPitch_err);
// }
// only run integrator if airborne
// TODO: proper landing/takeoff detection
if(u[3] > INT_CUTOFF_TH){
cstate.dRoll_err_integrator += cstate.dRoll_err * DT;
cstate.dPitch_err_integrator += cstate.dPitch_err * DT;
}
marchFilter(&cstate.roll_ctrl, cstate.dRoll_err);
marchFilter(&cstate.pitch_ctrl, cstate.dPitch_err);
if(setpoint.throttle<0.1){
saturateFilter(&cstate.roll_ctrl, -LAND_SATURATION,LAND_SATURATION);
saturateFilter(&cstate.pitch_ctrl, -LAND_SATURATION, LAND_SATURATION);
}
else{
saturateFilter(&cstate.roll_ctrl, -MAX_ROLL_COMPONENT, MAX_ROLL_COMPONENT);
saturateFilter(&cstate.pitch_ctrl, -MAX_PITCH_COMPONENT, MAX_PITCH_COMPONENT);
}
u[0] = cstate.roll_ctrl.current_output;
u[1] = cstate.pitch_ctrl.current_output;
/************************************************************************
* Yaw Controller
************************************************************************/
cstate.yaw_err = setpoint.yaw - cstate.yaw;
// only run integrator if airborne
if(u[3] > INT_CUTOFF_TH){
cstate.yaw_err_integrator += cstate.yaw_err * DT;
}
marchFilter(&cstate.yaw_ctrl, cstate.yaw_err);
if (user_interface.throttle_stick < -0.95){
zeroFilter(&cstate.yaw_ctrl);
}
else{
saturateFilter(&cstate.yaw_ctrl, -MAX_YAW_COMPONENT, MAX_YAW_COMPONENT);
}
u[2] = cstate.yaw_ctrl.current_output;
/************************************************************************
* Throttle Controller
************************************************************************/
// compensate for roll/pitch angle to maintain Z thrust
float throttle_compensation;
throttle_compensation = 1 / cos(cstate.roll);
throttle_compensation *= 1 / cos(cstate.pitch);
float thr = setpoint.throttle*(MAX_THRUST_COMPONENT-config.idle_speed)
+ config.idle_speed;
u[3] = throttle_compensation * thr;
// saturate thrust component now, this will help prevent saturation
// later once r,p,y are added
saturate_number(&u[3], 0, MAX_THRUST_COMPONENT);
/************************************************************************
* see mixing_matrixes.h for how the mixer works
************************************************************************/
if(mix_controls(u[0],u[1],u[2],u[3],&new_esc[0],config.rotors)<0){
printf("ERROR, mixing failed\n");
return -1;
}
/************************************************************************
* Prevent saturation under heavy vertical acceleration by reducing all
* outputs evenly such that the largest doesn't exceed 1
************************************************************************/
// find control output limits
float largest_value = 0;
float smallest_value = 1;
for(i=0;i<config.rotors;i++){
if(new_esc[i]>largest_value){
largest_value = new_esc[i];
}
if(new_esc[i]<smallest_value){
smallest_value=new_esc[i];
}
}
// if upper saturation would have occurred, reduce all outputs evenly
if(largest_value>1){
for(i=0;i<config.rotors;i++){
float offset = largest_value - 1;
new_esc[i]-=offset;
}
}
/************************************************************************
* Send a servo pulse immediately at the end of the control loop.
* Intended to update ESCs exactly once per control timestep
* also record this action to cstate.new_esc_out[] for telemetry
************************************************************************/
// if this is the first time armed, make sure to send minimum
// pulse width to prevent ESCs from going into calibration
if(previous_arm_state == DISARMED){
for(i=0;i<config.rotors;i++){
send_servo_pulse_normalized(i+1,0);
}
}
else{
for(i=0;i<config.rotors;i++){
if(new_esc[i]>1.0){
new_esc[i]=1.0;
}
else if(new_esc[i]<0){
new_esc[i]=0;
}
send_servo_pulse_normalized(i+1,new_esc[i]);
cstate.esc_out[i] = new_esc[i];
cstate.control_u[i] = u[i];
}
}
// // pass new information to the log with add_to_buffer
// // this only puts information in memory, doesn't
// // write to disk immediately
// if(config.enable_logging){
// new_log_entry.roll = cstate.roll;
// new_log_entry.pitch = cstate.pitch;
// new_log_entry.yaw = cstate.yaw;
// new_log_entry.u_0 = cstate.u[0];
// new_log_entry.u_1 = cstate.u[1];
// new_log_entry.u_2 = cstate.u[2];
// new_log_entry.u_3 = cstate.u[3];
// new_log_entry.esc_1 = cstate.esc_out[0];
// new_log_entry.esc_2 = cstate.esc_out[1];
// new_log_entry.esc_3 = cstate.esc_out[2];
// new_log_entry.esc_4 = cstate.esc_out[3];
// new_log_entry.v_batt = cstate.v_batt;
// add_to_buffer(new_log_entry);
// }
break;
default:
break;
} // end of switch(get_state())
END: // allow quick jump to here to clean up the several switch statements
//remember the last state to detect transition from DISARMED to ARMED
previous_arm_state = setpoint.arm_state;
return 0;
}
/******************************************************************
* arm_controller()
* - zero out the controller
* - set the setpoint.armed_state to ARMED
* - enable motors
*******************************************************************/
int arm_controller(){
zero_out_controller();
setpoint.arm_state = ARMED;
enable_motors();
return 0;
}
/******************************************************************
* balance_stack
* This is the medium between the user_interface and setpoint
* structs. dsm2, bluetooth, and mavlink threads may be
* unreliable and shouldn't touch the controller setpoint
* directly. balance_stack and balance_core should be the only
* things touching the controller setpoint.
*******************************************************************/
void* balance_stack(void* ptr){
// wait for IMU to settle
disarm_controller();
usleep(1000000);
usleep(1000000);
usleep(500000);
set_state(RUNNING);
setpoint.core_mode = POSITION; //start in position control
set_led(RED,LOW);
set_led(GREEN,HIGH);
// exiting condition is checked inside the switch case instead
while(1){
switch (get_state()){
case EXITING:
return NULL;
case PAUSED:
// not much to do if paused!
break;
// when running, balance_stack checks if an input mode
// like mavlink, DSM2, or bluetooth is enabled
// and moves the controller setpoints corresponding to
// user input and current controller mode
case RUNNING:
if(setpoint.arm_state==DISARMED){
// check if the user has picked MIP upright before starting again
wait_for_starting_condition();
// user may have pressed the pause button or shut down while waiting
// check before continuing
if(get_state()!=RUNNING){
break;
}
// write a blank log entry to mark this time
log_blank_entry();
// read config each time it's picked up to recognize new
// settings user may have changed
// only actually reads from disk if the file was modified
load_config(&config);
zero_out_controller();
arm_controller();
}
if(user_interface.input_mode == NONE){
// no user input, just keep the controller setpoint at zero
setpoint.theta = 0;
setpoint.phi_dot = 0;
setpoint.gamma_dot = 0;
break;
}
else{
setpoint.core_mode=POSITION;
// scale user input from -1 to 1 to
// the minimum and maximum phi_dot, the rate of change of the
// phi reference angle
// leave setpoint.theta alone as it is set by the core itself
// using the D2 position controller
saturate_float(&user_interface.drive_stick,-1,1);
saturate_float(&user_interface.turn_stick,-1,1);
// use a small deadzone to prevent slow drifts in position
if(fabs(user_interface.drive_stick)<0.03)setpoint.phi_dot = 0;
else if(user_interface.drive_mode == NOVICE) \
setpoint.phi_dot = config.drive_rate_novice*user_interface.drive_stick;
else setpoint.phi_dot = config.drive_rate_advanced*user_interface.drive_stick;
if(fabs(user_interface.turn_stick)<0.03) setpoint.gamma_dot = 0;
else if(user_interface.drive_mode == NOVICE) \
setpoint.gamma_dot = config.turn_rate_novice*user_interface.turn_stick;
else setpoint.gamma_dot = config.turn_rate_advanced*user_interface.turn_stick;
}
break; // end of RUNNING case
default:
break;
} // end of switch get_state()
// run about as fast as the core itself
usleep(1000000/SAMPLE_RATE_HZ);
}
return NULL;
}