void parse_rssi_dl(void)
{
uint8_t sender_id = SenderIdOfPprzMsg(dl_buffer);
uint8_t msg_id = IdOfPprzMsg(dl_buffer);
if (sender_id > 0 && msg_id == DL_RSSI) {
set_rssi(sender_id,
DL_RSSI_tx_power(dl_buffer),
DL_RSSI_rssi(dl_buffer));
}
}
#include "subsystems/datalink/datalink.h"
#include "pprzlink/dl_protocol.h"
#ifdef USE_NAVIGATION
#include "firmwares/rotorcraft/navigation.h"
#include "math/pprz_geodetic_int.h"
#endif
#include "firmwares/rotorcraft/autopilot.h"
#include "firmwares/rotorcraft/autopilot_guided.h"
void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
uint8_t msg_id = IdOfPprzMsg(buf);
/* parse telemetry messages coming from ground station */
switch (msg_id) {
#ifdef USE_NAVIGATION
case DL_BLOCK : {
if (DL_BLOCK_ac_id(buf) != AC_ID) { break; }
nav_goto_block(DL_BLOCK_block_id(buf));
}
break;
case DL_MOVE_WP : {
uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
if (ac_id != AC_ID) { break; }
if (stateIsLocalCoordinateValid()) {
void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
{
uint8_t sender_id = SenderIdOfPprzMsg(buf);
uint8_t msg_id = IdOfPprzMsg(buf);
/* parse telemetry messages coming from other AC */
if (sender_id != 0) {
switch (msg_id) {
default: {
break;
}
}
} else {
/* parse telemetry messages coming from ground station */
switch (msg_id) {
case DL_PING: {
pprz_msg_send_PONG(trans, dev, AC_ID);
}
break;
case DL_SETTING : {
if (DL_SETTING_ac_id(buf) != AC_ID) { break; }
uint8_t i = DL_SETTING_index(buf);
float var = DL_SETTING_value(buf);
DlSetting(i, var);
pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &var);
}
break;
case DL_GET_SETTING : {
if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
uint8_t i = DL_GET_SETTING_index(buf);
float val = settings_get_value(i);
pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &val);
}
break;
#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(buf),
DL_RC_3CH_roll(buf),
DL_RC_3CH_pitch(buf));
break;
case DL_RC_4CH :
if (DL_RC_4CH_ac_id(buf) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(DL_RC_4CH_mode(buf),
DL_RC_4CH_throttle(buf),
DL_RC_4CH_roll(buf),
DL_RC_4CH_pitch(buf),
DL_RC_4CH_yaw(buf));
}
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
#if USE_GPS
case DL_GPS_INJECT : {
// Check if the GPS is for this AC
if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { break; }
// GPS parse data
gps_inject_data(
DL_GPS_INJECT_packet_id(buf),
DL_GPS_INJECT_data_length(buf),
DL_GPS_INJECT_data(buf)
);
}
break;
#endif // USE_GPS
default:
break;
}
}
/* Parse firmware specific datalink */
firmware_parse_msg(dev, trans, buf);
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
int parse_acinfo_dl()
{
uint8_t sender_id = SenderIdOfPprzMsg(dl_buffer);
uint8_t msg_id = IdOfPprzMsg(dl_buffer);
if (sender_id > 0) {
switch (msg_id) {
case DL_GPS_SMALL: {
uint32_t multiplex_speed = DL_GPS_SMALL_multiplex_speed(dl_buffer);
// decode compressed values
int16_t course = (int16_t)((multiplex_speed >> 21) & 0x7FF); // bits 31-21 course in decideg
if (course & 0x400) {
course |= 0xF800; // fix for twos complements
}
course *= 2; // scale course by resolution
int16_t gspeed = (int16_t)((multiplex_speed >> 10) & 0x7FF); // bits 20-10 ground speed cm/s
if (gspeed & 0x400) {
gspeed |= 0xF800; // fix for twos complements
}
int16_t climb = (int16_t)(multiplex_speed & 0x3FF); // bits 9-0 z climb speed in cm/s
if (climb & 0x200) {
climb |= 0xFC00; // fix for twos complements
}
set_ac_info_lla(sender_id,
DL_GPS_SMALL_lla_lat(dl_buffer),
DL_GPS_SMALL_lla_lon(dl_buffer),
(int32_t)DL_GPS_SMALL_alt(dl_buffer) * 10,
course,
gspeed,
climb,
gps_tow_from_sys_ticks(sys_time.nb_tick));
}
break;
case DL_GPS: {
set_ac_info(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));
}
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));
}
break;
default:
return 0;
}
} else {
switch (msg_id) {
