/**
* Get an update from the sensors
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not
*/
static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
{
struct pios_sensor_gyro_data gyros;
struct pios_sensor_accel_data accels;
xQueueHandle queue;
queue = PIOS_SENSORS_GetQueue(PIOS_SENSOR_GYRO);
if(queue == NULL || xQueueReceive(queue, (void *) &gyros, 4) == errQUEUE_EMPTY) {
return-1;
}
// As it says below, because the rest of the code expects the accel to be ready when
// the gyro is we must block here too
queue = PIOS_SENSORS_GetQueue(PIOS_SENSOR_ACCEL);
if(queue == NULL || xQueueReceive(queue, (void *) &accels, 1) == errQUEUE_EMPTY) {
return -1;
}
else
update_accels(&accels, accelsData);
// Update gyros after the accels since the rest of the code expects
// the accels to be available first
update_gyros(&gyros, gyrosData);
update_trimming(accelsData);
GyrosSet(gyrosData);
AccelsSet(accelsData);
return 0;
}
int main() {
// The physical computations run at about 1500 Hz. 30 consecutive
// time steps are mixed into a frame buffer for motion blur. But
// this is not only about motion blur. This higher temporal resolution
// is necessary to avoid discretization errors to grow too large and
// to keep the whole thing "stable".
//
// The framebuffer is "oversampled" (higher resolution) for
// anti-aliasing.
const int num_particles = 10;
const int substeps = 30; // temporal oversampling
const int sover = 5; // spatial oversampling
std::srand(std::time(0));
std::vector<particle> ps;
ps.push_back( // bottom border particle that is not moving
make_particle(
make_random_color(),
-1
)
);
for (int i = 0; i < num_particles; ++i) {
ps.push_back( // almost equidistantly spaced particles
make_particle(
make_random_color(),
(i + 0.9f + 0.2f * uniform_random()) / (1 + num_particles) * (num_leds + 1) - 1
)
);
}
ps.push_back( // top border particle that is not moving
make_particle(
make_random_color(),
num_leds
)
);
std::vector<int> framebuff (num_leds * 3 * sover);
std::vector<unsigned char> scratch;
int time = -1;
int period = 10000; // time steps until next impulse
int part_change_color_index = 1;
color part_change_color_new = {{0}}, part_change_color_old = {{0}};
#ifdef WRITE_PPM
const int iters = 500; // for debugging purposes
write_binary_ppm_header(num_leds, iters, std::cout);
for (int i = 0; i < iters; ++i) {
#else
for (;;) {
#endif
// blank frame buffer (black)
std::fill(framebuff.begin(), framebuff.end(), 0);
for (int s = 0; s < substeps; ++s) {
render_frame(ps, sover, substeps, framebuff);
time = (time + 1) % period;
if (time <= 1000) { // impulse phase
if (time == 0) {
period = 5000 + std::rand() % 10000;
part_change_color_index = (std::rand() % num_particles) + 1;
part_change_color_old = ps[part_change_color_index].col;
part_change_color_new = make_random_color();
}
float s1 = squared(std::sin(time * (3.14159265 / 2000)));
float s2 = squared(std::sin(time * (3.14159265 / 1000)));
ps[0].pos = s2 * 15 - 1; // move bottom particle around
// also change the color of a random particle once in a while
ps[part_change_color_index].col = fade(
part_change_color_old,
part_change_color_new, s1);
}
update_accels(ps);
progress(ps);
}
write_frame(framebuff, 1.0f/substeps, sover, scratch, std::cout);
#ifndef WRITE_PPM
usleep(20000); // wait 20 ms (=> about 50 frames/sec)
#endif
}
return 0;
}