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;
}
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();
}
}