Exemplo n.º 1
0
void pidStrategy_calculateMotorSpeedsFlying(struct horizontal_velocities_struct *hv, struct att_struct *att, struct setpoint_struct *setpoint, struct control_limits_struct *control_limits, float motorOut[4])
{
	/* overwrite setpoints for now */
	targetRoll=setpoint->roll;
	targetPitch=setpoint->pitch;
	
	if(0) {
	  targetRoll =pid_Calc(&pid_hor_vel_y, -hv->yv, -hv->dt);
	  targetPitch=pid_Calc(&pid_hor_vel_x, -hv->xv, -hv->dt);
	}   

	//flying, calc pid controller corrections
	adj_roll  = pid_CalcD(&pid_roll,  targetRoll  - att->roll , att->dt, att->gx); //err positive = need to roll right
	adj_pitch = pid_CalcD(&pid_pitch, targetPitch - att->pitch, att->dt, att->gy); //err positive = need to pitch down
	adj_yaw   = pid_CalcD(&pid_yaw,   setpoint->yaw   - att->yaw,   att->dt, att->gz); //err positive = need to increase yaw to the left
	adj_h     = pid_CalcD(&pid_h,     setpoint->h     - att->h,     att->dt, att->hv); //err positive = need to increase height

	throttle = control_limits->throttle_hover + adj_h;
	if (throttle < control_limits->throttle_min)
		throttle = control_limits->throttle_min;
	if (throttle > control_limits->throttle_max)
		throttle = control_limits->throttle_max;

	//convert pid adjustments to motor values
	motorOut[0] = throttle + adj_roll - adj_pitch + adj_yaw;
	motorOut[1] = throttle - adj_roll - adj_pitch - adj_yaw;
	motorOut[2] = throttle - adj_roll + adj_pitch + adj_yaw;
	motorOut[3] = throttle + adj_roll + adj_pitch - adj_yaw;
	
	if (motorOut[0]<control_limits->throttle_hover/3)
		motorOut[0] = control_limits->throttle_hover/3;
	if (motorOut[1]<control_limits->throttle_hover/3)
		motorOut[1] = control_limits->throttle_hover/3;
	if (motorOut[2]<control_limits->throttle_hover/3)
		motorOut[2] = control_limits->throttle_hover/3;
	if (motorOut[3]<control_limits->throttle_hover/3)
		motorOut[3] = control_limits->throttle_hover/3;
}
Exemplo n.º 2
0
void *ctl_thread_main(void* data)
{
	int cnt;
	int rc;
	//printf("run pthread");
	//navdata_update(&ahrsdata); //get data from thread	
    
	while(1) {
		// get navdata
		navdata_update(&ahrsdata); //get data from thread
	
    		float motor[4]; 
    
    		if(setpoint.h<=0.02) {
      			//motors off
      			adj_roll = 0;
      			adj_pitch = 0;
      			adj_h = 0;
      			adj_yaw = 0;
      			throttle = 0;
		}else{    
 
      			//attitude pid controller
      			adj_roll  = pid_Calc(&pid_roll,  setpoint.roll   - ahrsdata.roll,  ahrsdata.dt);
      			adj_pitch = pid_Calc(&pid_pitch, setpoint.pitch  - ahrsdata.pitch, ahrsdata.dt);
      			adj_yaw = setpoint.yaw;    
      			throttle = setpoint.h;
      			if(throttle < setpoint.throttle_min) throttle = setpoint.throttle_min;
      			if(throttle > setpoint.throttle_max) throttle = setpoint.throttle_max;  
    		}
    
    	// Convert pid adjustments to motor values
        motor[0] = throttle +adj_roll -adj_pitch +adj_yaw;
    	motor[1] = throttle -adj_roll -adj_pitch -adj_yaw;
    	motor[2] = throttle -adj_roll +adj_pitch +adj_yaw;
    	motor[3] = throttle +adj_roll +adj_pitch -adj_yaw;
    	
    	float motor_min = 0.125;
    	if(motor[0] < motor_min) motor[0] = motor_min;
    	if(motor[1] < motor_min) motor[1] = motor_min;
    	if(motor[2] < motor_min) motor[2] = motor_min;
    	if(motor[3] < motor_min) motor[3] = motor_min;

    	ahrsdata.motor1 = motor[0];
    	ahrsdata.motor2 = motor[1];
    	ahrsdata.motor3 = motor[2];
    	ahrsdata.motor4 = motor[3];
	
    	//printf("motor: %.5f,%.5f,%.5f,%.5f\n",motor[0],motor[1],motor[2],motor[3]);
   	//printf("throttle = %.5f, adj_roll = %0.5f, adj_pitch = %0.5f, adj_yaw = %0.5f \n", throttle,adj_roll,adj_pitch,adj_yaw);
    	//printf("throttle = %.5f, adj_roll = %0.5f, adj_pitch = %0.5f, motor: %.5f,%.5f,%.5f,%.5f \n", throttle,adj_roll,adj_pitch,motor[0],motor[1],motor[2],motor[3]);
    	//send to motors
    	mot_Run(motor[0],motor[1],motor[2],motor[3]); // Comment when you do not want motors rotate
        
    	//blink leds    
    	cnt++;
    	if((cnt%200)==0) 
      		mot_SetLeds(MOT_LEDGREEN,MOT_LEDGREEN,MOT_LEDGREEN,MOT_LEDGREEN);
    	else if((cnt%200)==100) 
      		mot_SetLeds(0,0,0,0);
        
    	// Send UDP nav log packet    
    	navLog_Send();
  
	// Yield to other threads
	pthread_yield();
	}
}