/******************************************************************
* 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;
}
/************************************************************************
* 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_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;
}