Anticipated FANET protocol update.

lyusupov · May 30, 2020 · 5ce5c38 · 5ce5c38
1 parent 4e80d30
commit 5ce5c38
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 6 deletions.
diff --git a/software/firmware/source/SoftRF/Protocol_FANET.cpp b/software/firmware/source/SoftRF/Protocol_FANET.cpp
@@ -205,8 +205,8 @@ bool fanet_decode(void *fanet_pkt, ufo_t *this_aircraft, ufo_t *fop) {
 
  fanet_packet_t *pkt = (fanet_packet_t *) fanet_pkt;
  unsigned int altitude;
- uint8_t speed_byte, climb_byte;
- int speed_int, climb_int;
+ uint8_t speed_byte, climb_byte, offset_byte;
+ int speed_int, climb_int, offset_int;
  bool rval = false;
 
  if (pkt->ext_header == 0 && pkt->type == 1 ) { /* Tracking */
@@ -254,6 +254,17 @@ bool fanet_decode(void *fanet_pkt, ufo_t *this_aircraft, ufo_t *fop) {
  }
  fop->vs = ((float)climb_int) * (_GPS_FEET_PER_METER * 6.0);
 
+#if defined(FANET_NEXT)
+ offset_byte = pkt->qne_offset;
+ offset_int = (int) (offset_byte | (offset_byte & (1<<6) ? 0xFFFFFF80U : 0));
+
+ if (pkt->qne_scale) {
+ offset_int *= 4;
+ }
+
+ fop->pressure_altitude = fop->altitude + (float) offset_int;
+#endif
+
  fop->addr_type = ADDR_TYPE_FANET;
  fop->timestamp = this_aircraft->timestamp;
 
@@ -298,6 +309,8 @@ size_t fanet_encode(void *fanet_pkt, ufo_t *this_aircraft) {
  float climb = this_aircraft->vs / (_GPS_FEET_PER_METER * 60.0);
  float heading = this_aircraft->course;
  float turnrate = 0;
+ int16_t alt_diff = this_aircraft->pressure_altitude == 0 ? 0 :
+ (int16_t) (this_aircraft->pressure_altitude - this_aircraft->altitude);
 
  fanet_packet_t *pkt = (fanet_packet_t *) fanet_pkt;
 
@@ -352,5 +365,16 @@ size_t fanet_encode(void *fanet_pkt, ufo_t *this_aircraft) {
  pkt->turn_rate = turnr4 & 0x7F;
  }
 
+#if defined(FANET_NEXT)
+ int16_t offset = constrain(alt_diff, -254, 254);
+ if(abs(offset) > 63) {
+ pkt->qne_scale = 1;
+ pkt->qne_offset = ((offset + (offset >= 0 ? 2 : -2)) / 4);
+ } else {
+ pkt->qne_scale = 0;
+ pkt->qne_offset = offset & 0x7F;
+ }
+#endif
+
  return sizeof(fanet_packet_t);
 }
diff --git a/software/firmware/source/SoftRF/Protocol_FANET.h b/software/firmware/source/SoftRF/Protocol_FANET.h
@@ -35,6 +35,8 @@
 
 #define SOFRF_FANET_VENDOR_ID 0x07
 
+//#define FANET_NEXT
+
 enum
 {
  FANET_AIRCRAFT_TYPE_OTHER,
@@ -87,6 +89,10 @@ typedef struct {
  unsigned int turn_rate :7;
  unsigned int turn_scale :1;
 
+#if defined(FANET_NEXT)
+ unsigned int qne_offset :7;
+ unsigned int qne_scale :1;
+#endif
 } __attribute__((packed)) fanet_packet_t;
 
 #define FANET_PAYLOAD_SIZE sizeof(fanet_packet_t)

diff --git a/software/firmware/source/SoftRF/RFHelper.h b/software/firmware/source/SoftRF/RFHelper.h
@@ -105,8 +105,4 @@ extern bool (*protocol_decode)(void *, ufo_t *, ufo_t *);
 
 extern int8_t RF_last_rssi;
 
-#if !defined(EXCLUDE_CC13XX)
-extern const rfchip_ops_t cc13xx_ops;
-#endif /* EXCLUDE_CC13XX */
-
 #endif /* RFHELPER_H */