//apply the attitude correction to the motors
int apply_correction(struct quad_state *curr, struct quad_state *past, struct quad_static *stat){
#ifdef BQUAD
//BQUAD settings
stat->pidval.px = 2.8;
stat->pidval.py = 2.8;
stat->pidval.pz = 3.9;
stat->pidval.dx = 1.8;
stat->pidval.dy = 1.8;
stat->pidval.dz = 2.7;
stat->pidval.ix = .5;
stat->pidval.iy = .5;
stat->pidval.iz = .5;
#endif
//TODO need to increase d term to deal with bad blade tracking old val 1.1
#ifdef SQUAD2S
//SQUAD2S settings
stat->pidval.px = 2.8;
stat->pidval.py = 2.8;
stat->pidval.pz = 2.5;
stat->pidval.dx = 1.7;
stat->pidval.dy = 1.7;
stat->pidval.dz = 1.8;
stat->pidval.ix = .7;
stat->pidval.iy = .7;
stat->pidval.iz = .7;
#endif
#ifdef SQUAD3S
//SQUAD3S settings
stat->pidval.px = 1.5;
stat->pidval.py = 1.5;
stat->pidval.pz = 2;
stat->pidval.dx = 1.1;
stat->pidval.dy = 1.1;
stat->pidval.dz = 1.8;
stat->pidval.ix = .5;
stat->pidval.iy = .5;
stat->pidval.iz = .5;
#endif
struct ivector p, i, d;
p.x = curr->att_correct.x * stat->pidval.px;
p.y = curr->att_correct.y * stat->pidval.py;
//p.z = curr->att_correct.z * stat->pidval.pz;
float yaw = (stat->control[3] - 11059) * ( 180.0 / (2949.0 * 6.0));
p.z = yaw * stat->pidval.pz;
d.x = curr->sdata[1][0] * stat->pidval.dx * (180 / 3.1415926535897);
d.y = curr->sdata[1][1] * stat->pidval.dy * (180 / 3.1415926535897);
d.z = curr->sdata[1][2] * stat->pidval.dz * (180 / 3.1415926535897);
//TODO FIX
if(0 && stat->throttle > 200){
curr->int_correct.x = past->int_correct.x + (curr->dt * curr->att_correct.x * stat->pidval.ix);
curr->int_correct.y = past->int_correct.y + (curr->dt * curr->att_correct.y * stat->pidval.iy);
curr->int_correct.z = past->int_correct.z + (curr->dt * curr->att_correct.z * stat->pidval.iz);
}else{
clone_vector(&past->int_correct, &curr->int_correct);
}
clear_motor(stat);
/*
* assumed motor configuration and coordinate axis
* 1 y
* 4-+-2 | z axis is positive out of the screen
* 3 |---x
*
*/
//good
stat->motor[0] += p.x;
stat->motor[1] -= p.y;
stat->motor[2] -= p.x;
stat->motor[3] += p.y;
//good
stat->motor[0] += p.z;
stat->motor[1] -= p.z;
stat->motor[2] += p.z;
stat->motor[3] -= p.z;
/* //???
stat->motor[0] += curr->int_correct.x;
stat->motor[1] += curr->int_correct.y;
stat->motor[2] -= curr->int_correct.x;
stat->motor[3] -= curr->int_correct.y;
//???
stat->motor[0] += curr->int_correct.z;
stat->motor[1] -= curr->int_correct.z;
stat->motor[2] += curr->int_correct.z;
stat->motor[3] -= curr->int_correct.z;
*/
//good
stat->motor[0] -= d.x;
stat->motor[1] += d.y;
stat->motor[2] += d.x;
stat->motor[3] -= d.y;
//good
stat->motor[0] -= d.z;
stat->motor[1] += d.z;
stat->motor[2] -= d.z;
stat->motor[3] += d.z;
stat->motor[0] += stat->throttle;
stat->motor[1] += stat->throttle;
stat->motor[2] += stat->throttle;
stat->motor[3] += stat->throttle;
int j;
for(j = 0; j < 4; ++j){
if(stat->motor[j] < 0) stat->motor[j] = 0;
if(stat->motor[j] > 990) stat->motor[j] = 990;
}
#if defined(SQUAD2S) || defined(SQUAD3S)
//SQUAD motors are oriented in the opposite direction
int tmp[4];
for(j = 0; j < 4; ++j){
tmp[j] = stat->motor[j];
}
for(j = 0; j < 4; ++j){
stat->motor[j] = tmp[3-j];
}
#endif
return 0;
}
SGVector<T> SGVector<T>::clone() const
{
return SGVector<T>(clone_vector(vector, vlen), vlen);
}
