/******************************************************************
* 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;
}
/************************************************************************
* on_pause_press
* If the user holds the pause button for a second, exit cleanly
* disarm on momentary press
************************************************************************/
int on_pause_press(){
int i=0;
const int samples = 100; // check for release 100 times in this period
const int us_wait = 2000000; // 2 seconds
switch(get_state()){
// pause if running
case EXITING:
return 0;
case RUNNING:
set_state(PAUSED);
disarm_controller();
setRED(HIGH);
setGRN(LOW);
break;
case PAUSED:
set_state(RUNNING);
disarm_controller();
setGRN(HIGH);
setRED(LOW);
break;
default:
break;
}
// now wait to see if the user wants to shut down the program
while(i<samples){
usleep(us_wait/samples);
if(get_pause_button_state() == UNPRESSED){
return 0; //user let go before time-out
}
i++;
}
printf("long press detected, shutting down\n");
//user held the button down long enough, blink and exit cleanly
blink_red();
set_state(EXITING);
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;
}
/************************************************************************
* flight_stack()
* Translates the flight mode and user controls from user_interface
* into setpoints for the flight_core position and attitude controller
*
* If the core gets disarmed by another thread, flight_stack manages
* recognizing the rearming sequence
*
* The flight_core only takes setpoint values for feedback control,
************************************************************************/
void* flight_stack(void* ptr){
int i;
//flight_mode_t previous_flight_mode;
// wait for IMU to settle
disarm_controller();
usleep(1000000);
usleep(1000000);
usleep(500000);
set_state(RUNNING);
setpoint.core_mode = POSITION; //start in position control
setRED(LOW);
setGRN(HIGH);
// run until state indicates thread should close
while(1){
switch (get_state()){
case EXITING:
return NULL;
case PAUSED:
// not much to do if paused!
break;
case RUNNING:
// if the core got disarmed, wait for arming sequence
if(setpoint.arm_state == DISARMED){
wait_for_arming_sequence();
// user may have pressed the pause button or shut down while waiting
// check before continuing
if(get_state()!=RUNNING){
break;
}
// 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);
initialize_controllers();
// // write a blank log entry to mark this time
// log_blank_entry();
// wake ESCs up at minimum throttle to avoid calibration mode
// flight_core also sends one minimum pulse at first when armed
for(i=0; i<config.rotors; i++){
send_servo_pulse_normalized(i+1,0);
}
usleep(10000);
arm_controller();
}
// kill switches seem to be fine
// switch behaviour based on user flight mode
if(setpoint.arm_state == ARMED){
// shutdown core on kill switch
if(user_interface.kill_switch == DISARMED){
//printf("stack disarmed controller\n");
disarm_controller();
break;
}
if(user_interface.input_mode==NONE){
// no input devices connected, keep things zero'd
disarm_controller();
break;
}
else{
// decide how to change setpoint based on flight mode
switch(user_interface.flight_mode){
// Raw attitude mode lets user control the inner attitude loop directly
case USER_ATTITUDE:
setpoint.core_mode = ATTITUDE;
// translate throttle stick (-1,1) to throttle (0,1)
setpoint.throttle = (user_interface.throttle_stick + 1.0)/2.0;
// scale roll and pitch angle by max setpoint in rad
setpoint.roll = user_interface.roll_stick *
config.max_roll_setpoint;
setpoint.pitch = user_interface.pitch_stick *
config.max_pitch_setpoint;
// scale yaw_rate by max yaw rate in rad/s
setpoint.yaw_rate = user_interface.yaw_stick *
config.max_yaw_rate;
break;
// emergency land just sets the throttle low for now
// TODO: gently lower altitude till landing detected
case EMERGENCY_LAND:
setpoint.core_mode = ATTITUDE;
setpoint.throttle = config.land_throttle;
setpoint.roll = 0;
setpoint.pitch = 0;
setpoint.yaw_rate = 0;
break;
// TODO: other modes
// case USER_LOITER:
// break;
// case USER_POSITION_CARTESIAN:
// break;
// case USER_POSITION_RADIAL:
// break;
// case TARGET_HOLD:
// break;
default:
break;
}
}
}
default:
break;
}// end of switch(get_state())
// // record previous flight mode to detect changes
// previous_flight_mode = user_interface.flight_mode;
// run about as fast as the core itself
usleep(1000000/SAMPLE_RATE_HZ);
}
return NULL;
}
/******************************************************************
* balance_core()
* discrete-time balance controller operated off IMU interrupt
* Called at SAMPLE_RATE_HZ
*******************************************************************/
int balance_core(){
// local variables only in memory scope of balance_core
static int D1_saturation_counter = 0;
float compensated_D1_output = 0;
float dutyL = 0;
float dutyR = 0;
static log_entry_t new_log_entry;
float output_scale; //battery voltage/nominal voltage
// if an IMU packet read failed, ignore and just return
// the mpu9150_read function may print it's own warnings
if (mpu9150_read(&mpu) != 0){
return -1;
}
/**************************************************************
* STATE_ESTIMATION
* read sensors and compute the state regardless of if the
* controller is ARMED or DISARMED
***************************************************************/
// angle theta is positive in the direction of forward tip
// add mounting angle of BBB
cstate.theta[2] = cstate.theta[1]; cstate.theta[1] = cstate.theta[0];
cstate.theta[0] = mpu.fusedEuler[VEC3_X] + config.bb_mount_angle;
cstate.current_theta = cstate.theta[0];
cstate.d_theta = (cstate.theta[0]-cstate.theta[1])/DT;
// collect encoder positions
cstate.wheelAngleR = -(get_encoder_pos(config.encoder_channel_R) * 2*PI) \
/(config.gearbox * config.encoder_res);
cstate.wheelAngleL = (get_encoder_pos(config.encoder_channel_L) * 2*PI) \
/(config.gearbox * config.encoder_res);
// log phi estimate
// wheel angle is relative to body,
// add theta body angle to get absolute wheel position
cstate.phi[2] = cstate.phi[1]; cstate.phi[1] = cstate.phi[0];
cstate.phi[0] = ((cstate.wheelAngleL + cstate.wheelAngleR)/2) +cstate.current_theta;
cstate.current_phi = cstate.phi[0];
cstate.d_phi = (cstate.phi[0]-cstate.phi[1])/DT;
// body turning estimation
cstate.gamma[2] = cstate.gamma[1]; cstate.gamma[1] = cstate.phi[0];
cstate.gamma[0]=(cstate.wheelAngleL-cstate.wheelAngleR) \
* (config.wheel_radius/config.track_width);
cstate.d_gamma = (cstate.gamma[0]-cstate.gamma[1])/DT;
cstate.current_gamma = cstate.gamma[0];
// output scaling
output_scale = cstate.vBatt/config.v_nominal;
/*************************************************************
* Control based on the robotics_library defined state variable
* PAUSED: make sure the controller stays DISARMED
* RUNNING: Normal operation of controller.
* - check for tipover
* - wait for MiP to be within config.start_angle of
* upright
* - 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:
disable_motors();
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){
return 0;
}
// check for a tipover before anything else
if(fabs(cstate.current_theta)>config.tip_angle){
disarm_controller();
printf("tip detected \n");
break;
}
/**********************************************************
* POSITION Phi controller
* feedback control of wheel angle Phi by outputting a
* reference theta body angle. This is controller D2 in
* config
***********************************************************/
if(setpoint.core_mode == POSITION){
// move the position set points based on user input
if(setpoint.phi_dot != 0.0){
//setpoint.phi == cstate.current_phi + setpoint.phi_dot*DT;
setpoint.phi += setpoint.phi_dot * DT;
}
// march the different equation terms for the input Phi Error
// and the output theta reference angle
cstate.ePhi[2] = cstate.ePhi[1];
cstate.ePhi[1] = cstate.ePhi[0];
cstate.ePhi[0] = setpoint.phi-cstate.current_phi;
cstate.theta_ref[2] = cstate.theta_ref[1];
cstate.theta_ref[1] = cstate.theta_ref[0];
// evaluate D2 difference equation
cstate.theta_ref[0] = config.K_D2*( \
config.numD2_2 * cstate.ePhi[2] \
+ config.numD2_1 * cstate.ePhi[1] \
+ config.numD2_0 * cstate.ePhi[0]) \
-(config.denD2_2 * cstate.theta_ref[2] \
+ config.denD2_1 * cstate.theta_ref[1]);
//check saturation of outer loop theta reference output signal
saturate_float(&cstate.theta_ref[0],-config.theta_ref_max,config.theta_ref_max);
setpoint.theta = cstate.theta_ref[0];
}
// evaluate inner loop controller D1z
cstate.eTheta[2] = cstate.eTheta[1];
cstate.eTheta[1] = cstate.eTheta[0];
cstate.eTheta[0] = setpoint.theta - cstate.current_theta;
cstate.u[2] = cstate.u[1];
cstate.u[1] = cstate.u[0];
cstate.u[0] = \
config.K_D1 * (config.numD1_0 * cstate.eTheta[0] \
+ config.numD1_1 * cstate.eTheta[1] \
+ config.numD1_2 * cstate.eTheta[2]) \
- (config.denD1_1 * cstate.u[1] \
+ config.denD1_2 * cstate.u[2]);
// check saturation of inner loop knowing that right after
// this control will be scaled by battery voltage
if(saturate_float(&cstate.u[0], -output_scale, output_scale)){
D1_saturation_counter ++;
if(D1_saturation_counter > SAMPLE_RATE_HZ*config.pickup_detection_time){
printf("inner loop controller saturated\n");
disarm_controller();
D1_saturation_counter = 0;
break;
}
}
else{
D1_saturation_counter = 0;
}
cstate.current_u = cstate.u[0];
// scale output to compensate for battery charge level
compensated_D1_output = cstate.u[0] / output_scale;
// // integrate the reference theta to correct for imbalance or sensor
// // only if standing relatively still with zero phi reference
// // to-do, wait for stillness for a time period before integrating
// if(setpoint.phi==0 && fabs(cstate.phi_dot)<2){
// state.thetaTrim += (config.kTrim*cstate.theta_ref[0]) * DT;
// }
//steering controller
// move the controller set points based on user input
setpoint.gamma += setpoint.gamma_dot * DT;
cstate.egamma[1] = cstate.egamma[0];
cstate.egamma[0] = setpoint.gamma - cstate.current_gamma;
cstate.duty_split = config.KP_steer*(cstate.egamma[0] \
+config.KD_steer*(cstate.egamma[0]-cstate.egamma[1]));
// if the steering input would saturate a motor, reduce
// the steering input to prevent compromising balance input
if(fabs(compensated_D1_output)+fabs(cstate.duty_split) > 1){
if(cstate.duty_split > 0){
cstate.duty_split = 1-fabs(compensated_D1_output);
}
else cstate.duty_split = -(1-fabs(compensated_D1_output));
}
// add D1 balance controller and steering control
dutyL = compensated_D1_output - cstate.duty_split;
dutyR = compensated_D1_output + cstate.duty_split;
// send to motors
// one motor is flipped on chassis so reverse duty to L
set_motor(config.motor_channel_L,-dutyL);
set_motor(config.motor_channel_R,dutyR);
cstate.time_us = microsSinceEpoch();
// 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.time_us = cstate.time_us-core_start_time_us;
new_log_entry.theta = cstate.current_theta;
new_log_entry.theta_ref = setpoint.theta;
new_log_entry.phi = cstate.current_phi;
new_log_entry.u = cstate.current_u;
add_to_buffer(new_log_entry);
}
// end of normal balancing routine
// last_state will be updated beginning of next interrupt
break;
default:
break; // nothing to do if UNINITIALIZED
}
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;
}
