From 018f34ec6fdaddde9940d0cd52bdb783d2ebeb92 Mon Sep 17 00:00:00 2001 From: Manuel Haag Date: Thu, 9 May 2019 23:59:55 +0200 Subject: [PATCH] fixed sending of MAV_ADSB_VEHICLE --- main/gps.cpp | 2 +- main/ogn.h | 7 ++++++- main/ogn1.h | 28 +++++++++++++++++++++++++++- main/proc.cpp | 28 +++++++++++++++++++++++++--- 4 files changed, 59 insertions(+), 6 deletions(-) diff --git a/main/gps.cpp b/main/gps.cpp index 3a7ad9c..77f1911 100644 --- a/main/gps.cpp +++ b/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); diff --git a/main/ogn.h b/main/ogn.h index 3c40470..9096a4e 100644 --- a/main/ogn.h +++ b/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; diff --git a/main/ogn1.h b/main/ogn1.h index 7bff608..1b372b3 100644 --- a/main/ogn1.h +++ b/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 diff --git a/main/proc.cpp b/main/proc.cpp index a0730a1..eeb004c 100644 --- a/main/proc.cpp +++ b/main/proc.cpp @@ -284,9 +284,13 @@ static void ProcessRxPacket(OGN_RxPacket *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); - // 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); - // xSemaphoreGive(CONS_Mutex); +#ifdef DEBUG_PRINT + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + 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