/******************************************************************
*	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;
}
Exemple #2
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;
}