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