/**
* Recieve data from the IMU, and integrate to a state estimation
*/
void state_receive_imu(const char *ID, uint8_t *timestamp, uint16_t len, void *buf) {
const uint64_t now = from_psas_time(timestamp);
ADIS16405Data *imu = buf;
// Convert raw IMU data to MKS/degrees unit system
const double accel = ADIS_GLSB * (int16_t) ntohs(imu->acc_x);
const double roll_rate = ADIS_RLSB * (int16_t) ntohs(imu->gyro_x);
if (!has_launched && fabs(accel) > 40) {
has_launched = true;
launch_time = now;
}
if (last_time == 0)
last_time = now;
if (has_launched) {
const double dt = (now - last_time) / 1.0e9;
// Integrate sensors
current_state.time = (now - launch_time) / 1.0e9;
current_state.acc_up = accel - 9.81; // Subtract gravity (Remember, IMU is upside down)
current_state.vel_up += current_state.acc_up*dt;
current_state.altitude += current_state.vel_up*dt;
current_state.roll_rate = -roll_rate; //Sensor coordinate frame is rotated 180
current_state.roll_angle += roll_rate*dt; //This is wrong (negative) but we don't care, nothing uses it.
}
// Send data
state_send_message("VSTE", timestamp, sizeof(StateData), ¤t_state);
last_time = now;
}
/* Moves the game agent in its current direction, assuming the
given amount of time has passed. Returns 1 if the move was
successful, 0 otherwise. */
int agent_move(GameAgent *ga, Uint32 time)
{
int last_block_x, last_block_y, next_block_x, next_block_y;
int last_block_x2, last_block_y2, next_block_x2, next_block_y2;
int block_changed;
FixedVector v1, v2, ga_loc2;
/* Scale the normalized movement vector relative to the game agent's speed. */
v1 = fixed_vector_scale(&ga->curr_move, ga->speed);
/* Now scale that vector to the amount of time that's passed. */
v1 = fixed_vector_scale(&v1, FIXED_SET_INT(time));
/* Now add that delta vector to the game agent's current position to
get their predicted new position. */
v1 = fixed_vector_add(&ga->loc, &v1);
/* If we've crossed a block boundary and we're not on the
first pixel of the block boundary, put us on it. */
/* Block coordinates of the game agent's "last" (actually current) position,
relative to the top-right of the game agent. */
last_block_x = GET_BLOCK_FIXED(ga->loc.x);
last_block_y = GET_BLOCK_FIXED(ga->loc.y);
/* Block coordinates of the game agent's "next" (predicted) position, relative
to the top-left of the game agent. */
next_block_x = GET_BLOCK_FIXED(v1.x);
next_block_y = GET_BLOCK_FIXED(v1.y);
/* Now calculate the same info for the bottom-right corner of the game agent.
This is used if the player is moving left or up. */
ga_loc2 = fixed_vector_add(&ga->loc, &ga->physical_dim);
v2 = fixed_vector_add(&v1, &ga->physical_dim);
last_block_x2 = GET_BLOCK_FIXED(ga_loc2.x);
last_block_y2 = GET_BLOCK_FIXED(ga_loc2.y);
next_block_x2 = GET_BLOCK_FIXED(v2.x);
next_block_y2 = GET_BLOCK_FIXED(v2.y);
/* block_changed is a boolean that tells us whether the game agent
has crossed a block boundary or not. */
block_changed = 0;
/* Figure out if we've passed a block boundary. If we have, we may need
to change our next position if it's not exactly on a block boundary. If
this happens, v1 will be changed. */
if (last_block_x < next_block_x) {
/* If we're moving to the right (our next block is more than our last one)... */
block_changed = 1;
if (FIXED_GET_INT(v1.x) > next_block_x * BLOCK_SIZE)
v1.x = FIXED_SET_INT(next_block_x * BLOCK_SIZE);
} else if (last_block_x2 > next_block_x2) {
/* If we're moving to the left... */
block_changed = 1;
if (FIXED_GET_INT(v1.x) < next_block_x2 * BLOCK_SIZE)
v1.x = FIXED_SET_INT(next_block_x2 * BLOCK_SIZE);
} else if (last_block_y < next_block_y) {
/* If we're moving down... */
block_changed = 1;
if (FIXED_GET_INT(v1.y) > next_block_y * BLOCK_SIZE)
v1.y = FIXED_SET_INT(next_block_y * BLOCK_SIZE);
} else if (last_block_y2 > next_block_y2) {
/* If we're moving up... */
block_changed = 1;
if (FIXED_GET_INT(v1.y) < next_block_y2 * BLOCK_SIZE)
v1.y = FIXED_SET_INT(next_block_y2 * BLOCK_SIZE);
}
if (agent_is_position_viable(ga, &v1)) {
ga->last_loc = ga->loc;
ga->loc = v1;
/* If we passed into a new block, alert the game agent's state machine (FSM). */
if (block_changed) {
if (FIXED_GET_INT(ga->loc.x) == (BOARD_WIDTH+2)*BLOCK_SIZE) {
/* If we've gone through a tunnel to the right, wrap around
to the left side of the screen. */
fixed_vector_set(&ga->loc, -1*BLOCK_SIZE, FIXED_GET_INT(ga->loc.y));
ga->last_loc = ga->loc;
} else if (FIXED_GET_INT(ga->loc.x) == -1*BLOCK_SIZE) {
/* If we've gone through a tunnel to the left, wrap around
to the right side of the screen. */
fixed_vector_set(&ga->loc, (BOARD_WIDTH+2)*BLOCK_SIZE, FIXED_GET_INT(ga->loc.y));
ga->last_loc = ga->loc;
}
state_send_message(GAME_AGENT_MSG_BLOCK_CHANGE, 0, ga->state.state_id, 0, 0);
}
return 1;
} else return 0;
}
