Added code for new gnss message in comments

Author: avtoku

boards/varmint_h7/common/Varmint.cpp

@@ -248,14 +248,15 @@ bool Varmint::gnss_read(rosflight_firmware::GnssStruct * gnss)
   if (gps_.rxFifoReadMostRecent((uint8_t *) &p, sizeof(p))) {
     gnss->header = p.header;
     gnss->pps = p.pps;
-    struct tm tm;
-    tm.tm_sec = p.pvt.sec;
-    tm.tm_min = p.pvt.min;
-    tm.tm_hour = p.pvt.hour;
-    tm.tm_mday = p.pvt.day;
-    tm.tm_mon = p.pvt.month - 1;
-    tm.tm_year = p.pvt.year - 1900;
-    gnss->time = mktime(&tm);
+//    struct tm tm;
+//    tm.tm_sec = p.pvt.sec;
+//    tm.tm_min = p.pvt.min;
+//    tm.tm_hour = p.pvt.hour;
+//    tm.tm_mday = p.pvt.day;
+//    tm.tm_mon = p.pvt.month - 1;
+//    tm.tm_year = p.pvt.year - 1900;
+    //gnss->time = mktime(&tm);
+    gnss->unix_seconds = p.unix_seconds; // Unix time
     gnss->t_acc = p.pvt.tAcc;
     gnss->time_of_week = p.pvt.iTOW;
     gnss->year = p.pvt.year;

boards/varmint_h7/common/drivers/Ubx.cpp

@@ -41,7 +41,7 @@
 
 #include "Packets.h"
 #include "misc.h"
-
+#include <ctime>
 extern Time64 time64;
 
 #define SET(buf, data, type) *((type *) (buf)) = data
@@ -235,6 +235,19 @@ void Ubx::endDma(void)
       if ((p.cl == 0x01) && (p.id == 0x07)) {
         gotPvt_ = time64.Us();
         memcpy((uint8_t *) &(ubx_.pvt), p.payload, sizeof(ubx_.pvt));
+        struct tm tm;
+        tm.tm_sec  = ubx_.pvt.sec;
+        tm.tm_min  = ubx_.pvt.min;
+        tm.tm_hour = ubx_.pvt.hour;
+        tm.tm_mday = ubx_.pvt.day;
+        tm.tm_mon  = ubx_.pvt.month - 1;
+        tm.tm_year = ubx_.pvt.year - 1900;
+        ubx_.unix_seconds = mktime(&tm);
+        if (ubx_.pvt.nano<0)
+        {
+          ubx_.unix_seconds -= 1;
+          ubx_.pvt.nano += 1000000000;
+        }
       } else if ((p.cl == 0x01) && (p.id == 0x20)) {
         gotTime_ = time64.Us();
         memcpy((uint8_t *) (&ubx_.time), p.payload, sizeof(ubx_.time));

boards/varmint_h7/common/drivers/Ubx.h

@@ -136,6 +136,7 @@ typedef struct __attribute__((__packed__))
 typedef struct __attribute__((__packed__)) // This matches the Ubx packet, do not modify
 {
   rosflight_firmware::PacketHeader header;
+  int64_t unix_seconds; // computed from pvt time values
   uint64_t drdy;
   uint64_t groupDelay; // us, time from measurement to drdy, (approximate!)
   uint64_t pps;

comms/mavlink/generating_v1.0_instructions.txt

@@ -7,7 +7,8 @@ git clone https://github.com/rosflight/mavlink.git
 # Create the .c/h files from that directory
 # in windows:
 set PYTHONPATH=%PYTHONPATH%;path\to\mavlink\repo && python3 -m pymavlink.tools.mavgen --lang=C --wire-protocol=1.0 --output=./v1.0 rosflight.xml
-# e.g., 
+# e.g.,
+rmdir /s v1.0 
 set PYTHONPATH=%PYTHONPATH%;C:\Projects\mavlink && python3 -m pymavlink.tools.mavgen --lang=C --wire-protocol=1.0 --output=./v1.0 rosflight.xml
 
 # in linux:

comms/mavlink/mavlink.cpp

@@ -142,16 +142,35 @@ void Mavlink::send_imu(uint8_t system_id, uint64_t timestamp_us, const turbomath
                              gyro.x, gyro.y, gyro.z, temperature);
   send_message(msg, 0);
 }
+
+#ifdef MAVLINK_MESSAGE_INFO_ROSFLIGHT2_GNSS
+void Mavlink::send_gnss(uint8_t system_id, const GNSSData & data)
+{
+  mavlink_message_t msg;
+  mavlink_msg_rosflight2_gnss_pack(
+    system_id, compid_, &msg,
+    data.unix_seconds, // Unix time in seconds
+    data.nano, // nanoseconds
+    data.fix_type,
+    data.num_sat,
+    data.lat, data.lon, data.height_ellipsoid,
+    data.vel_n, data.vel_e, data.vel_d,
+    data.h_acc, data.v_acc, data.speed_accy,
+    data.header.timestamp);
+  send_message(msg);
+}
+#else
 void Mavlink::send_gnss(uint8_t system_id, const GNSSData & data)
 {
   mavlink_message_t msg;
   mavlink_msg_rosflight_gnss_pack(
-    system_id, compid_, &msg, data.time_of_week, data.fix_type, data.time, data.nano, data.lat,
+    system_id, compid_, &msg, data.time_of_week, data.fix_type, data.unix_seconds, data.nano, data.lat,
     data.lon, data.height_ellipsoid, data.vel_n, data.vel_e, data.vel_d, data.h_acc, data.v_acc, data.ecef.x,
     data.ecef.y, data.ecef.z, data.ecef.p_acc, data.ecef.vx, data.ecef.vy, data.ecef.vz,
     data.ecef.s_acc, data.header.timestamp);
   send_message(msg);
 }
+#endif
 
 void Mavlink::send_gnss_full(uint8_t system_id, const GNSSFull & full)
 {

comms/mavlink/rosflight.xml

@@ -8,11 +8,16 @@ git clone https://github.com/rosflight/mavlink.git
 
 # Create the .c/h files from that directory
 # in windows:
+rmdir /s v1.0 
+Y
 set PYTHONPATH=%PYTHONPATH%;path\to\mavlink\repo && python3 -m pymavlink.tools.mavgen --lang=C --wire-protocol=1.0 --output=./v1.0 rosflight.xml
 # e.g., 
+rmdir /s v1.0 
+Y
 set PYTHONPATH=%PYTHONPATH%;C:\Projects\mavlink && python3 -m pymavlink.tools.mavgen --lang=C --wire-protocol=1.0 --output=./v1.0 rosflight.xml
 
 # in linux:
+sudo rm -r ./v1.0
 PYTHONPATH=$PYTHONPATH:/path/to/mavlink/repo python3 -m pymavlink.tools.mavgen --lang=C --wire-protocol=1.0 --output=./v1.0 rosflight.xml
 
 # If you are in the mavlink repo environment with rosflight.xml in the standard location:
@@ -316,8 +321,8 @@ python3 -m pymavlink.tools.mavgen --lang=C --wire-protocol=1.0 --output=generate
 			<field type="uint32_t" name="doRearm"/>
 		</message>
 		<!--
-		<message id="197" name="ROSFLIGHT_GNSS">
-			<description>Rosflight GNSS message</description>
+		<message id="197" name="ROSFLIGHT2_GNSS">
+			<description>Rosflight2 GNSS message</description>
 			<field type="int64_t" name="seconds">Unix time, in seconds</field>
 			<field type="int32_t" name="nanos">Fractional Unix time</field>
 			<field type="uint8_t" name="fix_type" enum="GNSS_FIX_TYPE">GNSS fix type</field>

comms/mavlink/v1.0/rosflight/mavlink.h

@@ -6,7 +6,7 @@
 #ifndef MAVLINK_H
 #define MAVLINK_H
 
-#define MAVLINK_PRIMARY_XML_HASH -7137736376506596709
+#define MAVLINK_PRIMARY_XML_HASH -428044560288662501
 
 #ifndef MAVLINK_STX
 #define MAVLINK_STX 254