diff --git a/RemoteIDModule/Makefile b/RemoteIDModule/Makefile index 989c8cd..9fe5de5 100644 --- a/RemoteIDModule/Makefile +++ b/RemoteIDModule/Makefile @@ -20,38 +20,6 @@ all: headers esp32s3dev esp32c3dev bluemark-db200 bluemark-db110 jw-tbd mro-rid esp32s3dev: CHIP=esp32s3 esp32s3dev: ArduRemoteID-ESP32S3_DEV.bin -esp32c3dev: CHIP=esp32c3 -esp32c3dev: ArduRemoteID-ESP32C3_DEV.bin - -bluemark-db200: CHIP=esp32c3 -bluemark-db200: ArduRemoteID-BLUEMARK_DB200.bin - -bluemark-db110: CHIP=esp32c3 -bluemark-db110: ArduRemoteID-BLUEMARK_DB110.bin - -jw-tbd: CHIP=esp32s3 -jw-tbd: ArduRemoteID-JW_TBD.bin - -jwrid-esp32s3: CHIP=esp32s3 -jwrid-esp32s3: ArduRemoteID-JWRID_ESP32S3.bin - -mro-rid: CHIP=esp32c3 -mro-rid: ArduRemoteID-MRO_RID.bin - -bluemark-db202: CHIP=esp32c3 -bluemark-db202: ArduRemoteID-BLUEMARK_DB202.bin - -bluemark-db210: CHIP=esp32s3 -bluemark-db210: ArduRemoteID-BLUEMARK_DB210.bin - -bluemark-db203: CHIP=esp32c3 -bluemark-db203: ArduRemoteID-BLUEMARK_DB203.bin - -holybro-RemoteID: CHIP=esp32c3 -holybro-RemoteID: ArduRemoteID-Holybro_RemoteID.bin - -CUAV-RID: CHIP=esp32s3 -CUAV-RID: ArduRemoteID-CUAV_RID.bin setup: @echo "Installing ESP32 support" diff --git a/RemoteIDModule/RemoteIDModule.ino b/RemoteIDModule/RemoteIDModule.ino index fd3bcd7..7158513 100644 --- a/RemoteIDModule/RemoteIDModule.ino +++ b/RemoteIDModule/RemoteIDModule.ino @@ -24,7 +24,9 @@ #include #include "efuse.h" #include "led.h" +#include "Serial_Comms.h" +#include #if AP_DRONECAN_ENABLED static DroneCAN dronecan; @@ -52,6 +54,32 @@ static WebInterface webif; static bool arm_check_ok = false; // goes true for LED arm check status static bool pfst_check_ok = false; +char incomingByte = 0; +String str; + +const byte numChars = 100; +char receivedChars[numChars]; +char transmitChars[numChars]; + +boolean newData = false; + +static boolean recvInProgress = false; +static byte ndx = 0; +char startMarker = '$'; +char endMarker = '\r'; +char rc; + +char* ext_NMEA_data; +char* ext_NMEA_data_sep; +char* NMEA_Lat; +char* NMEA_Lon; +char NMEA_NS, NMEA_EW; +double ext_gps_lat; +double ext_gps_lon; +float ext_gps_direction, ext_gps_speed_H, ext_gps_alt_geo, ext_gps_time; +//ArduinoNmeaParser parser(onRmcUpdate, onGgaUpdate); + + /* setup serial ports */ @@ -125,6 +153,102 @@ void setup() esp_ota_mark_app_valid_cancel_rollback(); } +float GpsToDecimalDegrees(const char* nmeaPos, char quadrant) +{ + float v= 0; + if(strlen(nmeaPos)>5) + { + char integerPart[3+1]; + int digitCount= (nmeaPos[4]=='.' ? 2 : 3); + memcpy(integerPart, nmeaPos, digitCount); + integerPart[digitCount]= 0; + nmeaPos+= digitCount; + v= atoi(integerPart) + atof(nmeaPos)/60.; + if(quadrant=='W' || quadrant=='S') + v= -v; + } + return v; +} + +void onRmcUpdate(nmea::RmcData const); +void onGgaUpdate(nmea::GgaData const); + +ArduinoNmeaParser parser(onRmcUpdate, onGgaUpdate); + +void onRmcUpdate(nmea::RmcData const rmc) +{ + Serial.print("RMC "); + + if (rmc.source == nmea::RmcSource::GPS) Serial.print("GPS"); + else if (rmc.source == nmea::RmcSource::GLONASS) Serial.print("GLONASS"); + else if (rmc.source == nmea::RmcSource::Galileo) Serial.print("Galileo"); + else if (rmc.source == nmea::RmcSource::GNSS) Serial.print("GNSS"); + else if (rmc.source == nmea::RmcSource::BDS) Serial.print("BDS"); + + Serial.print(" "); + Serial.print(rmc.time_utc.hour); + Serial.print(":"); + Serial.print(rmc.time_utc.minute); + Serial.print(":"); + Serial.print(rmc.time_utc.second); + Serial.print("."); + Serial.print(rmc.time_utc.microsecond); + + if (rmc.is_valid) + { + Serial.print(" : LON "); + Serial.print(rmc.longitude); + Serial.print(" ° | LAT "); + Serial.print(rmc.latitude); + Serial.print(" ° | VEL "); + Serial.print(rmc.speed); + Serial.print(" m/s | HEADING "); + Serial.print(rmc.course); + Serial.print(" °"); + } + + Serial.println(); +} + +void onGgaUpdate(nmea::GgaData const gga) +{ + Serial.print("GGA "); + + if (gga.source == nmea::GgaSource::GPS) Serial.print("GPS"); + else if (gga.source == nmea::GgaSource::GLONASS) Serial.print("GLONASS"); + else if (gga.source == nmea::GgaSource::Galileo) Serial.print("Galileo"); + else if (gga.source == nmea::GgaSource::GNSS) Serial.print("GNSS"); + else if (gga.source == nmea::GgaSource::BDS) Serial.print("BDS"); + + Serial.print(" "); + Serial.print(gga.time_utc.hour); + Serial.print(":"); + Serial.print(gga.time_utc.minute); + Serial.print(":"); + Serial.print(gga.time_utc.second); + Serial.print("."); + Serial.print(gga.time_utc.microsecond); + + if (gga.fix_quality != nmea::FixQuality::Invalid) + { + Serial.print(" : LON "); + Serial.print(gga.longitude); + Serial.print(" ° | LAT "); + Serial.print(gga.latitude); + Serial.print(" ° | Num Sat. "); + Serial.print(gga.num_satellites); + Serial.print(" | HDOP = "); + Serial.print(gga.hdop); + Serial.print(" m | Altitude "); + Serial.print(gga.altitude); + Serial.print(" m | Geoidal Separation "); + Serial.print(gga.geoidal_separation); + Serial.print(" m"); + } + + Serial.println(); +} + #define IMIN(x,y) ((x)<(y)?(x):(y)) #define ODID_COPY_STR(to, from) strncpy(to, (const char*)from, IMIN(sizeof(to), sizeof(from))) @@ -323,6 +447,12 @@ static void set_data(Transport &t) UAS_data.Location.TimeStamp = location.timestamp; UAS_data.LocationValid = 1; } + //Serial.print(ext_gps_lat); + UAS_data.Location.Latitude = ext_gps_lat; + UAS_data.Location.Longitude = ext_gps_lon; + //UAS_data.Location.Direction = ext_gps_direction; + UAS_data.Location.SpeedHorizontal = ext_gps_speed_H; + UAS_data.LocationValid = 1; const char *reason = check_parse(); t.arm_status_check(reason); @@ -441,7 +571,102 @@ void loop() last_update_bt4_ms = now_ms; ble.transmit_legacy(UAS_data); } +/*incomingByte = Serial.read(); +if (Serial.available() > 0) { + str = Serial.readStringUntil(13); + str.trim(); + if(str.startsWith("$GPR")){ + Serial.println(str); + }*/ + //parser.encode((char)Serial.read()); + //onGgaUpdate(); + //void onRmcUpdate(nmea::RmcData const rmc) + // { + // Serial.print(nmea::RmcSource); + // } + //} + + while (Serial.available() > 0 ) { + rc = Serial.read(); + if (recvInProgress == true) { + if (rc != endMarker) { + receivedChars[ndx] = rc; + ndx++; + if (ndx >= numChars) { + ndx = numChars - 1; + } + } else { + receivedChars[ndx] = '\0'; // terminate the string + recvInProgress = false; + ndx = 0; + newData = true; + //Serial.print('.'); + //break; + } + } else if (rc == startMarker) { + recvInProgress = true; + } + //} + if (newData == true) { + if(receivedChars[4] == 'C'){ + strcpy(transmitChars, receivedChars); + int N = 0; + ext_NMEA_data = strdup(transmitChars); + //ext_NMEA_data = strtok(transmitChars, ",");//old method + //ext_NMEA_data = strsep(transmitChars, ","); + //while (ext_NMEA_data != NULL) { //Old method + while((ext_NMEA_data_sep = strsep(&ext_NMEA_data, ",")) != NULL) { + if(N == 1){ + ext_gps_time = atof(ext_NMEA_data_sep); + //Serial.println(ext_gps_time); + //Serial.print(ext_NMEA_data_sep); + } + if(N == 3){ + NMEA_Lat = ext_NMEA_data_sep; + //Serial.println(NMEA_Lat); + } + else if (N == 4){ + NMEA_NS = *ext_NMEA_data_sep; + //Serial.println(NMEA_NS); + } + + else if (N == 5){ + NMEA_Lon = ext_NMEA_data_sep; + //Serial.println(NMEA_Lon); + } + else if (N == 6){ + NMEA_EW = *ext_NMEA_data_sep; + //Serial.println(NMEA_EW); + } + else if (N == 7){ + ext_gps_speed_H = atof(ext_NMEA_data_sep); + //Serial.println(ext_gps_speed_H); + } + else if (N == 8){ + ext_gps_direction = atof(ext_NMEA_data_sep); + //Serial.println(ext_gps_direction); + } + //ext_NMEA_data = strtok(NULL, ","); + N++; + } + //Serial.print(GpsToDecimalDegrees(NMEA_Lat, NMEA_NS), 5); + ext_gps_lat = GpsToDecimalDegrees(NMEA_Lat, NMEA_NS); //needed + ext_gps_lon = GpsToDecimalDegrees(NMEA_Lon, NMEA_EW); //needed + } + newData = false; + } + +} + +if(Serial1.read() == 2){ +Send_RemoteID_Serial(UAS_data); +} +//struct ODID_Location_data *LocationPointer; +//LocationPointer = &UAS_data.Location; +//printLocation_data(LocationPointer); + + // sleep for a bit for power saving - delay(1); + //delay(1); } diff --git a/RemoteIDModule/Serial_Comms.cpp b/RemoteIDModule/Serial_Comms.cpp new file mode 100644 index 0000000..b132fbd --- /dev/null +++ b/RemoteIDModule/Serial_Comms.cpp @@ -0,0 +1,30 @@ +#include +#include + + +struct ODID_BasicID_data *BasicIDPointer; +struct ODID_Location_data *LocationPointer; +struct ODID_Auth_data *AuthPointer; +struct ODID_SelfID_data *SelfIDPointer; +struct ODID_System_data *SystemPointer; +struct ODID_OperatorID_data *OperatorIDPointer; + +void Send_RemoteID_Serial(ODID_UAS_Data UAS_data) +{ + //Serial.print(UAS_data.Location.Latitude, 6); + //Serial.print(0x55); + //struct ODID_Location_data *LocationPointer; + BasicIDPointer = &UAS_data.BasicID[0]; + LocationPointer = &UAS_data.Location; + //AuthPointer = &UAS_data.Auth[0]; + SelfIDPointer = &UAS_data.SelfID; + SystemPointer = &UAS_data.System; + OperatorIDPointer = &UAS_data.OperatorID; + + printBasicID_data(BasicIDPointer); + printLocation_data(LocationPointer); + printSelfID_data(SelfIDPointer); + printSystem_data(SystemPointer); + printOperatorID_data(OperatorIDPointer); + +} diff --git a/RemoteIDModule/Serial_Comms.h b/RemoteIDModule/Serial_Comms.h new file mode 100644 index 0000000..3a5c5f2 --- /dev/null +++ b/RemoteIDModule/Serial_Comms.h @@ -0,0 +1,3 @@ + + +void Send_RemoteID_Serial(ODID_UAS_Data UAS_data); diff --git a/RemoteIDModule/parameters.cpp b/RemoteIDModule/parameters.cpp index fbeb3a0..fac1c54 100644 --- a/RemoteIDModule/parameters.cpp +++ b/RemoteIDModule/parameters.cpp @@ -18,7 +18,7 @@ const Parameters::Param Parameters::params[] = { { "UAS_TYPE_2", Parameters::ParamType::UINT8, (const void*)&g.ua_type_2, 0, 0, 15 }, { "UAS_ID_TYPE_2", Parameters::ParamType::UINT8, (const void*)&g.id_type_2, 0, 0, 4 }, { "UAS_ID_2", Parameters::ParamType::CHAR20, (const void*)&g.uas_id_2[0], 0, 0, 0 }, - { "BAUDRATE", Parameters::ParamType::UINT32, (const void*)&g.baudrate, 57600, 9600, 921600 }, + { "BAUDRATE", Parameters::ParamType::UINT32, (const void*)&g.baudrate, 9600, 9600, 921600 }, { "WIFI_NAN_RATE", Parameters::ParamType::FLOAT, (const void*)&g.wifi_nan_rate, 0, 0, 5 }, { "WIFI_BCN_RATE", Parameters::ParamType::FLOAT, (const void*)&g.wifi_beacon_rate, 0, 0, 5 }, { "WIFI_POWER", Parameters::ParamType::FLOAT, (const void*)&g.wifi_power, 20, 2, 20 }, diff --git a/RemoteIDModule/parameters.h b/RemoteIDModule/parameters.h index b537c97..1b6ee6b 100644 --- a/RemoteIDModule/parameters.h +++ b/RemoteIDModule/parameters.h @@ -15,7 +15,7 @@ class Parameters { int8_t lock_level; uint8_t can_node; uint8_t bcast_powerup; - uint32_t baudrate = 57600; + uint32_t baudrate = 9600; uint8_t ua_type; uint8_t id_type; char uas_id[21] = "ABCD123456789"; diff --git a/scripts/install_build_env.sh b/scripts/install_build_env.sh index cdf2b5d..cde9fc8 100755 --- a/scripts/install_build_env.sh +++ b/scripts/install_build_env.sh @@ -1,6 +1,6 @@ #!/bin/bash -python3 -m pip install empy +python3 -m pip install empy==3.3.4 python3 -m pip install pymavlink python3 -m pip install dronecan python3 -m pip install pyserial