void controller(float32_t *input, ControllerParams params, float32_t *thetadot, float32_t *error, float32_t dt)
{
float32_t ctheta, cphi;
float32_t total;
float32_t tmp_1[3], tmp_2[3], tmp_3[3];
float32_t err[3];
//Compute total thrust
cphi = FastCos(params.integral[0]);
ctheta = FastCos(params.integral[1]);
total = params.m * params.g / params.k / (cphi * ctheta);
//Compute error and inputs.
Vector_Multiply_By_Scale(tmp_1, thetadot, params.Kd);
Vector_Multiply_By_Scale(tmp_2, params.integral, params.Kp);
Vector_Multiply_By_Scale(tmp_3, params.integral2rd, params.Ki);
Vector_Add(err, tmp_1, tmp_2);
Vector_Subtract(err, err, tmp_3);
err2inputs(input, params, err, total);
// Update controller state.
Vector_Integral(params.integral, thetadot, dt);
Vector_Integral(params.integral2rd, params.integral, dt);
if ((thetadot[0] < 0.2f) && (thetadot[1] < 0.2f) && (thetadot[2] < 0.2f)){
//recalculate
cphi = FastCos(params.integral[0]);
ctheta = FastCos(params.integral[1]);
total = params.m * params.g / params.k / (cphi * ctheta);
//to do
}
}
// Compute angular acceleration in body frame
// Parameters:
// I: inertia matrix
void angular_acceleration(float32_t *omegad , float32_t *inputs, float32_t* omega, float32_t* I, float32_t L, float32_t b, float32_t k)
{
float32_t tau[3];
float32_t tmp_3x1[3], cross[3];
float32_t II[9];
torques(tau, inputs, L, b, k);
Matrix_Inv_3x3(II, I);
Matrix_3x3_Multiply_Vector_3x1(tmp_3x1, I, omega);
Vector_Cross(cross, omega, tmp_3x1);
Vector_Subtract(tmp_3x1, tau, cross);
Matrix_3x3_Multiply_Vector_3x1(omegad, II, tmp_3x1);
}
struct trace *Trace_ClipMoveToEdict(struct server *server, struct edict *edict, vec3_t mins, vec3_t maxs, vec3_t start, vec3_t stop)
{
struct trace *trace;
struct hull *hull;
vec3_t offset, start_l, stop_l;
if (!server || !edict)
return NULL;
hull = Server_HullForEdict(server, edict, mins, maxs, offset);
Vector_Subtract(start_l, start, offset);
Vector_Subtract(stop_l, stop, offset);
trace = Trace_HullTrace(NULL, hull, start_l, stop_l);
Vector_Add(trace->endpos, offset, trace->endpos);
if (trace->fraction < 1 || trace->allsolid)
trace->e.ent = edict;
return trace;
}
