Quaternion quaternion_from_cube(int value)
{
assert(value >= 0 && value < 24);
Quaternion dimension_q = quaternion_from_cube_dimension(value / 8);
Quaternion corner_q = quaternion_from_cube_corner(value % 8);
return quaternion_product(dimension_q, corner_q);
}
void rotate_vector(struct vect *out, struct vect *in, struct quaternion *r)
{
struct quaternion temp, sum, res, inverse;
temp.x = in->x;
temp.y = in->y;
temp.z = in->z;
temp.w = 0;
get_conjugate(&inverse, r);
quaternion_product(&sum, r, &temp);
quaternion_product(&res, &sum, &inverse);
out->x = res.x;
out->y = res.y;
out->z = res.z;
}
quaternion ahrs_orientation_from_gyro(dataexchange_t *data) {
//angle component
vector3d gyr_rad_s = l3g4200d_raw_to_rad(data);
double angle = vector_magnitude(gyr_rad_s) * (*data).time_period;
//axis, normalized
vector_norm(&gyr_rad_s);
//quaternion from axis/angle
quaternion q = quaternion_from_axis_angle(gyr_rad_s, angle);
//normalize
quaternion_norm(&q);
(*data).qr = quaternion_product((*data).qr, q);
return q;
}
int point_at_local(struct turret *turret, struct vect *point)
{
int can_point = 1;
// point->x = 0;
// point->y = -40;
// point->z = 400;
struct vect normalized_local;
struct vect diff;
difference(&diff, point, &camera_center);
//printf("%f, %f, %f\n", diff.x, diff.y, diff.z);
//printf("%f, %f, %f\n", point->x, point->y, point->z);
float magnitude = norm(&diff);
if (magnitude == 0)
{
normalized_local.x = 0;
normalized_local.y = 0;
normalized_local.z = 1.0;
}
else
{
normalized_local.x = diff.x / magnitude;
normalized_local.y = diff.y / magnitude;
normalized_local.z = diff.z / magnitude;
}
float conversion_factor = (1000.0 / 90.0) * (180 / M_PI);
turret->yaw = (atan2f(normalized_local.y, -normalized_local.x));
float distance = sqrtf(normalized_local.x * normalized_local.x +
normalized_local.y * normalized_local.y);
turret->pitch = -(atan2f(normalized_local.z, distance));
// fprintf(stderr, "%f\n", turret->pitch);
if (turret->yaw * conversion_factor > 1960)
{
can_point =0;
turret->yaw = M_PI / 2;
turret->pitch = M_PI / 4;
}
if (turret->yaw * conversion_factor < 160)
{
can_point = 0;
turret->yaw = M_PI / 2;
turret->pitch = M_PI / 4;
}
if (turret->pitch * conversion_factor > 1440)
{
can_point = 0;
turret->yaw = M_PI / 2;
turret->pitch = M_PI / 4;
}
if (turret->pitch *conversion_factor < 140)
{
can_point = 0;
turret->yaw = M_PI / 2;
turret->pitch = M_PI / 4;
}
float yaw_setting = turret->yaw * conversion_factor + 415;
float pitch_setting = 2125 - (turret->pitch * conversion_factor);
if (yaw_setting > 500 && yaw_setting < 2500)
{
gpioServo(24, yaw_setting);
}
if (pitch_setting > 500 && pitch_setting < 2500)
{
gpioServo(4, pitch_setting);
}
if (can_point)
{
//gpioWrite(14, 1);
}
else
{
//gpioWrite(14, 0);
}
struct quaternion yaw, pitch;
yaw.w = cosf((turret->yaw - M_PI / 2) / 2);
yaw.x = 0;
yaw.y = 0;
yaw.z = sinf((turret->yaw - M_PI / 2) / 2);
pitch.w = cosf(turret->pitch / 2);
pitch.x = -sinf(turret->pitch / 2);
pitch.y = 0;
pitch.z = 0;
quaternion_product(&turret->turret_rotation, &yaw, &pitch);
return can_point;
}
Quaternion quaternion_sandwich_product(Quaternion a, Quaternion b, Quaternion c)
{
return quaternion_product(a, quaternion_product(b, c));
}
