// Called on each vision analysis result after receiving the struct
void run_avoid_navigation_onvision(void)
{
// Send ALL vision data to the ground
DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 5, avoid_navigation_data.stereo_bin);
switch (avoid_navigation_data.mode)
{
case 0: // Go to Goal and stop at obstacles
//count 4 subsequent obstacles
if(avoid_navigation_data.stereo_bin[0]>20) {
counter = counter + 1;
if(counter > 2) {
counter = 0;
//Obstacle detected, go to turn until clear mode
obstacle_detected = TRUE;
avoid_navigation_data.mode = 1;
}
}
else
counter = 0;
break;
case 1: // Turn until clear
//count 20 subsequent free frames
if(avoid_navigation_data.stereo_bin[0]<15) {
counter = counter + 1;
if(counter > 12) {
counter = 0;
//Stop and put waypoint 2.5 m ahead
struct EnuCoor_i new_coor;
struct EnuCoor_i* pos = stateGetPositionEnu_i();
float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading*2.0);
new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading*2.0);
new_coor.z = pos->z;
nav_move_waypoint(WP_W1, &new_coor);
obstacle_detected = FALSE;
avoid_navigation_data.mode = 0;
}
}
else
counter = 0;
break;
case 2:
break;
default: // do nothing
break;
}
avoid_navigation_data.stereo_bin[2] = avoid_navigation_data.stereo_bin[0]>20;
avoid_navigation_data.stereo_bin[3] = avoid_navigation_data.mode;
avoid_navigation_data.stereo_bin[4] = counter;
if(obstacle_detected) {
LED_ON(3);
}
else {
LED_OFF(3);
}
}
void dl_parse_msg(void) {
datalink_time = 0;
uint8_t msg_id = IdOfMsg(dl_buffer);
#if 0 // not ready yet
uint8_t sender_id = SenderIdOfMsg(dl_buffer);
/* parse telemetry messages coming from other AC */
if (sender_id != 0) {
switch (msg_id) {
#ifdef TCAS
case DL_TCAS_RA:
{
if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) {
uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer);
tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer);
}
}
#endif
}
return;
}
#endif
if (msg_id == DL_PING) {
DOWNLINK_SEND_PONG(DefaultChannel);
} else
#ifdef TRAFFIC_INFO
if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) {
uint8_t id = DL_ACINFO_ac_id(dl_buffer);
float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
float a = MOfCm(DL_ACINFO_alt(dl_buffer));
float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
float s = MOfCm(DL_ACINFO_speed(dl_buffer));
float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
uint32_t t = DL_ACINFO_itow(dl_buffer);
SetAcInfo(id, ux, uy, c, a, s, cl, t);
} else
#endif
#ifdef NAV
if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) {
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
/* Computes from (lat, long) in the referenced UTM zone */
float lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7));
float lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7));
latlong_utm_of(lat, lon, nav_utm_zone0);
nav_move_waypoint(wp_id, latlong_utm_x, latlong_utm_y, a);
/* Waypoint range is limited. Computes the UTM pos back from the relative
coordinates */
latlong_utm_x = waypoints[wp_id].x + nav_utm_east0;
latlong_utm_y = waypoints[wp_id].y + nav_utm_north0;
DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &latlong_utm_x, &latlong_utm_y, &a, &nav_utm_zone0);
} else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) {
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
SEND_NAVIGATION(DefaultChannel);
} else
#endif /** NAV */
#ifdef WIND_INFO
if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
wind_east = DL_WIND_INFO_east(dl_buffer);
wind_north = DL_WIND_INFO_north(dl_buffer);
#ifndef USE_AIRSPEED
estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer);
#endif
#ifdef WIND_INFO_RET
DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, &wind_east, &wind_north, &estimator_airspeed);
#endif
} else
#endif /** WIND_INFO */
#ifdef HITL
/** Infrared and GPS sensors are replaced by messages on the datalink */
if (msg_id == DL_HITL_INFRARED) {
/** This code simulates infrared.c:ir_update() */
ir_roll = DL_HITL_INFRARED_roll(dl_buffer);
ir_pitch = DL_HITL_INFRARED_pitch(dl_buffer);
ir_top = DL_HITL_INFRARED_top(dl_buffer);
} else if (msg_id == DL_HITL_UBX) {
/** This code simulates gps_ubx.c:parse_ubx() */
if (gps_msg_received) {
gps_nb_ovrn++;
} else {
ubx_class = DL_HITL_UBX_class(dl_buffer);
ubx_id = DL_HITL_UBX_id(dl_buffer);
uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer);
uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer);
memcpy(ubx_msg_buf, ubx_payload, l);
gps_msg_received = TRUE;
}
} else
#endif
#ifdef DlSetting
if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
uint8_t i = DL_SETTING_index(dl_buffer);
float val = DL_SETTING_value(dl_buffer);
DlSetting(i, val);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val);
} else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val);
} else
#endif /** Else there is no dl_settings section in the flight plan */
#ifdef USE_JOYSTICK
if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer),
DL_JOYSTICK_RAW_pitch(dl_buffer),
DL_JOYSTICK_RAW_throttle(dl_buffer));
} else
#endif // USE_JOYSTICK
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
LED_TOGGLE(3);
parse_rc_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
} else
#endif // RC_DATALINK
{ /* Last else */
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
}
