/** initialize global and local waypoints */
void waypoints_init(void)
{
struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU;
struct LlaCoor_i wp_tmp_lla_i[NB_WAYPOINT] = WAYPOINTS_LLA_WGS84;
/* element in array is TRUE if absolute/global waypoint */
bool is_global[NB_WAYPOINT] = WAYPOINTS_GLOBAL;
uint8_t i = 0;
for (i = 0; i < nb_waypoint; i++) {
/* clear all flags */
waypoints[i].flags = 0;
/* init waypoint as global LLA or local ENU */
if (is_global[i]) {
waypoint_set_global_flag(i);
waypoint_set_lla(i, &wp_tmp_lla_i[i]);
} else {
waypoint_set_enu(i, &wp_tmp_float[i]);
}
}
}
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
}
