static void handle_cmd(handler_arg_t arg)
{
switch ((char) (uint32_t) arg) {
case 't':
temperature_sensor();
break;
case 'l':
light_sensor();
break;
case 'p':
pressure_sensor();
break;
case 's':
send_packet();
break;
case 'b':
send_big_packet();
break;
case 'e':
leds_action();
break;
case '\n':
printf("\ncmd > ");
break;
case 'h':
default:
print_usage();
break;
}
}
static void handle_cmd(handler_arg_t arg)
{
switch ((char) (uint32_t) arg) {
#ifdef IOTLAB_M3
case 't':
temperature_sensor();
break;
case 'l':
light_sensor();
break;
case 'p':
pressure_sensor();
break;
#endif
case 'u':
print_node_uid();
break;
case 'd':
print_cn_time();
break;
case 's':
send_packet();
break;
case 'b':
send_big_packet();
break;
case 'e':
leds_action();
break;
case '\n':
printf("\ncmd > ");
break;
case 'h':
default:
print_usage();
break;
}
}
static accelerometer_i quantize_accelerometer(accelerometer_d value, unsigned mask)
{
return (accelerometer_i) {
.x = quantize(value.x, mask),
.y = quantize(value.y, mask),
.z = quantize(value.z, mask),
.q = quantize(value.q, mask),
};
}
static vec3_i quantize_vec(vec3 value, unsigned mask)
{
return (vec3_i) {
.x = quantize(value.x, mask),
.y = quantize(value.y, mask),
.z = quantize(value.z, mask),
};
}
static accelerometer_d add_accelerometer_noise(accelerometer_d value)
{
return (accelerometer_d) {
.x = value.x + gaussian(accelerometer_sd.x),
.y = value.y + gaussian(accelerometer_sd.y),
.z = value.z + gaussian(accelerometer_sd.z),
.q = value.q + gaussian(accelerometer_sd.q),
};
}
static vec3 vec_noise(vec3 value, vec3 sd)
{
return (vec3) {
.x = value.x + gaussian(sd.x),
.y = value.y + gaussian(sd.y),
.z = value.z + gaussian(sd.z),
};
}
static void update_simulator(void)
{
trace_state("sim", &rocket_state, ", %4.1f kg, %c%c%c\n",
mass,
engine_burning ? 'B' : '-',
drogue_chute_deployed ? 'D' : '-',
main_chute_deployed ? 'M' : '-');
accelerometer_sensor(quantize_accelerometer(add_accelerometer_noise(accelerometer_measurement(&rocket_state)), 0xfff));
if(t % 2000 == 0)
gyroscope_sensor(quantize_vec(vec_noise(gyroscope_measurement(&rocket_state), gyroscope_sd), 0xfff));
if(t % 100000 == 0)
pressure_sensor(quantize(pressure_measurement(&rocket_state) + gaussian(pressure_sd), 0xfff));
if(t % 100000 == 25000)
magnetometer_sensor(quantize_vec(vec_noise(magnetometer_measurement(&rocket_state), magnetometer_sd), 0xfff));
if(t % 100000 == 50000)
gps_sensor(vec_noise(rocket_state.pos, gps_pos_sd),
vec_noise(rocket_state.vel, gps_vel_sd));
if(!engine_ignited && t >= LAUNCH_TIME && last_reported_state() == STATE_ARMED)
{
trace_printf("Sending launch signal\n");
launch();
}
if(engine_burning
&& t - engine_ignition_time >= ENGINE_BURN_TIME)
{
trace_printf("Engine burn-out.\n");
engine_burning = false;
}
if(last_reported_state() == STATE_PREFLIGHT)
{
trace_printf("Sending arm signal\n");
arm();
}
if(engine_burning)
mass -= FUEL_MASS * DELTA_T_SECONDS / (ENGINE_BURN_TIME / 1e6);
geodetic pos = ECEF_to_geodetic(rocket_state.pos);
if(pos.altitude <= initial_geodetic.altitude)
{
if(pos.altitude < initial_geodetic.altitude)
{
pos.altitude = initial_geodetic.altitude;
rocket_state.pos = geodetic_to_ECEF(pos);
}
mat3 rot = make_LTP_rotation(pos);
ground_clip(&rocket_state.vel, rot);
}
}
static void init_rocket_state(struct rocket_state *rocket_state)
{
/* TODO: accept an initial orientation for leaving the tower */
mass = ROCKET_EMPTY_MASS + FUEL_MASS;
rocket_state->pos = geodetic_to_ECEF(initial_geodetic);
rocket_state->rotpos = make_LTP_rotation(initial_geodetic);
rocket_state->acc = expected_acceleration(0, rocket_state);
}
int main(int argc, const char *const argv[])
{
parse_trace_args(argc, argv);
initial_geodetic = (geodetic) {
.latitude = M_PI_2,
.longitude = 0,
.altitude = 0,
};
init_atmosphere(LAYER0_BASE_TEMPERATURE, LAYER0_BASE_PRESSURE);
init_rocket_state(&rocket_state);
init(initial_geodetic, rocket_state.rotpos);
while(last_reported_state() != STATE_RECOVERY)
{
update_rocket_state_sim(&rocket_state, DELTA_T_SECONDS, expected_acceleration, (double)t);
t += DELTA_T;
update_simulator();
tick(DELTA_T_SECONDS);
}
return 0;
}
