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