Пример #1
0
/************************************************************************
*	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;
}
Пример #2
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;
}
Пример #3
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;
}
Пример #4
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;
}
Пример #5
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;
}
Пример #6
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;
}
Пример #7
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;
}