Merge pull request #5 from helijunky/master

fixed sending of MAV_ADSB_VEHICLE

main/gps.cpp

@@ -588,7 +588,7 @@ static void GPS_MAV(void)                                                  // wh
   uint64_t UnixTime_ms = MAV_getUnixTime();                                   // get the time from the MAVlink message
   if( (MsgID!=MAV_ID_SYSTEM_TIME) && UnixTime_ms)
   { if(Position[PosIdx].hasTime)
-    { uint64_t PrevUnixTime_ms = 1000*(uint64_t)Position[PosIdx].getUnixTime()+10*(uint32_t)Position[PosIdx].FracSec; // Position[PosIdx].getUnixTime_ms();
+    { uint64_t PrevUnixTime_ms = Position[PosIdx].getUnixTime_ms();
       int32_t TimeDiff_ms = UnixTime_ms-PrevUnixTime_ms;
 #ifdef DEBUG_PRINT
     xSemaphoreTake(CONS_Mutex, portMAX_DELAY);

main/ogn.h

@@ -985,7 +985,8 @@ class GPS_Position
      return TimeDiff; } // [0.01s]
 
    void Write(MAV_GPS_RAW_INT *MAV) const
-   { MAV->time_usec = (int64_t)1000000*getUnixTime()+10000*FracSec;
+//   { MAV->time_usec = (int64_t)1000000*getUnixTime()+10000*FracSec;
+   { MAV->time_usec = getUnixTime_ms()*1000; // (int64_t)1000000*getUnixTime()+10000*FracSec;
      MAV->lat = ((int64_t)50*Latitude+1)/3;
      MAV->lon = ((int64_t)50*Longitude+1)/3;
      MAV->alt = 100*Altitude;
@@ -1318,6 +1319,10 @@ class GPS_Position
     setUnixTime(Time);
     FracSec = (Time_ms-(uint64_t)Time*1000)/10; }
 
+  uint64_t getUnixTime_ms(void) const
+  { return (uint64_t)getUnixTime()*1000 + (uint32_t)FracSec*10; }
+
+
   private:
 
    static const uint32_t SecsPerMin =       60;

main/ogn1.h

@@ -227,7 +227,7 @@ class OGN1_Packet          // Packet structure for the OGN tracker
      printf("\n");
    }
 
-   void Encode(MAV_ADSB_VEHICLE *MAV)
+/*   void Encode(MAV_ADSB_VEHICLE *MAV)
    { MAV->ICAO_address = HeaderWord&0x03FFFFFF;
      MAV->lat      = ((int64_t)50*DecodeLatitude()+1)/3;
      MAV->lon      = ((int64_t)50*DecodeLongitude()+1)/3;
@@ -240,6 +240,32 @@ class OGN1_Packet          // Packet structure for the OGN tracker
      MAV->callsign[0]   =    0;
      MAV->tslc          =    0;
      MAV->emiter_type   =    0; }
+*/
+
+   void Encode(MAV_ADSB_VEHICLE *MAV)
+   { MAV->ICAO_address = Header.Address;
+     MAV->lat           = ((int64_t)50*DecodeLatitude()+1)/3;          // convert coordinates to [1e-7deg]
+     MAV->lon           = ((int64_t)50*DecodeLongitude()+1)/3;
+     MAV->altitude      = 1000*DecodeAltitude();                       // convert to [mm[
+     MAV->heading       = 10*DecodeHeading();                          // [cdeg/s]
+     MAV->hor_velocity  = 10*DecodeSpeed();                            // [cm/s]
+     MAV->ver_velocity  = 10*DecodeClimbRate();                        // [cm/s]
+     MAV->flags         = 0x1F;                                        // all valid except for Squawk, not simulated
+     MAV->altitude_type =    1;                                        // GPS altitude
+     const static char Prefix[4] = { 'R', 'I', 'F', 'O' };             // prefix for Random, ICAO, Flarm and OGN address-types
+     MAV->callsign[0]   =    Prefix[Header.AddrType];                  // create a call-sign from address-type and address
+     Format_Hex((char *)MAV->callsign+1, ( uint8_t)(Header.Address>>16));      // highest byte
+     Format_Hex((char *)MAV->callsign+3, (uint16_t)(Header.Address&0xFFFF));   // two lower bytes
+     MAV->callsign[7]   =    0;                                        // end-of-string for call-sign
+     MAV->squawk        =    0;                                        // what shall we put there for OGN ?
+     MAV->tslc          =    1;                                        // 1sec for now but should be more precise
+     const static uint8_t EmitterType[16] =                            // conversion table from OGN aircraft-type
+     { 0,  9,  2,  7,     // unknown,          glider,           towplane,       helicopter
+      11,  3,  9, 11,     // parachute,        drop plane,       hang-glider,    para-glider
+       2,  3, 15, 10,     // powered aircraft, jet aircraft,     UFO,            balloon
+      10, 14,  2, 19  };  // airship,          UAV,              ground vehiele, fixed object
+     MAV->emiter_type   =    EmitterType[Position.AcftType];           // convert from the OGN
+   }
 
    static const char *getAprsIcon(uint8_t AcftType)
    { static const char *AprsIcon[16] = // Icons for various FLARM acftType's

main/proc.cpp

@@ -284,9 +284,13 @@ static void ProcessRxPacket(OGN_RxPacket<OGN_Packet> *RxPacket, uint8_t RxPacket
    MAV_ADSB_VEHICLE MAV_RxReport;
    RxPacket->Packet.Encode(&MAV_RxReport);
    MAV_RxMsg::Send(sizeof(MAV_RxReport), MAV_Seq++, MAV_SysID, MAV_COMP_ID_ADSB, MAV_ID_ADSB_VEHICLE, (const uint8_t *)&MAV_RxReport, GPS_UART_Write);
-   // xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
+#ifdef DEBUG_PRINT
-   // MAV_RxMsg::Send(sizeof(MAV_RxReport), MAV_Seq++, MAV_SysID, MAV_COMP_ID_ADSB, MAV_ID_ADSB_VEHICLE, (const uint8_t *)&MAV_RxReport, CONS_UART_Write);
+      xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
-   // xSemaphoreGive(CONS_Mutex);
+      Format_String(CONS_UART_Write, "MAV_ADSB_VEHICLE sent. ID=");
+      Format_Hex(CONS_UART_Write, MAV_RxReport.ICAO_address);
+      CONS_UART_Write('\r'); CONS_UART_Write('\n');
+      xSemaphoreGive(CONS_Mutex);
+#endif
 #endif
   }
 }
@@ -435,6 +439,24 @@ void vTaskPROC(void* pvParameters)
       if( (AverSpeed>10) || ((RX_Random&0x3)==0) )                      // send only some positions if the speed is less than 1m/s
         RF_TxFIFO.Write();                                              // complete the write into the TxFIFO
       Position->Sent=1;
+// #ifdef WITH_MAVLINK
+//     { MAV_HEARTBEAT MAV_HeartBeat;
+//     // = { custom_mode:0,
+//     //     type:0,
+//     //     autopilot:0,
+//     //     base_mode:0,
+//     //     system_status:4,
+//     //     mavlink_version:1
+//     //   };
+//       MAV_HeartBeat.custom_mode=0;
+//       MAV_HeartBeat.type=0;
+//       MAV_HeartBeat.autopilot=0;
+//       MAV_HeartBeat.base_mode=0;
+//       MAV_HeartBeat.system_status=4;
+//       MAV_HeartBeat.mavlink_version=1;
+//       MAV_RxMsg::Send(sizeof(MAV_HeartBeat), MAV_Seq++, MAV_SysID, MAV_COMP_ID_ADSB, MAV_ID_HEARTBEAT, (const uint8_t *)&MAV_HeartBeat, GPS_UART_Write);
+//     }
+// #endif
 #ifdef WITH_LOOKOUT
       const LookOut_Target *Tgt=Look.ProcessOwn(PosPacket.Packet);         // process own position, get the most dangerous target
 #ifdef WITH_PFLAA