extern void stereocam_periodic(void)
{
// read all data from the stereo com link, check that don't overtake extract
while (dev->char_available(dev->periph) && stereoprot_add(insert_loc, 1, STEREO_BUF_SIZE) != extract_loc) {
if (handleStereoPackage(StereoGetch(), STEREO_BUF_SIZE, &insert_loc, &extract_loc, &msg_start, msg_buf, ser_read_buf,
&stereocam_data.fresh, &stereocam_data.len)) {
freq_counter++;
if ((sys_time.nb_tick - previous_time) > sys_time.ticks_per_sec) { // 1s has past
frequency = (uint8_t)((freq_counter * (sys_time.nb_tick - previous_time)) / sys_time.ticks_per_sec);
freq_counter = 0;
previous_time = sys_time.nb_tick;
}
#if SEND_STEREO
DOWNLINK_SEND_STEREO_IMG(DefaultChannel, DefaultDevice, &frequency, &(stereocam_data.len), stereocam_data.len, msg_buf);
#endif
}
}
}
void stereocam_droplet_periodic(void)
{
static float heading = 0;
// Read Serial
while (StereoChAvailable()) {
stereo_parse(StereoGetch());
}
if (avoid_navigation_data.timeout <= 0)
return;
avoid_navigation_data.timeout --;
// Results
DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, avoid_navigation_data.stereo_bin);
volatile bool_t once = TRUE;
// Move waypoint with constant speed in current direction
if (
(avoid_navigation_data.stereo_bin[0] == 97) ||
(avoid_navigation_data.stereo_bin[0] == 100)
) {
once = TRUE;
struct EnuCoor_f enu;
enu.x = waypoint_get_x(WP_GOAL);
enu.y = waypoint_get_y(WP_GOAL);
enu.z = waypoint_get_alt(WP_GOAL);
float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
enu.x += (sin_heading * 1.3 / 20);
enu.y += (cos_heading * 1.3 / 20);
waypoint_set_enu(WP_GOAL, &enu);
} else if (avoid_navigation_data.stereo_bin[0] == 98) {
// STOP!!!
if (once) {
NavSetWaypointHere(WP_GOAL);
once = FALSE;
}
} else {
once = TRUE;
}
switch (avoid_navigation_data.stereo_bin[0]) {
case 99: // Turn
heading += 4;
if (heading > 360) { heading = 0; }
nav_set_heading_rad(RadOfDeg(heading));
break;
default: // do nothing
break;
}
#ifdef STEREO_LED
if (obstacle_detected) {
LED_ON(STEREO_LED);
} else {
LED_OFF(STEREO_LED);
}
#endif
}
