void dl_parse_msg(void) {
datalink_time = 0;
uint8_t msg_id = IdOfMsg(dl_buffer);
switch (msg_id) {
case DL_PING:
{
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
break;
case DL_SETTING :
{
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) break;
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_GET_SETTING :
{
if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) break;
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
break;
/* this case for DL_HOVER_POINT_CHANGE_ECEF is added by Yi and Edward */
/*********************************************************************/
case DL_HOVER_POINT_CHANGE_ECEF:
{
uint8_t ac_id = DL_HOVER_POINT_CHANGE_ECEF_ac_id(dl_buffer);
if(ac_id != AC_ID) break;
uint8_t external_gps_fixed = DL_HOVER_POINT_CHANGE_ECEF_external_gps_fix(dl_buffer);
/** getting the ecef coordinate from external gps **/
struct EcefCoor_i external_gps_ecef_pos;
external_gps_ecef_pos.x = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_x(dl_buffer);//DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_x() is defined in dl_protocol.h
external_gps_ecef_pos.y = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_y(dl_buffer);
external_gps_ecef_pos.z = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_z(dl_buffer);
DOWNLINK_SEND_HOVER_POINT_CHANGE_ECEF(DefaultChannel, DefaultDevice, &ac_id, &external_gps_fixed, &(external_gps_ecef_pos.x), &(external_gps_ecef_pos.y), &(external_gps_ecef_pos.z));
if(external_gps_fixed == 0x03)//case for 3D_Fix.
{
if(!ins_ltp_initialised || guidance_h_mode != GUIDANCE_H_MODE_HOVER) break;//ins_ltp_initialised defined in ins_int.h
//no HMOE reference
//not in the HOVER_Z_HOLD mode
//not in flight(looks like no needed)
struct NedCoor_i external_gps_ned_pos;
memset((void *) &external_gps_ned_pos,0,sizeof(struct NedCoor_i));//just in case, clear the memory.
/** convert the ecef coordinate into ned(ltp) coordinate **/
ned_of_ecef_point_i(&external_gps_ned_pos,&ins_ltp_def,&external_gps_ecef_pos);
/** change the current setpoint with external_gps_ned_pos coordinate **/
struct Int32Vect2 external_gps_h_pos;
//cm to m and store in external_gps_h_pos
//check function ins_update_gps() for reference.
INT32_VECT2_SCALE_2(external_gps_h_pos,external_gps_ned_pos,INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
//subsitute the guidance_h_pos_sp
VECT2_COPY(guidance_h_pos_sp,external_gps_h_pos);
/*
* the above code is used for congruity, but waste space and time.
* simple using
*
* guidance_h_pos_sp.x = external_gps_ned_pos.x;
* guidance_h_pos_sp.y = external_gps_ned_pox.y;
*
* can achieve the same effect.
*/
}
else break;//hover pos_sp no change
}
/**------------------------------------------------------------------*/
#if defined USE_NAVIGATION
case DL_BLOCK :
{
if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) break;
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
}
break;
case DL_MOVE_WP :
{
uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
if (ac_id != AC_ID) break;
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
struct LlaCoor_i lla;
struct EnuCoor_i enu;
lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
/* WP_alt is in cm, lla.alt in mm */
lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
}
break;
#endif /* USE_NAVIGATION */
#ifdef RADIO_CONTROL_TYPE_DATALINK
case DL_RC_3CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
break;
case DL_RC_4CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(
DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
default:
break;
}
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
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);
}
}
void dl_parse_msg(void)
{
datalink_time = 0;
uint8_t msg_id = IdOfMsg(dl_buffer);
switch (msg_id) {
case DL_PING: {
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
break;
case DL_SETTING : {
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_GET_SETTING : {
if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
break;
#if defined USE_NAVIGATION
case DL_BLOCK : {
if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
}
break;
case DL_MOVE_WP : {
uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
if (ac_id != AC_ID) { break; }
if (stateIsLocalCoordinateValid()) {
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
struct LlaCoor_i lla;
lla.lat = DL_MOVE_WP_lat(dl_buffer);
lla.lon = DL_MOVE_WP_lon(dl_buffer);
/* WP_alt from message is alt above MSL in mm
* lla.alt is above ellipsoid in mm
*/
lla.alt = DL_MOVE_WP_alt(dl_buffer) - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
waypoint_move_lla(wp_id, &lla);
}
}
break;
#endif /* USE_NAVIGATION */
#ifdef RADIO_CONTROL_TYPE_DATALINK
case DL_RC_3CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
break;
case DL_RC_4CH :
if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
}
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
#if defined GPS_DATALINK
case DL_REMOTE_GPS :
// Check if the GPS is for this AC
if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; }
// Parse the GPS
parse_gps_datalink(
DL_REMOTE_GPS_numsv(dl_buffer),
DL_REMOTE_GPS_ecef_x(dl_buffer),
DL_REMOTE_GPS_ecef_y(dl_buffer),
DL_REMOTE_GPS_ecef_z(dl_buffer),
DL_REMOTE_GPS_lat(dl_buffer),
DL_REMOTE_GPS_lon(dl_buffer),
DL_REMOTE_GPS_alt(dl_buffer),
DL_REMOTE_GPS_hmsl(dl_buffer),
DL_REMOTE_GPS_ecef_xd(dl_buffer),
DL_REMOTE_GPS_ecef_yd(dl_buffer),
DL_REMOTE_GPS_ecef_zd(dl_buffer),
DL_REMOTE_GPS_tow(dl_buffer),
DL_REMOTE_GPS_course(dl_buffer));
break;
#endif
default:
break;
}
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
void dl_parse_msg(void)
{
uint8_t msg_id = IdOfMsg(dl_buffer);
switch (msg_id) {
case DL_PING: {
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
break;
case DL_SETTING : {
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_GET_SETTING : {
if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
break;
#ifdef USE_NAVIGATION
case DL_BLOCK : {
if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
}
break;
case DL_MOVE_WP : {
uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
if (ac_id != AC_ID) { break; }
if (stateIsLocalCoordinateValid()) {
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
struct LlaCoor_i lla;
lla.lat = DL_MOVE_WP_lat(dl_buffer);
lla.lon = DL_MOVE_WP_lon(dl_buffer);
/* WP_alt from message is alt above MSL in mm
* lla.alt is above ellipsoid in mm
*/
lla.alt = DL_MOVE_WP_alt(dl_buffer) - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
waypoint_move_lla(wp_id, &lla);
}
}
break;
#endif /* USE_NAVIGATION */
#ifdef RADIO_CONTROL_TYPE_DATALINK
case DL_RC_3CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
break;
case DL_RC_4CH :
if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
}
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
#if USE_GPS
#ifdef GPS_DATALINK
case DL_REMOTE_GPS_SMALL : {
// Check if the GPS is for this AC
if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { break; }
parse_gps_datalink_small(
DL_REMOTE_GPS_SMALL_numsv(dl_buffer),
DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer),
DL_REMOTE_GPS_SMALL_speed_xyz(dl_buffer),
DL_REMOTE_GPS_SMALL_heading(dl_buffer));
}
break;
case DL_REMOTE_GPS : {
// Check if the GPS is for this AC
if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; }
// Parse the GPS
parse_gps_datalink(
DL_REMOTE_GPS_numsv(dl_buffer),
DL_REMOTE_GPS_ecef_x(dl_buffer),
DL_REMOTE_GPS_ecef_y(dl_buffer),
DL_REMOTE_GPS_ecef_z(dl_buffer),
DL_REMOTE_GPS_lat(dl_buffer),
DL_REMOTE_GPS_lon(dl_buffer),
DL_REMOTE_GPS_alt(dl_buffer),
DL_REMOTE_GPS_hmsl(dl_buffer),
DL_REMOTE_GPS_ecef_xd(dl_buffer),
DL_REMOTE_GPS_ecef_yd(dl_buffer),
DL_REMOTE_GPS_ecef_zd(dl_buffer),
DL_REMOTE_GPS_tow(dl_buffer),
DL_REMOTE_GPS_course(dl_buffer));
}
break;
#endif // GPS_DATALINK
case DL_GPS_INJECT : {
// Check if the GPS is for this AC
if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
// GPS parse data
gps_inject_data(
DL_GPS_INJECT_packet_id(dl_buffer),
DL_GPS_INJECT_data_length(dl_buffer),
DL_GPS_INJECT_data(dl_buffer)
);
}
break;
#endif // USE_GPS
case DL_GUIDED_SETPOINT_NED:
if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t flags = DL_GUIDED_SETPOINT_NED_flags(dl_buffer);
float x = DL_GUIDED_SETPOINT_NED_x(dl_buffer);
float y = DL_GUIDED_SETPOINT_NED_y(dl_buffer);
float z = DL_GUIDED_SETPOINT_NED_z(dl_buffer);
float yaw = DL_GUIDED_SETPOINT_NED_yaw(dl_buffer);
switch (flags) {
case 0x00:
case 0x02:
/* local NED position setpoints */
autopilot_guided_goto_ned(x, y, z, yaw);
break;
case 0x01:
/* local NED offset position setpoints */
autopilot_guided_goto_ned_relative(x, y, z, yaw);
break;
case 0x03:
/* body NED offset position setpoints */
autopilot_guided_goto_body_relative(x, y, z, yaw);
break;
default:
/* others not handled yet */
break;
}
break;
default:
break;
}
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
