mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[datalink] parse the correct datalink buffer (#2494)
When using a different datalink buffer than dl_buffer, the callback functions from the modules must use the buffer address passed as a parameter, otherwise there is no guarentee that the correct buffer will be parsed. Co-authored-by: pprz <xp31@free.fr>
This commit is contained in:
Gautier Hattenberger
committed by
GitHub
GitHub
parent
96126e2878
commit
683037ad95
@@ -32,10 +32,10 @@ This module needs of custom messages in messages.xml. Please, check the wiki htt
|
||||
|
||||
<init fun="ctc_init()"/>
|
||||
|
||||
<datalink message="CTC_REG_TABLE" fun="parse_ctc_RegTable()"/>
|
||||
<datalink message="CTC_CLEAN_TABLE" fun="parse_ctc_CleanTable()"/>
|
||||
<datalink message="CTC_INFO_TO_NEI" fun="parse_ctc_NeiInfoTable()"/>
|
||||
<datalink message="CTC_INFO_FROM_TARGET" fun="parse_ctc_TargetInfo()"/>
|
||||
<datalink message="CTC_REG_TABLE" fun="parse_ctc_RegTable(buf)"/>
|
||||
<datalink message="CTC_CLEAN_TABLE" fun="parse_ctc_CleanTable(buf)"/>
|
||||
<datalink message="CTC_INFO_TO_NEI" fun="parse_ctc_NeiInfoTable(buf)"/>
|
||||
<datalink message="CTC_INFO_FROM_TARGET" fun="parse_ctc_TargetInfo(buf)"/>
|
||||
|
||||
<makefile firmware="fixedwing">
|
||||
<file name="ctc.c"/>
|
||||
|
||||
@@ -17,8 +17,8 @@
|
||||
|
||||
<periodic fun="ctc_target_send_info_to_nei()" freq="10"/>
|
||||
|
||||
<datalink message="CTC_REG_TABLE" fun="parse_ctc_target_RegTable()"/>
|
||||
<datalink message="CTC_CLEAN_TABLE" fun="parse_ctc_target_CleanTable()"/>
|
||||
<datalink message="CTC_REG_TABLE" fun="parse_ctc_target_RegTable(buf)"/>
|
||||
<datalink message="CTC_CLEAN_TABLE" fun="parse_ctc_target_CleanTable(buf)"/>
|
||||
|
||||
<makefile>
|
||||
<file name="ctc_target.c"/>
|
||||
|
||||
@@ -24,7 +24,7 @@
|
||||
<periodic fun="atmega_i2c_cam_ctrl_periodic()" autorun="TRUE" freq="10"/>
|
||||
<event fun="atmega_i2c_cam_ctrl_event()"/>
|
||||
|
||||
<datalink message="PAYLOAD_COMMAND" fun="ParseCameraCommand()"/>
|
||||
<datalink message="PAYLOAD_COMMAND" fun="ParseCameraCommand(buf)"/>
|
||||
|
||||
<makefile target="ap">
|
||||
<configure name="ATMEGA_I2C_DEV" default="i2c0" case="upper|lower"/>
|
||||
|
||||
@@ -35,8 +35,8 @@
|
||||
|
||||
<init fun="dcf_init()"/>
|
||||
|
||||
<datalink message="DCF_REG_TABLE" fun="parseRegTable()"/>
|
||||
<datalink message="DCF_THETA" fun="parseThetaTable()"/>
|
||||
<datalink message="DCF_REG_TABLE" fun="parseRegTable(buf)"/>
|
||||
<datalink message="DCF_THETA" fun="parseThetaTable(buf)"/>
|
||||
|
||||
<makefile firmware="fixedwing">
|
||||
<file name="dcf.c"/>
|
||||
|
||||
@@ -14,7 +14,7 @@
|
||||
|
||||
<init fun = "fc_rotor_init()"/>
|
||||
|
||||
<datalink message="DESIRED_SETPOINT" fun="fc_read_msg()"/>
|
||||
<datalink message="DESIRED_SETPOINT" fun="fc_read_msg(buf)"/>
|
||||
|
||||
<makefile firmware="rotorcraft">
|
||||
<define name="FC_ROTOR"/>
|
||||
|
||||
@@ -21,8 +21,8 @@
|
||||
<file name="formation.h"/>
|
||||
</header>
|
||||
<init fun="formation_init()"/>
|
||||
<datalink message="FORMATION_STATUS" fun="parseFormationStatus()"/>
|
||||
<datalink message="FORMATION_SLOT" fun="parseFormationSlot()"/>
|
||||
<datalink message="FORMATION_STATUS" fun="parseFormationStatus(buf)"/>
|
||||
<datalink message="FORMATION_SLOT" fun="parseFormationSlot(buf)"/>
|
||||
<makefile>
|
||||
<file name="formation.c"/>
|
||||
</makefile>
|
||||
|
||||
@@ -15,9 +15,9 @@
|
||||
</header>
|
||||
<init fun="gps_datalink_init()"/>
|
||||
<periodic fun="gps_datalink_periodic_check()" freq="1." autorun="TRUE"/>
|
||||
<datalink message="REMOTE_GPS" fun="gps_datalink_parse_REMOTE_GPS()"/>
|
||||
<datalink message="REMOTE_GPS_SMALL" fun="gps_datalink_parse_REMOTE_GPS_SMALL()"/>
|
||||
<datalink message="REMOTE_GPS_LOCAL" fun="gps_datalink_parse_REMOTE_GPS_LOCAL()"/>
|
||||
<datalink message="REMOTE_GPS" fun="gps_datalink_parse_REMOTE_GPS(buf)"/>
|
||||
<datalink message="REMOTE_GPS_SMALL" fun="gps_datalink_parse_REMOTE_GPS_SMALL(buf)"/>
|
||||
<datalink message="REMOTE_GPS_LOCAL" fun="gps_datalink_parse_REMOTE_GPS_LOCAL(buf)"/>
|
||||
<makefile target="ap|fbw">
|
||||
<file name="gps_datalink.c" dir="subsystems/gps"/>
|
||||
<raw>
|
||||
|
||||
@@ -14,7 +14,7 @@
|
||||
<file name="joystick.h"/>
|
||||
</header>
|
||||
<init fun="joystick_init()"/>
|
||||
<datalink message="JOYSTICK_RAW" fun="joystick_parse()"/>
|
||||
<datalink message="JOYSTICK_RAW" fun="joystick_parse(buf)"/>
|
||||
<makefile>
|
||||
<file name="joystick.c"/>
|
||||
</makefile>
|
||||
|
||||
@@ -20,7 +20,7 @@
|
||||
<init fun="init_mf_daq()"/>
|
||||
<periodic fun="mf_daq_send_state()" freq="10."/>
|
||||
<periodic fun="mf_daq_send_report()" freq="1." autorun="TRUE"/>
|
||||
<datalink message="PAYLOAD_FLOAT" fun="parse_mf_daq_msg()"/>
|
||||
<datalink message="PAYLOAD_FLOAT" fun="parse_mf_daq_msg(buf)"/>
|
||||
<makefile target="ap">
|
||||
<file name="meteo_france_DAQ.c"/>
|
||||
</makefile>
|
||||
|
||||
+12
-12
@@ -14,18 +14,18 @@
|
||||
<init fun="mission_init()"/>
|
||||
<periodic fun="mission_status_report()" freq="2" autorun="TRUE"/>
|
||||
|
||||
<datalink message="MISSION_GOTO_WP" fun="mission_parse_GOTO_WP()"/>
|
||||
<datalink message="MISSION_GOTO_WP_LLA" fun="mission_parse_GOTO_WP_LLA()"/>
|
||||
<datalink message="MISSION_CIRCLE" fun="mission_parse_CIRCLE()"/>
|
||||
<datalink message="MISSION_CIRCLE_LLA" fun="mission_parse_CIRCLE_LLA()"/>
|
||||
<datalink message="MISSION_SEGMENT" fun="mission_parse_SEGMENT()"/>
|
||||
<datalink message="MISSION_SEGMENT_LLA" fun="mission_parse_SEGMENT_LLA()"/>
|
||||
<datalink message="MISSION_PATH" fun="mission_parse_PATH()"/>
|
||||
<datalink message="MISSION_PATH_LLA" fun="mission_parse_PATH_LLA()"/>
|
||||
<datalink message="MISSION_CUSTOM" fun="mission_parse_CUSTOM()"/>
|
||||
<datalink message="GOTO_MISSION" fun="mission_parse_GOTO_MISSION()"/>
|
||||
<datalink message="NEXT_MISSION" fun="mission_parse_NEXT_MISSION()"/>
|
||||
<datalink message="END_MISSION" fun="mission_parse_END_MISSION()"/>
|
||||
<datalink message="MISSION_GOTO_WP" fun="mission_parse_GOTO_WP(buf)"/>
|
||||
<datalink message="MISSION_GOTO_WP_LLA" fun="mission_parse_GOTO_WP_LLA(buf)"/>
|
||||
<datalink message="MISSION_CIRCLE" fun="mission_parse_CIRCLE(buf)"/>
|
||||
<datalink message="MISSION_CIRCLE_LLA" fun="mission_parse_CIRCLE_LLA(buf)"/>
|
||||
<datalink message="MISSION_SEGMENT" fun="mission_parse_SEGMENT(buf)"/>
|
||||
<datalink message="MISSION_SEGMENT_LLA" fun="mission_parse_SEGMENT_LLA(buf)"/>
|
||||
<datalink message="MISSION_PATH" fun="mission_parse_PATH(buf)"/>
|
||||
<datalink message="MISSION_PATH_LLA" fun="mission_parse_PATH_LLA(buf)"/>
|
||||
<datalink message="MISSION_CUSTOM" fun="mission_parse_CUSTOM(buf)"/>
|
||||
<datalink message="GOTO_MISSION" fun="mission_parse_GOTO_MISSION(buf)"/>
|
||||
<datalink message="NEXT_MISSION" fun="mission_parse_NEXT_MISSION(buf)"/>
|
||||
<datalink message="END_MISSION" fun="mission_parse_END_MISSION(buf)"/>
|
||||
|
||||
<makefile>
|
||||
<define name="USE_MISSION"/>
|
||||
|
||||
@@ -14,18 +14,18 @@
|
||||
<init fun="mission_init()"/>
|
||||
<periodic fun="mission_status_report()" freq="2" autorun="TRUE"/>
|
||||
|
||||
<datalink message="MISSION_GOTO_WP" fun="mission_parse_GOTO_WP()"/>
|
||||
<datalink message="MISSION_GOTO_WP_LLA" fun="mission_parse_GOTO_WP_LLA()"/>
|
||||
<datalink message="MISSION_CIRCLE" fun="mission_parse_CIRCLE()"/>
|
||||
<datalink message="MISSION_CIRCLE_LLA" fun="mission_parse_CIRCLE_LLA()"/>
|
||||
<datalink message="MISSION_SEGMENT" fun="mission_parse_SEGMENT()"/>
|
||||
<datalink message="MISSION_SEGMENT_LLA" fun="mission_parse_SEGMENT_LLA()"/>
|
||||
<datalink message="MISSION_PATH" fun="mission_parse_PATH()"/>
|
||||
<datalink message="MISSION_PATH_LLA" fun="mission_parse_PATH_LLA()"/>
|
||||
<datalink message="MISSION_CUSTOM" fun="mission_parse_CUSTOM()"/>
|
||||
<datalink message="GOTO_MISSION" fun="mission_parse_GOTO_MISSION()"/>
|
||||
<datalink message="NEXT_MISSION" fun="mission_parse_NEXT_MISSION()"/>
|
||||
<datalink message="END_MISSION" fun="mission_parse_END_MISSION()"/>
|
||||
<datalink message="MISSION_GOTO_WP" fun="mission_parse_GOTO_WP(buf)"/>
|
||||
<datalink message="MISSION_GOTO_WP_LLA" fun="mission_parse_GOTO_WP_LLA(buf)"/>
|
||||
<datalink message="MISSION_CIRCLE" fun="mission_parse_CIRCLE(buf)"/>
|
||||
<datalink message="MISSION_CIRCLE_LLA" fun="mission_parse_CIRCLE_LLA(buf)"/>
|
||||
<datalink message="MISSION_SEGMENT" fun="mission_parse_SEGMENT(buf)"/>
|
||||
<datalink message="MISSION_SEGMENT_LLA" fun="mission_parse_SEGMENT_LLA(buf)"/>
|
||||
<datalink message="MISSION_PATH" fun="mission_parse_PATH(buf)"/>
|
||||
<datalink message="MISSION_PATH_LLA" fun="mission_parse_PATH_LLA(buf)"/>
|
||||
<datalink message="MISSION_CUSTOM" fun="mission_parse_CUSTOM(buf)"/>
|
||||
<datalink message="GOTO_MISSION" fun="mission_parse_GOTO_MISSION(buf)"/>
|
||||
<datalink message="NEXT_MISSION" fun="mission_parse_NEXT_MISSION(buf)"/>
|
||||
<datalink message="END_MISSION" fun="mission_parse_END_MISSION(buf)"/>
|
||||
|
||||
<makefile>
|
||||
<define name="USE_MISSION"/>
|
||||
|
||||
@@ -56,7 +56,7 @@ On boards with CAM_SWITCH, ROTORCRAFT_CAM_SWITCH_GPIO can be defined to CAM_SWIT
|
||||
</header>
|
||||
<init fun="rotorcraft_cam_init()"/>
|
||||
<periodic fun="rotorcraft_cam_periodic()" freq="10."/>
|
||||
<datalink message="ROTORCRAFT_CAM_STICK" fun="ROTORCRAFT_CAM_STICK_PARSE(dl_buffer)"/>
|
||||
<datalink message="ROTORCRAFT_CAM_STICK" fun="ROTORCRAFT_CAM_STICK_PARSE(buf)"/>
|
||||
<makefile>
|
||||
<file name="rotorcraft_cam.c"/>
|
||||
</makefile>
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
<file name="rssi.h"/>
|
||||
</header>
|
||||
<init fun="rssi_init()"/>
|
||||
<datalink message="RSSI" fun="parse_rssi_dl()"/>
|
||||
<datalink message="RSSI" fun="parse_rssi_dl(buf)"/>
|
||||
<makefile>
|
||||
<file name="rssi.c"/>
|
||||
</makefile>
|
||||
|
||||
@@ -11,8 +11,8 @@
|
||||
<init fun="tcas_init()"/>
|
||||
<periodic fun="tcas_periodic_task_1Hz()" freq="1"/>
|
||||
<periodic fun="tcas_periodic_task_4Hz()" freq="4"/>
|
||||
<datalink message="TCAS_RESOLVE" fun="parseTcasResolve()"/>
|
||||
<datalink message="TCAS_RA" fun="parseTcasRA()"/>
|
||||
<datalink message="TCAS_RESOLVE" fun="parseTcasResolve(buf)"/>
|
||||
<datalink message="TCAS_RA" fun="parseTcasRA(buf)"/>
|
||||
<makefile>
|
||||
<file name="tcas.c"/>
|
||||
<define name="TCAS"/>
|
||||
|
||||
@@ -9,11 +9,11 @@
|
||||
</header>
|
||||
<init fun="traffic_info_init()"/>
|
||||
|
||||
<datalink message="ACINFO" fun="parse_acinfo_dl()"/>
|
||||
<datalink message="ACINFO_LLA" fun="parse_acinfo_dl()"/>
|
||||
<datalink message="GPS_SMALL" fun="parse_acinfo_dl()"/>
|
||||
<datalink message="GPS" fun="parse_acinfo_dl()"/>
|
||||
<datalink message="GPS_LLA" fun="parse_acinfo_dl()"/>
|
||||
<datalink message="ACINFO" fun="parse_acinfo_dl(buf)"/>
|
||||
<datalink message="ACINFO_LLA" fun="parse_acinfo_dl(buf)"/>
|
||||
<datalink message="GPS_SMALL" fun="parse_acinfo_dl(buf)"/>
|
||||
<datalink message="GPS" fun="parse_acinfo_dl(buf)"/>
|
||||
<datalink message="GPS_LLA" fun="parse_acinfo_dl(buf)"/>
|
||||
|
||||
<makefile>
|
||||
<file name="traffic_info.c"/>
|
||||
|
||||
@@ -19,8 +19,8 @@
|
||||
</header>
|
||||
<init fun="vi_init()"/>
|
||||
<periodic fun="vi_periodic()" freq="25"/>
|
||||
<datalink message="BOOZ2_FMS_COMMAND" fun="VI_PARSE_DATALINK(dl_buffer)"/>
|
||||
<datalink message="BOOZ_NAV_STICK" fun="VI_NAV_STICK_PARSE_DL(dl_buffer)"/>
|
||||
<datalink message="BOOZ2_FMS_COMMAND" fun="VI_PARSE_DATALINK(buf)"/>
|
||||
<datalink message="BOOZ_NAV_STICK" fun="VI_NAV_STICK_PARSE_DL(buf)"/>
|
||||
<makefile>
|
||||
<file name="vi.c"/>
|
||||
<file name="vi_datalink.c"/>
|
||||
|
||||
@@ -116,9 +116,9 @@ extern void rotorcraft_cam_set_mode(uint8_t mode);
|
||||
#define ROTORCRAFT_CAM_STICK_PAN_INC RadOfDeg(20.)
|
||||
#endif
|
||||
|
||||
#define ROTORCRAFT_CAM_STICK_PARSE(_dl_buffer) { \
|
||||
rotorcraft_cam_tilt += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_TILT_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_tilt(_dl_buffer)); \
|
||||
rotorcraft_cam_pan += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_PAN_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_pan(dl_buffer)); \
|
||||
#define ROTORCRAFT_CAM_STICK_PARSE(buf) { \
|
||||
rotorcraft_cam_tilt += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_TILT_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_tilt(buf)); \
|
||||
rotorcraft_cam_pan += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_PAN_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_pan(buf)); \
|
||||
INT32_COURSE_NORMALIZE(rotorcraft_cam_pan); \
|
||||
}
|
||||
|
||||
|
||||
@@ -35,8 +35,8 @@ void atmega_i2c_cam_ctrl_send(uint8_t cmd);
|
||||
// Allow commands to be set by datalink
|
||||
#define ParseCameraCommand() { \
|
||||
{ \
|
||||
if ( DL_PAYLOAD_COMMAND_command_length(dl_buffer) == 1){ \
|
||||
dc_send_command(DL_PAYLOAD_COMMAND_command(dl_buffer)[0]); \
|
||||
if ( DL_PAYLOAD_COMMAND_command_length(buf) == 1){ \
|
||||
dc_send_command(DL_PAYLOAD_COMMAND_command(buf)[0]); \
|
||||
} \
|
||||
} \
|
||||
}
|
||||
|
||||
@@ -39,13 +39,13 @@ void joystick_init(void)
|
||||
joystick.throttle = 0;
|
||||
}
|
||||
|
||||
void joystick_parse(void)
|
||||
void joystick_parse(uint8_t *buf)
|
||||
{
|
||||
if (DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
|
||||
joystick.roll = DL_JOYSTICK_RAW_roll(dl_buffer);
|
||||
joystick.pitch = DL_JOYSTICK_RAW_pitch(dl_buffer);
|
||||
joystick.yaw = DL_JOYSTICK_RAW_yaw(dl_buffer);
|
||||
joystick.throttle = DL_JOYSTICK_RAW_throttle(dl_buffer);
|
||||
if (DL_JOYSTICK_RAW_ac_id(buf) == AC_ID) {
|
||||
joystick.roll = DL_JOYSTICK_RAW_roll(buf);
|
||||
joystick.pitch = DL_JOYSTICK_RAW_pitch(buf);
|
||||
joystick.yaw = DL_JOYSTICK_RAW_yaw(buf);
|
||||
joystick.throttle = DL_JOYSTICK_RAW_throttle(buf);
|
||||
AbiSendMsgJOYSTICK(JOYSTICK_ID, joystick.roll, joystick.pitch, joystick.yaw, joystick.throttle);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -49,7 +49,7 @@ extern void joystick_init(void);
|
||||
* JOYSTICK_RAW message parser
|
||||
* if valid, send a JOYSTICK ABI message
|
||||
*/
|
||||
extern void joystick_parse(void);
|
||||
extern void joystick_parse(uint8_t *buf);
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -115,14 +115,14 @@ void mf_daq_send_report(void)
|
||||
}
|
||||
}
|
||||
|
||||
void parse_mf_daq_msg(void)
|
||||
void parse_mf_daq_msg(uint8_t *buf)
|
||||
{
|
||||
mf_daq.nb = dl_buffer[2];
|
||||
mf_daq.nb = buf[2];
|
||||
if (mf_daq.nb > 0) {
|
||||
if (mf_daq.nb > MF_DAQ_SIZE) { mf_daq.nb = MF_DAQ_SIZE; }
|
||||
// Store data struct directly from dl_buffer
|
||||
float *buf = (float*)(dl_buffer+3);
|
||||
memcpy(mf_daq.values, buf, mf_daq.nb * sizeof(float));
|
||||
float *bufloc = (float*)(buf+3);
|
||||
memcpy(mf_daq.values, bufloc, mf_daq.nb * sizeof(float));
|
||||
// Log on SD card
|
||||
if (log_started) {
|
||||
DOWNLINK_SEND_PAYLOAD_FLOAT(pprzlog_tp, chibios_sdlog, mf_daq.nb, mf_daq.values);
|
||||
|
||||
@@ -51,7 +51,7 @@ extern struct MF_DAQ mf_daq;
|
||||
extern void init_mf_daq(void);
|
||||
extern void mf_daq_send_state(void);
|
||||
extern void mf_daq_send_report(void);
|
||||
extern void parse_mf_daq_msg(void);
|
||||
extern void parse_mf_daq_msg(uint8_t *buf);
|
||||
|
||||
#if (defined MF_DAQ_POWER_PORT) && (defined MF_DAQ_POWER_PIN)
|
||||
#define meteo_france_DAQ_SetPower(_x) { \
|
||||
|
||||
@@ -162,227 +162,227 @@ void mission_status_report(void)
|
||||
// Parsing functions //
|
||||
///////////////////////
|
||||
|
||||
int mission_parse_GOTO_WP(void)
|
||||
int mission_parse_GOTO_WP(uint8_t *buf)
|
||||
{
|
||||
if (DL_MISSION_GOTO_WP_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_MISSION_GOTO_WP_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionWP;
|
||||
me.element.mission_wp.wp.wp_f.x = DL_MISSION_GOTO_WP_wp_east(dl_buffer);
|
||||
me.element.mission_wp.wp.wp_f.y = DL_MISSION_GOTO_WP_wp_north(dl_buffer);
|
||||
me.element.mission_wp.wp.wp_f.z = DL_MISSION_GOTO_WP_wp_alt(dl_buffer);
|
||||
me.duration = DL_MISSION_GOTO_WP_duration(dl_buffer);
|
||||
me.index = DL_MISSION_GOTO_WP_index(dl_buffer);
|
||||
me.element.mission_wp.wp.wp_f.x = DL_MISSION_GOTO_WP_wp_east(buf);
|
||||
me.element.mission_wp.wp.wp_f.y = DL_MISSION_GOTO_WP_wp_north(buf);
|
||||
me.element.mission_wp.wp.wp_f.z = DL_MISSION_GOTO_WP_wp_alt(buf);
|
||||
me.duration = DL_MISSION_GOTO_WP_duration(buf);
|
||||
me.index = DL_MISSION_GOTO_WP_index(buf);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_insert(dl_buffer));
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_insert(buf));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_GOTO_WP_LLA(void)
|
||||
int mission_parse_GOTO_WP_LLA(uint8_t *buf)
|
||||
{
|
||||
if (DL_MISSION_GOTO_WP_LLA_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_MISSION_GOTO_WP_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
struct LlaCoor_i lla;
|
||||
lla.lat = DL_MISSION_GOTO_WP_LLA_wp_lat(dl_buffer);
|
||||
lla.lon = DL_MISSION_GOTO_WP_LLA_wp_lon(dl_buffer);
|
||||
lla.alt = DL_MISSION_GOTO_WP_LLA_wp_alt(dl_buffer);
|
||||
lla.lat = DL_MISSION_GOTO_WP_LLA_wp_lat(buf);
|
||||
lla.lon = DL_MISSION_GOTO_WP_LLA_wp_lon(buf);
|
||||
lla.alt = DL_MISSION_GOTO_WP_LLA_wp_alt(buf);
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionWP;
|
||||
// if there is no valid local coordinate, do not insert mission element
|
||||
if (!mission_point_of_lla(&me.element.mission_wp.wp.wp_f, &lla)) { return false; }
|
||||
me.duration = DL_MISSION_GOTO_WP_LLA_duration(dl_buffer);
|
||||
me.index = DL_MISSION_GOTO_WP_LLA_index(dl_buffer);
|
||||
me.duration = DL_MISSION_GOTO_WP_LLA_duration(buf);
|
||||
me.index = DL_MISSION_GOTO_WP_LLA_index(buf);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_LLA_insert(dl_buffer));
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_LLA_insert(buf));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_CIRCLE(void)
|
||||
int mission_parse_CIRCLE(uint8_t *buf)
|
||||
{
|
||||
if (DL_MISSION_CIRCLE_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_MISSION_CIRCLE_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionCircle;
|
||||
me.element.mission_circle.center.center_f.x = DL_MISSION_CIRCLE_center_east(dl_buffer);
|
||||
me.element.mission_circle.center.center_f.y = DL_MISSION_CIRCLE_center_north(dl_buffer);
|
||||
me.element.mission_circle.center.center_f.z = DL_MISSION_CIRCLE_center_alt(dl_buffer);
|
||||
me.element.mission_circle.radius = DL_MISSION_CIRCLE_radius(dl_buffer);
|
||||
me.duration = DL_MISSION_CIRCLE_duration(dl_buffer);
|
||||
me.index = DL_MISSION_CIRCLE_index(dl_buffer);
|
||||
me.element.mission_circle.center.center_f.x = DL_MISSION_CIRCLE_center_east(buf);
|
||||
me.element.mission_circle.center.center_f.y = DL_MISSION_CIRCLE_center_north(buf);
|
||||
me.element.mission_circle.center.center_f.z = DL_MISSION_CIRCLE_center_alt(buf);
|
||||
me.element.mission_circle.radius = DL_MISSION_CIRCLE_radius(buf);
|
||||
me.duration = DL_MISSION_CIRCLE_duration(buf);
|
||||
me.index = DL_MISSION_CIRCLE_index(buf);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_insert(dl_buffer));
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_insert(buf));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_CIRCLE_LLA(void)
|
||||
int mission_parse_CIRCLE_LLA(uint8_t *buf)
|
||||
{
|
||||
if (DL_MISSION_CIRCLE_LLA_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_MISSION_CIRCLE_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
struct LlaCoor_i lla;
|
||||
lla.lat = DL_MISSION_CIRCLE_LLA_center_lat(dl_buffer);
|
||||
lla.lon = DL_MISSION_CIRCLE_LLA_center_lon(dl_buffer);
|
||||
lla.alt = DL_MISSION_CIRCLE_LLA_center_alt(dl_buffer);
|
||||
lla.lat = DL_MISSION_CIRCLE_LLA_center_lat(buf);
|
||||
lla.lon = DL_MISSION_CIRCLE_LLA_center_lon(buf);
|
||||
lla.alt = DL_MISSION_CIRCLE_LLA_center_alt(buf);
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionCircle;
|
||||
// if there is no valid local coordinate, do not insert mission element
|
||||
if (!mission_point_of_lla(&me.element.mission_circle.center.center_f, &lla)) { return false; }
|
||||
me.element.mission_circle.radius = DL_MISSION_CIRCLE_LLA_radius(dl_buffer);
|
||||
me.duration = DL_MISSION_CIRCLE_LLA_duration(dl_buffer);
|
||||
me.index = DL_MISSION_CIRCLE_LLA_index(dl_buffer);
|
||||
me.element.mission_circle.radius = DL_MISSION_CIRCLE_LLA_radius(buf);
|
||||
me.duration = DL_MISSION_CIRCLE_LLA_duration(buf);
|
||||
me.index = DL_MISSION_CIRCLE_LLA_index(buf);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_LLA_insert(dl_buffer));
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_LLA_insert(buf));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_SEGMENT(void)
|
||||
int mission_parse_SEGMENT(uint8_t *buf)
|
||||
{
|
||||
if (DL_MISSION_SEGMENT_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_MISSION_SEGMENT_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionSegment;
|
||||
me.element.mission_segment.from.from_f.x = DL_MISSION_SEGMENT_segment_east_1(dl_buffer);
|
||||
me.element.mission_segment.from.from_f.y = DL_MISSION_SEGMENT_segment_north_1(dl_buffer);
|
||||
me.element.mission_segment.from.from_f.z = DL_MISSION_SEGMENT_segment_alt(dl_buffer);
|
||||
me.element.mission_segment.to.to_f.x = DL_MISSION_SEGMENT_segment_east_2(dl_buffer);
|
||||
me.element.mission_segment.to.to_f.y = DL_MISSION_SEGMENT_segment_north_2(dl_buffer);
|
||||
me.element.mission_segment.to.to_f.z = DL_MISSION_SEGMENT_segment_alt(dl_buffer);
|
||||
me.duration = DL_MISSION_SEGMENT_duration(dl_buffer);
|
||||
me.index = DL_MISSION_SEGMENT_index(dl_buffer);
|
||||
me.element.mission_segment.from.from_f.x = DL_MISSION_SEGMENT_segment_east_1(buf);
|
||||
me.element.mission_segment.from.from_f.y = DL_MISSION_SEGMENT_segment_north_1(buf);
|
||||
me.element.mission_segment.from.from_f.z = DL_MISSION_SEGMENT_segment_alt(buf);
|
||||
me.element.mission_segment.to.to_f.x = DL_MISSION_SEGMENT_segment_east_2(buf);
|
||||
me.element.mission_segment.to.to_f.y = DL_MISSION_SEGMENT_segment_north_2(buf);
|
||||
me.element.mission_segment.to.to_f.z = DL_MISSION_SEGMENT_segment_alt(buf);
|
||||
me.duration = DL_MISSION_SEGMENT_duration(buf);
|
||||
me.index = DL_MISSION_SEGMENT_index(buf);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_insert(dl_buffer));
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_insert(buf));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_SEGMENT_LLA(void)
|
||||
int mission_parse_SEGMENT_LLA(uint8_t *buf)
|
||||
{
|
||||
if (DL_MISSION_SEGMENT_LLA_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_MISSION_SEGMENT_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
struct LlaCoor_i from_lla, to_lla;
|
||||
from_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_1(dl_buffer);
|
||||
from_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_1(dl_buffer);
|
||||
from_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(dl_buffer);
|
||||
to_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_2(dl_buffer);
|
||||
to_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_2(dl_buffer);
|
||||
to_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(dl_buffer);
|
||||
from_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_1(buf);
|
||||
from_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_1(buf);
|
||||
from_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(buf);
|
||||
to_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_2(buf);
|
||||
to_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_2(buf);
|
||||
to_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(buf);
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionSegment;
|
||||
// if there is no valid local coordinate, do not insert mission element
|
||||
if (!mission_point_of_lla(&me.element.mission_segment.from.from_f, &from_lla)) { return false; }
|
||||
if (!mission_point_of_lla(&me.element.mission_segment.to.to_f, &to_lla)) { return false; }
|
||||
me.duration = DL_MISSION_SEGMENT_LLA_duration(dl_buffer);
|
||||
me.index = DL_MISSION_SEGMENT_LLA_index(dl_buffer);
|
||||
me.duration = DL_MISSION_SEGMENT_LLA_duration(buf);
|
||||
me.index = DL_MISSION_SEGMENT_LLA_index(buf);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_LLA_insert(dl_buffer));
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_LLA_insert(buf));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_PATH(void)
|
||||
int mission_parse_PATH(uint8_t *buf)
|
||||
{
|
||||
if (DL_MISSION_PATH_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_MISSION_PATH_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionPath;
|
||||
me.element.mission_path.path.path_f[0].x = DL_MISSION_PATH_point_east_1(dl_buffer);
|
||||
me.element.mission_path.path.path_f[0].y = DL_MISSION_PATH_point_north_1(dl_buffer);
|
||||
me.element.mission_path.path.path_f[0].z = DL_MISSION_PATH_path_alt(dl_buffer);
|
||||
me.element.mission_path.path.path_f[1].x = DL_MISSION_PATH_point_east_2(dl_buffer);
|
||||
me.element.mission_path.path.path_f[1].y = DL_MISSION_PATH_point_north_2(dl_buffer);
|
||||
me.element.mission_path.path.path_f[1].z = DL_MISSION_PATH_path_alt(dl_buffer);
|
||||
me.element.mission_path.path.path_f[2].x = DL_MISSION_PATH_point_east_3(dl_buffer);
|
||||
me.element.mission_path.path.path_f[2].y = DL_MISSION_PATH_point_north_3(dl_buffer);
|
||||
me.element.mission_path.path.path_f[2].z = DL_MISSION_PATH_path_alt(dl_buffer);
|
||||
me.element.mission_path.path.path_f[3].x = DL_MISSION_PATH_point_east_4(dl_buffer);
|
||||
me.element.mission_path.path.path_f[3].y = DL_MISSION_PATH_point_north_4(dl_buffer);
|
||||
me.element.mission_path.path.path_f[3].z = DL_MISSION_PATH_path_alt(dl_buffer);
|
||||
me.element.mission_path.path.path_f[4].x = DL_MISSION_PATH_point_east_5(dl_buffer);
|
||||
me.element.mission_path.path.path_f[4].y = DL_MISSION_PATH_point_north_5(dl_buffer);
|
||||
me.element.mission_path.path.path_f[4].z = DL_MISSION_PATH_path_alt(dl_buffer);
|
||||
me.element.mission_path.nb = DL_MISSION_PATH_nb(dl_buffer);
|
||||
me.element.mission_path.path.path_f[0].x = DL_MISSION_PATH_point_east_1(buf);
|
||||
me.element.mission_path.path.path_f[0].y = DL_MISSION_PATH_point_north_1(buf);
|
||||
me.element.mission_path.path.path_f[0].z = DL_MISSION_PATH_path_alt(buf);
|
||||
me.element.mission_path.path.path_f[1].x = DL_MISSION_PATH_point_east_2(buf);
|
||||
me.element.mission_path.path.path_f[1].y = DL_MISSION_PATH_point_north_2(buf);
|
||||
me.element.mission_path.path.path_f[1].z = DL_MISSION_PATH_path_alt(buf);
|
||||
me.element.mission_path.path.path_f[2].x = DL_MISSION_PATH_point_east_3(buf);
|
||||
me.element.mission_path.path.path_f[2].y = DL_MISSION_PATH_point_north_3(buf);
|
||||
me.element.mission_path.path.path_f[2].z = DL_MISSION_PATH_path_alt(buf);
|
||||
me.element.mission_path.path.path_f[3].x = DL_MISSION_PATH_point_east_4(buf);
|
||||
me.element.mission_path.path.path_f[3].y = DL_MISSION_PATH_point_north_4(buf);
|
||||
me.element.mission_path.path.path_f[3].z = DL_MISSION_PATH_path_alt(buf);
|
||||
me.element.mission_path.path.path_f[4].x = DL_MISSION_PATH_point_east_5(buf);
|
||||
me.element.mission_path.path.path_f[4].y = DL_MISSION_PATH_point_north_5(buf);
|
||||
me.element.mission_path.path.path_f[4].z = DL_MISSION_PATH_path_alt(buf);
|
||||
me.element.mission_path.nb = DL_MISSION_PATH_nb(buf);
|
||||
if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; }
|
||||
me.element.mission_path.path_idx = 0;
|
||||
me.duration = DL_MISSION_PATH_duration(dl_buffer);
|
||||
me.index = DL_MISSION_PATH_index(dl_buffer);
|
||||
me.duration = DL_MISSION_PATH_duration(buf);
|
||||
me.index = DL_MISSION_PATH_index(buf);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_insert(dl_buffer));
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_insert(buf));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_PATH_LLA(void)
|
||||
int mission_parse_PATH_LLA(uint8_t *buf)
|
||||
{
|
||||
if (DL_MISSION_PATH_LLA_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_MISSION_PATH_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
struct LlaCoor_i lla[MISSION_PATH_NB];
|
||||
lla[0].lat = DL_MISSION_PATH_LLA_point_lat_1(dl_buffer);
|
||||
lla[0].lon = DL_MISSION_PATH_LLA_point_lon_1(dl_buffer);
|
||||
lla[0].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
|
||||
lla[1].lat = DL_MISSION_PATH_LLA_point_lat_2(dl_buffer);
|
||||
lla[1].lon = DL_MISSION_PATH_LLA_point_lon_2(dl_buffer);
|
||||
lla[1].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
|
||||
lla[2].lat = DL_MISSION_PATH_LLA_point_lat_3(dl_buffer);
|
||||
lla[2].lon = DL_MISSION_PATH_LLA_point_lon_3(dl_buffer);
|
||||
lla[2].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
|
||||
lla[3].lat = DL_MISSION_PATH_LLA_point_lat_4(dl_buffer);
|
||||
lla[3].lon = DL_MISSION_PATH_LLA_point_lon_4(dl_buffer);
|
||||
lla[3].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
|
||||
lla[4].lat = DL_MISSION_PATH_LLA_point_lat_5(dl_buffer);
|
||||
lla[4].lon = DL_MISSION_PATH_LLA_point_lon_5(dl_buffer);
|
||||
lla[4].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
|
||||
lla[0].lat = DL_MISSION_PATH_LLA_point_lat_1(buf);
|
||||
lla[0].lon = DL_MISSION_PATH_LLA_point_lon_1(buf);
|
||||
lla[0].alt = DL_MISSION_PATH_LLA_path_alt(buf);
|
||||
lla[1].lat = DL_MISSION_PATH_LLA_point_lat_2(buf);
|
||||
lla[1].lon = DL_MISSION_PATH_LLA_point_lon_2(buf);
|
||||
lla[1].alt = DL_MISSION_PATH_LLA_path_alt(buf);
|
||||
lla[2].lat = DL_MISSION_PATH_LLA_point_lat_3(buf);
|
||||
lla[2].lon = DL_MISSION_PATH_LLA_point_lon_3(buf);
|
||||
lla[2].alt = DL_MISSION_PATH_LLA_path_alt(buf);
|
||||
lla[3].lat = DL_MISSION_PATH_LLA_point_lat_4(buf);
|
||||
lla[3].lon = DL_MISSION_PATH_LLA_point_lon_4(buf);
|
||||
lla[3].alt = DL_MISSION_PATH_LLA_path_alt(buf);
|
||||
lla[4].lat = DL_MISSION_PATH_LLA_point_lat_5(buf);
|
||||
lla[4].lon = DL_MISSION_PATH_LLA_point_lon_5(buf);
|
||||
lla[4].alt = DL_MISSION_PATH_LLA_path_alt(buf);
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionPath;
|
||||
uint8_t i;
|
||||
me.element.mission_path.nb = DL_MISSION_PATH_LLA_nb(dl_buffer);
|
||||
me.element.mission_path.nb = DL_MISSION_PATH_LLA_nb(buf);
|
||||
if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; }
|
||||
for (i = 0; i < me.element.mission_path.nb; i++) {
|
||||
// if there is no valid local coordinate, do not insert mission element
|
||||
if (!mission_point_of_lla(&me.element.mission_path.path.path_f[i], &lla[i])) { return false; }
|
||||
}
|
||||
me.element.mission_path.path_idx = 0;
|
||||
me.duration = DL_MISSION_PATH_LLA_duration(dl_buffer);
|
||||
me.index = DL_MISSION_PATH_LLA_index(dl_buffer);
|
||||
me.duration = DL_MISSION_PATH_LLA_duration(buf);
|
||||
me.index = DL_MISSION_PATH_LLA_index(buf);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_LLA_insert(dl_buffer));
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_LLA_insert(buf));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_CUSTOM(void)
|
||||
int mission_parse_CUSTOM(uint8_t *buf)
|
||||
{
|
||||
if (DL_MISSION_CUSTOM_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_MISSION_CUSTOM_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionCustom;
|
||||
me.element.mission_custom.reg = mission_get_registered(DL_MISSION_CUSTOM_type(dl_buffer));
|
||||
me.element.mission_custom.reg = mission_get_registered(DL_MISSION_CUSTOM_type(buf));
|
||||
if (me.element.mission_custom.reg == NULL) {
|
||||
return false; // unknown type
|
||||
}
|
||||
me.element.mission_custom.nb = DL_MISSION_CUSTOM_params_length(dl_buffer);
|
||||
me.element.mission_custom.nb = DL_MISSION_CUSTOM_params_length(buf);
|
||||
for (int i = 0; i < me.element.mission_custom.nb; i++) {
|
||||
me.element.mission_custom.params[i] = DL_MISSION_CUSTOM_params(dl_buffer)[i];
|
||||
me.element.mission_custom.params[i] = DL_MISSION_CUSTOM_params(buf)[i];
|
||||
}
|
||||
me.duration = DL_MISSION_CUSTOM_duration(dl_buffer);
|
||||
me.index = DL_MISSION_CUSTOM_index(dl_buffer);
|
||||
me.duration = DL_MISSION_CUSTOM_duration(buf);
|
||||
me.index = DL_MISSION_CUSTOM_index(buf);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CUSTOM_insert(dl_buffer));
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CUSTOM_insert(buf));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_GOTO_MISSION(void)
|
||||
int mission_parse_GOTO_MISSION(uint8_t *buf)
|
||||
{
|
||||
if (DL_GOTO_MISSION_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_GOTO_MISSION_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
uint8_t mission_id = DL_GOTO_MISSION_mission_id(dl_buffer);
|
||||
uint8_t mission_id = DL_GOTO_MISSION_mission_id(buf);
|
||||
if (mission_id < MISSION_ELEMENT_NB) {
|
||||
mission.current_idx = mission_id;
|
||||
} else { return false; }
|
||||
@@ -390,9 +390,9 @@ int mission_parse_GOTO_MISSION(void)
|
||||
return true;
|
||||
}
|
||||
|
||||
int mission_parse_NEXT_MISSION(void)
|
||||
int mission_parse_NEXT_MISSION(uint8_t *buf)
|
||||
{
|
||||
if (DL_NEXT_MISSION_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_NEXT_MISSION_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
if (mission.current_idx == mission.insert_idx) { return false; } // already at the last position
|
||||
|
||||
@@ -401,9 +401,9 @@ int mission_parse_NEXT_MISSION(void)
|
||||
return true;
|
||||
}
|
||||
|
||||
int mission_parse_END_MISSION(void)
|
||||
int mission_parse_END_MISSION(uint8_t *buf)
|
||||
{
|
||||
if (DL_END_MISSION_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
|
||||
if (DL_END_MISSION_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
|
||||
|
||||
// set current index to insert index (last position)
|
||||
mission.current_idx = mission.insert_idx;
|
||||
|
||||
@@ -205,18 +205,18 @@ extern void mission_status_report(void);
|
||||
|
||||
/** Parsing functions called when a mission message is received
|
||||
*/
|
||||
extern int mission_parse_GOTO_WP(void);
|
||||
extern int mission_parse_GOTO_WP_LLA(void);
|
||||
extern int mission_parse_CIRCLE(void);
|
||||
extern int mission_parse_CIRCLE_LLA(void);
|
||||
extern int mission_parse_SEGMENT(void);
|
||||
extern int mission_parse_SEGMENT_LLA(void);
|
||||
extern int mission_parse_PATH(void);
|
||||
extern int mission_parse_PATH_LLA(void);
|
||||
extern int mission_parse_CUSTOM(void);
|
||||
extern int mission_parse_GOTO_MISSION(void);
|
||||
extern int mission_parse_NEXT_MISSION(void);
|
||||
extern int mission_parse_END_MISSION(void);
|
||||
extern int mission_parse_GOTO_WP(uint8_t *buf);
|
||||
extern int mission_parse_GOTO_WP_LLA(uint8_t *buf);
|
||||
extern int mission_parse_CIRCLE(uint8_t *buf);
|
||||
extern int mission_parse_CIRCLE_LLA(uint8_t *buf);
|
||||
extern int mission_parse_SEGMENT(uint8_t *buf);
|
||||
extern int mission_parse_SEGMENT_LLA(uint8_t *buf);
|
||||
extern int mission_parse_PATH(uint8_t *buf);
|
||||
extern int mission_parse_PATH_LLA(uint8_t *buf);
|
||||
extern int mission_parse_CUSTOM(uint8_t *buf);
|
||||
extern int mission_parse_GOTO_MISSION(uint8_t *buf);
|
||||
extern int mission_parse_NEXT_MISSION(uint8_t *buf);
|
||||
extern int mission_parse_END_MISSION(uint8_t *buf);
|
||||
|
||||
#endif // MISSION_COMMON_H
|
||||
|
||||
|
||||
@@ -25,7 +25,7 @@
|
||||
#include <stdio.h>
|
||||
|
||||
#include "modules/multi/ctc/ctc.h"
|
||||
#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
//#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
#include "subsystems/datalink/telemetry.h"
|
||||
#include "subsystems/navigation/common_nav.h"
|
||||
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
|
||||
@@ -298,11 +298,11 @@ void ctc_send_info_to_nei(void)
|
||||
}
|
||||
}
|
||||
|
||||
void parse_ctc_RegTable(void)
|
||||
void parse_ctc_RegTable(uint8_t *buf)
|
||||
{
|
||||
uint8_t ac_id = DL_CTC_REG_TABLE_ac_id(dl_buffer);
|
||||
uint8_t ac_id = DL_CTC_REG_TABLE_ac_id(buf);
|
||||
if (ac_id == AC_ID) {
|
||||
uint8_t nei_id = DL_CTC_REG_TABLE_nei_id(dl_buffer);
|
||||
uint8_t nei_id = DL_CTC_REG_TABLE_nei_id(buf);
|
||||
for (int i = 0; i < CTC_MAX_AC; i++)
|
||||
if (tableNei[i][0] == -1) {
|
||||
tableNei[i][0] = (int16_t)nei_id;
|
||||
@@ -311,9 +311,9 @@ void parse_ctc_RegTable(void)
|
||||
}
|
||||
}
|
||||
|
||||
void parse_ctc_CleanTable(void)
|
||||
void parse_ctc_CleanTable(uint8_t *buf)
|
||||
{
|
||||
uint8_t ac_id = DL_CTC_REG_TABLE_ac_id(dl_buffer);
|
||||
uint8_t ac_id = DL_CTC_REG_TABLE_ac_id(uint8_t *buf);
|
||||
if (ac_id == AC_ID)
|
||||
for (int i = 0; i < CTC_MAX_AC; i++) {
|
||||
tableNei[i][0] = -1;
|
||||
@@ -339,24 +339,24 @@ void parse_ctc_CleanTable(void)
|
||||
ctc_gogo = false;
|
||||
}
|
||||
|
||||
void parse_ctc_NeiInfoTable(void)
|
||||
void parse_ctc_NeiInfoTable(uint8_t *buf)
|
||||
{
|
||||
int16_t sender_id = (int16_t)(SenderIdOfPprzMsg(dl_buffer));
|
||||
int16_t sender_id = (int16_t)(SenderIdOfPprzMsg(buf));
|
||||
for (int i = 0; i < CTC_MAX_AC; i++)
|
||||
if (tableNei[i][0] == sender_id) {
|
||||
last_info[i] = get_sys_time_msec();
|
||||
tableNei[i][1] = (int16_t)(DL_CTC_INFO_TO_NEI_vx(dl_buffer) * 100);
|
||||
tableNei[i][2] = (int16_t)(DL_CTC_INFO_TO_NEI_vy(dl_buffer) * 100);
|
||||
tableNei[i][3] = (int16_t)(DL_CTC_INFO_TO_NEI_px(dl_buffer) * 100);
|
||||
tableNei[i][4] = (int16_t)(DL_CTC_INFO_TO_NEI_py(dl_buffer) * 100);
|
||||
tableNei[i][1] = (int16_t)(DL_CTC_INFO_TO_NEI_vx(buf) * 100);
|
||||
tableNei[i][2] = (int16_t)(DL_CTC_INFO_TO_NEI_vy(buf) * 100);
|
||||
tableNei[i][3] = (int16_t)(DL_CTC_INFO_TO_NEI_px(buf) * 100);
|
||||
tableNei[i][4] = (int16_t)(DL_CTC_INFO_TO_NEI_py(buf) * 100);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void parse_ctc_TargetInfo(void)
|
||||
void parse_ctc_TargetInfo(uint8_t *buf)
|
||||
{
|
||||
moving_target_px = DL_CTC_INFO_FROM_TARGET_px(dl_buffer);
|
||||
moving_target_py = DL_CTC_INFO_FROM_TARGET_py(dl_buffer);
|
||||
moving_target_vx = DL_CTC_INFO_FROM_TARGET_vx(dl_buffer);
|
||||
moving_target_vy = DL_CTC_INFO_FROM_TARGET_vy(dl_buffer);
|
||||
moving_target_px = DL_CTC_INFO_FROM_TARGET_px(buf);
|
||||
moving_target_py = DL_CTC_INFO_FROM_TARGET_py(buf);
|
||||
moving_target_vx = DL_CTC_INFO_FROM_TARGET_vx(buf);
|
||||
moving_target_vy = DL_CTC_INFO_FROM_TARGET_vy(buf);
|
||||
}
|
||||
|
||||
@@ -64,9 +64,9 @@ extern bool collective_tracking_point(float x, float y);
|
||||
extern void collective_tracking_control(void);
|
||||
extern void ctc_send_info_to_nei(void);
|
||||
|
||||
extern void parse_ctc_RegTable(void);
|
||||
extern void parse_ctc_CleanTable(void);
|
||||
extern void parse_ctc_NeiInfoTable(void);
|
||||
extern void parse_ctc_TargetInfo(void);
|
||||
extern void parse_ctc_RegTable(uint8_t *buf);
|
||||
extern void parse_ctc_CleanTable(uint8_t *buf);
|
||||
extern void parse_ctc_NeiInfoTable(uint8_t *buf);
|
||||
extern void parse_ctc_TargetInfo(uint8_t *buf);
|
||||
|
||||
#endif // CTC_H
|
||||
|
||||
@@ -25,7 +25,7 @@
|
||||
#include <stdio.h>
|
||||
|
||||
#include "modules/multi/ctc/ctc_target.h"
|
||||
#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
//#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
#include "subsystems/datalink/telemetry.h"
|
||||
#include "firmwares/rotorcraft/navigation.h"
|
||||
#include "autopilot.h"
|
||||
|
||||
@@ -35,7 +35,7 @@ extern int16_t tableNei[][6];
|
||||
extern void ctc_target_init(void);
|
||||
extern void ctc_target_send_info_to_nei(void);
|
||||
|
||||
extern void parse_ctc_target_RegTable(void);
|
||||
extern void parse_ctc_target_CleanTable(void);
|
||||
extern void parse_ctc_target_RegTable(uint8_t *buf);
|
||||
extern void parse_ctc_target_CleanTable(uint8_t *buf));
|
||||
|
||||
#endif // CTC_TARGET_H
|
||||
|
||||
@@ -23,7 +23,7 @@
|
||||
#include <std.h>
|
||||
|
||||
#include "modules/muti/dcf/dcf.h"
|
||||
#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
//#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
#include "subsystems/datalink/telemetry.h"
|
||||
#include "subsystems/navigation/common_nav.h"
|
||||
#include "autopilot.h"
|
||||
@@ -137,12 +137,12 @@ void send_theta_to_nei(void)
|
||||
}
|
||||
}
|
||||
|
||||
void parseRegTable(void)
|
||||
void parseRegTable(uint8_t *buf)
|
||||
{
|
||||
uint8_t ac_id = DL_DCF_REG_TABLE_ac_id(dl_buffer);
|
||||
uint8_t ac_id = DL_DCF_REG_TABLE_ac_id(buf);
|
||||
if (ac_id == AC_ID) {
|
||||
uint8_t nei_id = DL_DCF_REG_TABLE_nei_id(dl_buffer);
|
||||
int16_t desired_sigma = DL_DCF_REG_TABLE_desired_sigma(dl_buffer);
|
||||
uint8_t nei_id = DL_DCF_REG_TABLE_nei_id(buf);
|
||||
int16_t desired_sigma = DL_DCF_REG_TABLE_desired_sigma(buf);
|
||||
|
||||
if (nei_id == 0) {
|
||||
for (int i = 0; i < DCF_MAX_NEIGHBORS; i++) {
|
||||
@@ -166,13 +166,13 @@ void parseRegTable(void)
|
||||
}
|
||||
}
|
||||
|
||||
void parseThetaTable(void)
|
||||
void parseThetaTable(uint8_t *buf)
|
||||
{
|
||||
int16_t sender_id = (int16_t)(SenderIdOfPprzMsg(dl_buffer));
|
||||
int16_t sender_id = (int16_t)(SenderIdOfPprzMsg(buf));
|
||||
for (int i = 0; i < DCF_MAX_NEIGHBORS; i++)
|
||||
if (dcf_tables.tableNei[i][0] == sender_id) {
|
||||
dcf_tables.last_theta[i] = get_sys_time_msec();
|
||||
dcf_tables.tableNei[i][1] = (int16_t)((DL_DCF_THETA_theta(dl_buffer)) * 1800 / M_PI);
|
||||
dcf_tables.tableNei[i][1] = (int16_t)((DL_DCF_THETA_theta(buf)) * 1800 / M_PI);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -56,7 +56,7 @@ extern void dcf_init(void);
|
||||
extern bool distributed_circular(uint8_t wp);
|
||||
extern void send_theta_to_nei(void);
|
||||
|
||||
extern void parseRegTable(void);
|
||||
extern void parseThetaTable(void);
|
||||
extern void parseRegTable(uint8_t *buf);
|
||||
extern void parseThetaTable(uint8_t *buf);
|
||||
|
||||
#endif // DCF_H
|
||||
|
||||
@@ -20,7 +20,7 @@
|
||||
*/
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "subsystems/abi.h"
|
||||
#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
//#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
#include "autopilot.h"
|
||||
#include "modules/multi/fc_rotor/fc_rotor.h"
|
||||
#include "firmwares/rotorcraft/navigation.h"
|
||||
@@ -29,18 +29,18 @@ void fc_rotor_init(void)
|
||||
{
|
||||
}
|
||||
|
||||
void fc_read_msg(void)
|
||||
void fc_read_msg(uint8_t *buf)
|
||||
{
|
||||
struct FloatVect3 u;
|
||||
uint8_t ac_id = DL_DESIRED_SETPOINT_ac_id(dl_buffer);
|
||||
uint8_t ac_id = DL_DESIRED_SETPOINT_ac_id();
|
||||
|
||||
if (ac_id == AC_ID) {
|
||||
// 0: 2D control, 1: 3D control
|
||||
uint8_t flag = DL_DESIRED_SETPOINT_flag(dl_buffer);
|
||||
uint8_t flag = DL_DESIRED_SETPOINT_flag(buf);
|
||||
|
||||
u.x = DL_DESIRED_SETPOINT_ux(dl_buffer);
|
||||
u.y = DL_DESIRED_SETPOINT_uy(dl_buffer);
|
||||
u.z = DL_DESIRED_SETPOINT_uz(dl_buffer);
|
||||
u.x = DL_DESIRED_SETPOINT_ux(buf);
|
||||
u.y = DL_DESIRED_SETPOINT_uy(buf);
|
||||
u.z = DL_DESIRED_SETPOINT_uz(buf);
|
||||
|
||||
AbiSendMsgACCEL_SP(ACCEL_SP_FCR_ID, flag, &u);
|
||||
|
||||
|
||||
@@ -30,6 +30,6 @@
|
||||
#define FC_ROTOR_H
|
||||
|
||||
extern void fc_rotor_init(void);
|
||||
extern void fc_read_msg(void);
|
||||
extern void fc_read_msg(uint8_t *buf);
|
||||
|
||||
#endif // FC_ROTOR_H
|
||||
|
||||
@@ -7,7 +7,7 @@
|
||||
#ifndef FORMATION_H
|
||||
#define FORMATION_H
|
||||
|
||||
#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
//#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
#include "generated/airframe.h" // AC_ID
|
||||
#include "modules/multi/traffic_info.h"
|
||||
|
||||
@@ -43,23 +43,23 @@ static inline void updateSlot(uint8_t id, float se, float sn, float sa)
|
||||
|
||||
static inline void updateFormationStatus(uint8_t id, uint8_t status) { formation[ti_acs_id[id]].status = status; }
|
||||
|
||||
static inline void parseFormationStatus(void)
|
||||
static inline void parseFormationStatus(uint8_t *buff)
|
||||
{
|
||||
uint8_t ac_id = DL_FORMATION_STATUS_ac_id(dl_buffer);
|
||||
uint8_t leader = DL_FORMATION_STATUS_leader_id(dl_buffer);
|
||||
uint8_t status = DL_FORMATION_STATUS_status(dl_buffer);
|
||||
uint8_t ac_id = DL_FORMATION_STATUS_ac_id(buf);
|
||||
uint8_t leader = DL_FORMATION_STATUS_leader_id(buf);
|
||||
uint8_t status = DL_FORMATION_STATUS_status(buf);
|
||||
if (ac_id == AC_ID) { leader_id = leader; }
|
||||
else if (leader == leader_id) { updateFormationStatus(ac_id, status); }
|
||||
else { updateFormationStatus(ac_id, UNSET); }
|
||||
}
|
||||
|
||||
static inline void parseFormationSlot(void)
|
||||
static inline void parseFormationSlot(uint8_t *buf)
|
||||
{
|
||||
uint8_t ac_id = DL_FORMATION_SLOT_ac_id(dl_buffer);
|
||||
uint8_t mode = DL_FORMATION_SLOT_mode(dl_buffer);
|
||||
float slot_east = DL_FORMATION_SLOT_slot_east(dl_buffer);
|
||||
float slot_north = DL_FORMATION_SLOT_slot_north(dl_buffer);
|
||||
float slot_alt = DL_FORMATION_SLOT_slot_alt(dl_buffer);
|
||||
uint8_t ac_id = DL_FORMATION_SLOT_ac_id(buf);
|
||||
uint8_t mode = DL_FORMATION_SLOT_mode(buf);
|
||||
float slot_east = DL_FORMATION_SLOT_slot_east(buf);
|
||||
float slot_north = DL_FORMATION_SLOT_slot_north(buf);
|
||||
float slot_alt = DL_FORMATION_SLOT_slot_alt(buf);
|
||||
updateSlot(ac_id, slot_east, slot_north, slot_alt);
|
||||
if (ac_id == leader_id) { form_mode = mode; }
|
||||
}
|
||||
|
||||
@@ -62,15 +62,15 @@ void rssi_init()
|
||||
AbiBindMsgRSSI(ABI_BROADCAST, &ev, rssi_cb);
|
||||
}
|
||||
|
||||
void parse_rssi_dl(void)
|
||||
void parse_rssi_dl(uint8_t *buf)
|
||||
{
|
||||
uint8_t sender_id = SenderIdOfPprzMsg(dl_buffer);
|
||||
uint8_t msg_id = IdOfPprzMsg(dl_buffer);
|
||||
uint8_t sender_id = SenderIdOfPprzMsg(buf);
|
||||
uint8_t msg_id = IdOfPprzMsg(buf);
|
||||
|
||||
if (sender_id > 0 && msg_id == DL_RSSI) {
|
||||
set_rssi(sender_id,
|
||||
DL_RSSI_tx_power(dl_buffer),
|
||||
DL_RSSI_rssi(dl_buffer));
|
||||
DL_RSSI_tx_power(buf),
|
||||
DL_RSSI_rssi(buf));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -42,6 +42,6 @@ extern void rssi_init(void);
|
||||
extern void set_rssi(uint8_t _ac_id, int8_t _rssi, int8_t _tx_strength);
|
||||
extern struct rssi_info_ get_rssi(uint8_t _ac_id);
|
||||
|
||||
extern void parse_rssi_dl(void);
|
||||
extern void parse_rssi_dl(uint8_t *buf);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -84,19 +84,19 @@ void tcas_init(void)
|
||||
}
|
||||
}
|
||||
|
||||
void parseTcasResolve(void)
|
||||
void parseTcasResolve(uint8_t *buf)
|
||||
{
|
||||
if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID) {
|
||||
uint8_t ac_id_conflict = DL_TCAS_RESOLVE_ac_id_conflict(dl_buffer);
|
||||
tcas_acs_status[ti_acs_id[ac_id_conflict]].resolve = DL_TCAS_RESOLVE_resolve(dl_buffer);
|
||||
if (DL_TCAS_RESOLVE_ac_id(buf) == AC_ID) {
|
||||
uint8_t ac_id_conflict = DL_TCAS_RESOLVE_ac_id_conflict(buf);
|
||||
tcas_acs_status[ti_acs_id[ac_id_conflict]].resolve = DL_TCAS_RESOLVE_resolve(buf);
|
||||
}
|
||||
}
|
||||
|
||||
void parseTcasRA(void)
|
||||
void parseTcasRA(uint8_t *buf)
|
||||
{
|
||||
if (DL_TCAS_RA_ac_id(dl_buffer) == AC_ID && SenderIdOfPprzMsg(dl_buffer) != AC_ID) {
|
||||
if (DL_TCAS_RA_ac_id(buf) == AC_ID && SenderIdOfPprzMsg(buf) != AC_ID) {
|
||||
uint8_t ac_id_conflict = SenderIdOfPprzMsg(dl_buffer);
|
||||
tcas_acs_status[ti_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer);
|
||||
tcas_acs_status[ti_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(buf);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -29,7 +29,7 @@
|
||||
#define TCAS_H
|
||||
|
||||
#include "std.h"
|
||||
#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
//#include "subsystems/datalink/datalink.h" // dl_buffer
|
||||
#include "pprzlink/messages.h" // TCAS_RA
|
||||
#include "generated/airframe.h" // AC_INFO
|
||||
#include "modules/multi/traffic_info.h"
|
||||
@@ -59,7 +59,7 @@ extern void tcas_periodic_task_4Hz(void);
|
||||
|
||||
extern void callTCAS(void);
|
||||
|
||||
extern void parseTcasResolve(void);
|
||||
extern void parseTcasRA(void);
|
||||
extern void parseTcasResolve(uint8_t *buf);
|
||||
extern void parseTcasRA(uint8_t *buf);
|
||||
|
||||
#endif /* TCAS_H */
|
||||
|
||||
@@ -73,20 +73,20 @@ static void update_geoid_height(void) {
|
||||
} /* default is just keep last available height */
|
||||
}
|
||||
|
||||
bool parse_acinfo_dl(void)
|
||||
bool parse_acinfo_dl(uint8_t *buf)
|
||||
{
|
||||
uint8_t sender_id = SenderIdOfPprzMsg(dl_buffer);
|
||||
uint8_t msg_id = IdOfPprzMsg(dl_buffer);
|
||||
uint8_t sender_id = SenderIdOfPprzMsg(buf);
|
||||
uint8_t msg_id = IdOfPprzMsg(buf);
|
||||
|
||||
/* handle telemetry message */
|
||||
#if PPRZLINK_DEFAULT_VER == 2
|
||||
if (pprzlink_get_msg_class_id(dl_buffer) == DL_telemetry_CLASS_ID) {
|
||||
if (pprzlink_get_msg_class_id(buf) == DL_telemetry_CLASS_ID) {
|
||||
#else
|
||||
if (sender_id > 0) {
|
||||
#endif
|
||||
switch (msg_id) {
|
||||
case DL_GPS_SMALL: {
|
||||
uint32_t multiplex_speed = DL_GPS_SMALL_multiplex_speed(dl_buffer);
|
||||
uint32_t multiplex_speed = DL_GPS_SMALL_multiplex_speed(buf);
|
||||
|
||||
// decode compressed values
|
||||
int16_t course = (int16_t)((multiplex_speed >> 21) & 0x7FF); // bits 31-21 course in decideg
|
||||
@@ -104,9 +104,9 @@ bool parse_acinfo_dl(void)
|
||||
}
|
||||
|
||||
set_ac_info_lla(sender_id,
|
||||
DL_GPS_SMALL_lat(dl_buffer),
|
||||
DL_GPS_SMALL_lon(dl_buffer),
|
||||
(int32_t)DL_GPS_SMALL_alt(dl_buffer) * 10,
|
||||
DL_GPS_SMALL_lat(buf),
|
||||
DL_GPS_SMALL_lon(buf),
|
||||
(int32_t)DL_GPS_SMALL_alt(buf) * 10,
|
||||
course,
|
||||
gspeed,
|
||||
climb,
|
||||
@@ -115,25 +115,25 @@ bool parse_acinfo_dl(void)
|
||||
break;
|
||||
case DL_GPS: {
|
||||
set_ac_info_utm(sender_id,
|
||||
DL_GPS_utm_east(dl_buffer),
|
||||
DL_GPS_utm_north(dl_buffer),
|
||||
DL_GPS_alt(dl_buffer),
|
||||
DL_GPS_utm_zone(dl_buffer),
|
||||
DL_GPS_course(dl_buffer),
|
||||
DL_GPS_speed(dl_buffer),
|
||||
DL_GPS_climb(dl_buffer),
|
||||
DL_GPS_itow(dl_buffer));
|
||||
DL_GPS_utm_east(buf),
|
||||
DL_GPS_utm_north(buf),
|
||||
DL_GPS_alt(buf),
|
||||
DL_GPS_utm_zone(buf),
|
||||
DL_GPS_course(buf),
|
||||
DL_GPS_speed(buf),
|
||||
DL_GPS_climb(buf),
|
||||
DL_GPS_itow(buf));
|
||||
}
|
||||
break;
|
||||
case DL_GPS_LLA: {
|
||||
set_ac_info_lla(sender_id,
|
||||
DL_GPS_LLA_lat(dl_buffer),
|
||||
DL_GPS_LLA_lon(dl_buffer),
|
||||
DL_GPS_LLA_alt(dl_buffer),
|
||||
DL_GPS_LLA_course(dl_buffer),
|
||||
DL_GPS_LLA_speed(dl_buffer),
|
||||
DL_GPS_LLA_climb(dl_buffer),
|
||||
DL_GPS_LLA_itow(dl_buffer));
|
||||
DL_GPS_LLA_lat(buf),
|
||||
DL_GPS_LLA_lon(buf),
|
||||
DL_GPS_LLA_alt(buf),
|
||||
DL_GPS_LLA_course(buf),
|
||||
DL_GPS_LLA_speed(buf),
|
||||
DL_GPS_LLA_climb(buf),
|
||||
DL_GPS_LLA_itow(buf));
|
||||
}
|
||||
break;
|
||||
default:
|
||||
@@ -143,26 +143,26 @@ bool parse_acinfo_dl(void)
|
||||
} else {
|
||||
switch (msg_id) {
|
||||
case DL_ACINFO: {
|
||||
set_ac_info_utm(DL_ACINFO_ac_id(dl_buffer),
|
||||
DL_ACINFO_utm_east(dl_buffer),
|
||||
DL_ACINFO_utm_north(dl_buffer),
|
||||
DL_ACINFO_alt(dl_buffer) * 10,
|
||||
DL_ACINFO_utm_zone(dl_buffer),
|
||||
DL_ACINFO_course(dl_buffer),
|
||||
DL_ACINFO_speed(dl_buffer),
|
||||
DL_ACINFO_climb(dl_buffer),
|
||||
DL_ACINFO_itow(dl_buffer));
|
||||
set_ac_info_utm(DL_ACINFO_ac_id(buf),
|
||||
DL_ACINFO_utm_east(buf),
|
||||
DL_ACINFO_utm_north(buf),
|
||||
DL_ACINFO_alt(buf) * 10,
|
||||
DL_ACINFO_utm_zone(buf),
|
||||
DL_ACINFO_course(buf),
|
||||
DL_ACINFO_speed(buf),
|
||||
DL_ACINFO_climb(buf),
|
||||
DL_ACINFO_itow(buf));
|
||||
}
|
||||
break;
|
||||
case DL_ACINFO_LLA: {
|
||||
set_ac_info_lla(DL_ACINFO_LLA_ac_id(dl_buffer),
|
||||
DL_ACINFO_LLA_lat(dl_buffer),
|
||||
DL_ACINFO_LLA_lon(dl_buffer),
|
||||
DL_ACINFO_LLA_alt(dl_buffer) * 10,
|
||||
DL_ACINFO_LLA_course(dl_buffer),
|
||||
DL_ACINFO_LLA_speed(dl_buffer),
|
||||
DL_ACINFO_LLA_climb(dl_buffer),
|
||||
DL_ACINFO_LLA_itow(dl_buffer));
|
||||
set_ac_info_lla(DL_ACINFO_LLA_ac_id(buf),
|
||||
DL_ACINFO_LLA_lat(buf),
|
||||
DL_ACINFO_LLA_lon(buf),
|
||||
DL_ACINFO_LLA_alt(buf) * 10,
|
||||
DL_ACINFO_LLA_course(buf),
|
||||
DL_ACINFO_LLA_speed(buf),
|
||||
DL_ACINFO_LLA_climb(buf),
|
||||
DL_ACINFO_LLA_itow(buf));
|
||||
}
|
||||
break;
|
||||
default:
|
||||
|
||||
@@ -131,7 +131,7 @@ extern void traffic_info_init(void);
|
||||
* Telemetry (vehicle -> ground or vehicle -> vehicle): GPS_SMALL, GPS, GPS_LLA
|
||||
* Datalink (ground -> vehicle): ACINFO, ACINFO_LLA
|
||||
*/
|
||||
extern bool parse_acinfo_dl(void);
|
||||
extern bool parse_acinfo_dl(uint8_t *buf);
|
||||
|
||||
/************************ Set functions ****************************/
|
||||
|
||||
|
||||
@@ -228,45 +228,45 @@ static void parse_gps_datalink_local(float enu_x, float enu_y, float enu_z,
|
||||
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
|
||||
}
|
||||
|
||||
void gps_datalink_parse_REMOTE_GPS(void)
|
||||
void gps_datalink_parse_REMOTE_GPS(uint8_t *buf)
|
||||
{
|
||||
if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { return; } // not for this aircraft
|
||||
if (DL_REMOTE_GPS_ac_id(buf) != AC_ID) { return; } // not for this aircraft
|
||||
|
||||
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));
|
||||
parse_gps_datalink(DL_REMOTE_GPS_numsv(buf),
|
||||
DL_REMOTE_GPS_ecef_x(buf),
|
||||
DL_REMOTE_GPS_ecef_y(buf),
|
||||
DL_REMOTE_GPS_ecef_z(buf),
|
||||
DL_REMOTE_GPS_lat(buf),
|
||||
DL_REMOTE_GPS_lon(buf),
|
||||
DL_REMOTE_GPS_alt(buf),
|
||||
DL_REMOTE_GPS_hmsl(buf),
|
||||
DL_REMOTE_GPS_ecef_xd(buf),
|
||||
DL_REMOTE_GPS_ecef_yd(buf),
|
||||
DL_REMOTE_GPS_ecef_zd(buf),
|
||||
DL_REMOTE_GPS_tow(buf),
|
||||
DL_REMOTE_GPS_course(buf));
|
||||
}
|
||||
|
||||
void gps_datalink_parse_REMOTE_GPS_SMALL(void)
|
||||
void gps_datalink_parse_REMOTE_GPS_SMALL(uint8_t *buf)
|
||||
{
|
||||
if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { return; } // not for this aircraft
|
||||
if (DL_REMOTE_GPS_SMALL_ac_id(buf) != AC_ID) { return; } // not for this aircraft
|
||||
|
||||
parse_gps_datalink_small(DL_REMOTE_GPS_SMALL_heading(dl_buffer),
|
||||
DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer),
|
||||
DL_REMOTE_GPS_SMALL_speed_xyz(dl_buffer),
|
||||
DL_REMOTE_GPS_SMALL_tow(dl_buffer));
|
||||
parse_gps_datalink_small(DL_REMOTE_GPS_SMALL_heading(buf),
|
||||
DL_REMOTE_GPS_SMALL_pos_xyz(buf),
|
||||
DL_REMOTE_GPS_SMALL_speed_xyz(buf),
|
||||
DL_REMOTE_GPS_SMALL_tow(buf));
|
||||
}
|
||||
|
||||
void gps_datalink_parse_REMOTE_GPS_LOCAL(void)
|
||||
void gps_datalink_parse_REMOTE_GPS_LOCAL(uint8_t *buf)
|
||||
{
|
||||
if (DL_REMOTE_GPS_LOCAL_ac_id(dl_buffer) != AC_ID) { return; } // not for this aircraft
|
||||
if (DL_REMOTE_GPS_LOCAL_ac_id(buf) != AC_ID) { return; } // not for this aircraft
|
||||
|
||||
parse_gps_datalink_local(DL_REMOTE_GPS_LOCAL_enu_x(dl_buffer),
|
||||
DL_REMOTE_GPS_LOCAL_enu_y(dl_buffer),
|
||||
DL_REMOTE_GPS_LOCAL_enu_z(dl_buffer),
|
||||
DL_REMOTE_GPS_LOCAL_enu_xd(dl_buffer),
|
||||
DL_REMOTE_GPS_LOCAL_enu_yd(dl_buffer),
|
||||
DL_REMOTE_GPS_LOCAL_enu_zd(dl_buffer),
|
||||
DL_REMOTE_GPS_LOCAL_tow(dl_buffer),
|
||||
DL_REMOTE_GPS_LOCAL_course(dl_buffer));
|
||||
parse_gps_datalink_local(DL_REMOTE_GPS_LOCAL_enu_x(buf),
|
||||
DL_REMOTE_GPS_LOCAL_enu_y(buf),
|
||||
DL_REMOTE_GPS_LOCAL_enu_z(buf),
|
||||
DL_REMOTE_GPS_LOCAL_enu_xd(buf),
|
||||
DL_REMOTE_GPS_LOCAL_enu_yd(buf),
|
||||
DL_REMOTE_GPS_LOCAL_enu_zd(buf),
|
||||
DL_REMOTE_GPS_LOCAL_tow(buf),
|
||||
DL_REMOTE_GPS_LOCAL_course(buf));
|
||||
}
|
||||
|
||||
@@ -44,8 +44,8 @@ extern void gps_datalink_init(void);
|
||||
|
||||
#define gps_datalink_periodic_check() gps_periodic_check(&gps_datalink)
|
||||
|
||||
extern void gps_datalink_parse_REMOTE_GPS(void);
|
||||
extern void gps_datalink_parse_REMOTE_GPS_SMALL(void);
|
||||
extern void gps_datalink_parse_REMOTE_GPS_LOCAL(void);
|
||||
extern void gps_datalink_parse_REMOTE_GPS(uint8_t *buf);
|
||||
extern void gps_datalink_parse_REMOTE_GPS_SMALL(uint8_t *buf);
|
||||
extern void gps_datalink_parse_REMOTE_GPS_LOCAL(uint8_t *buf);
|
||||
|
||||
#endif /* GPS_DATALINK_H */
|
||||
|
||||
Reference in New Issue
Block a user