Gps and CAN fixes (#5)

Co-authored-by: Binghamton University Rover Team <[email protected]>
Co-authored-by: Levi Lesches <[email protected]>

bin/gps.dart

@@ -0,0 +1,6 @@
+import "package:subsystems/subsystems.dart";
+
+void main() async {
+  final reader = GpsReader();
+  await reader.init();
+}

bin/subsystems.dart

@@ -1,5 +1,7 @@
 import "package:subsystems/subsystems.dart";
+import "package:burt_network/logging.dart";
 
 void main() async {
+  BurtLogger.level = LogLevel.debug;  
   await collection.init();
 }

lib/can.dart

@@ -19,10 +19,22 @@ export "src/can/message.dart";
 export "src/can/socket_interface.dart";
 
 /// Maps CAN IDs to [WrappedMessage.name] for data messages.
-const Map<int, String> dataCanIDs = {1234: "ScienceData"};
+final Map<int, String> dataCanIDs = {
+	0x13: ElectricalData().messageName,
+	0x14: DriveData().messageName,
+	0x15: ArmData().messageName,
+	0x16: GripperData().messageName,
+	0x17: ScienceData().messageName,
+};
 
 /// Maps [WrappedMessage.name] to CAN IDs for command messages.
-const Map<String, int> commandCanIDs = {"ScienceCommand": 0x1234};
+final Map<String, int> commandCanIDs = {
+	ArmCommand().messageName: 0x23,
+	GripperCommand().messageName: 0x33,
+	ScienceCommand().messageName: 0x43,
+	DriveCommand().messageName: 0x53,
+	ElectricalCommand().messageName: 0x63,
+};
 
 /// Manages a CAN socket on the subsystems program.
 /// 
@@ -35,8 +47,8 @@ class CanService {
 	late final StreamSubscription<CanMessage> _subscription;
 
 	/// Initializes the CAN library.
-	void init() {
-		can.init();
+	Future<void> init() async {
+		await can.init();
 		_subscription = can.incomingMessages.listen(onMessage);
 	}
 
@@ -48,10 +60,10 @@ class CanService {
 
 	/// Handles an incoming CAN message.
 	void onMessage(CanMessage message) {
-		logger.debug("Received CAN message (${message.id.toRadixString(16)}): ${message.data}");
 		final name = dataCanIDs[message.id];
+		logger.debug("Received CAN message (0x${message.id.toRadixString(16)}): ${message.data}. Name=${name ?? 'None'}");
 		if (name == null) {
-			logger.warning("Unknown CAN ID: ${message.id}");
+			logger.warning("Unknown CAN ID: 0x${message.id.toRadixString(16)}");
 			return; 
 		}
 		// We must copy the data since we'll be disposing the pointer.

lib/src/can/socket_ffi.dart

@@ -1,4 +1,5 @@
 import "dart:async";
+import "dart:io";
 
 import "package:burt_network/logging.dart";
 
@@ -53,7 +54,14 @@ class CanFFI implements CanSocket {
   Timer? _timer;
 
   @override
-  void init() { 
+  Future<void> init() async { 
+    await Process.run("sudo", ["ip", "link", "set", "can0", "down"]);
+    final result = await Process.run("sudo", ["ip", "link", "set", "can0", "up", "type", "can", "bitrate", "500000"]);
+    if (result.exitCode != 0) {
+      logger.critical("Could not start the CAN bus");
+      logger.critical("Output: ${result.stderr}");
+      exit(1);
+    }
     final error = getCanError(nativeLib.BurtCan_open(_can));
     if (error != null) throw CanException(error);
     _startListening(); 
@@ -70,7 +78,8 @@ class CanFFI implements CanSocket {
   @override
   void sendMessage({required int id, required List<int> data}) {
     final message = CanMessage(id: id, data: data);
-    nativeLib.BurtCan_send(_can, message.pointer);
+    final error = getCanError(nativeLib.BurtCan_send(_can, message.pointer));
+    if (error != null) throw CanException(error);
     message.dispose();
   }
 
@@ -81,10 +90,12 @@ class CanFFI implements CanSocket {
       final pointer = nativeLib.NativeCanMessage_create();
       final error = getCanError(nativeLib.BurtCan_receive(_can, pointer));
       if (error != null) throw CanException(error);
-      if (pointer.ref.length == 0) return;
+      if (pointer.ref.length == 0) break;
       count++;
-      if (count == 10) logger.warning("Processed over 10 CAN messages in one callback. Consider decreasing the CAN read interval.");
-      final message = CanMessage.fromPointer(pointer, isNative: true);
+      if (count % 10 == 0) {
+      	logger.warning("Processed $count messages in one callback. Consider decreasing the CAN read interval.");
+      }
+    	final message = CanMessage.fromPointer(pointer, isNative: true);
       _controller.add(message);
     }
   }

lib/src/can/socket_interface.dart

@@ -30,7 +30,7 @@ abstract class CanSocket {
   factory CanSocket() => Platform.isLinux ? CanFFI() : CanStub();
 
   /// Starts listening for CAN messages.
-  void init() { }
+  Future<void> init();
 
   /// Disposes of native resources allocated to this object, and stops listening for CAN messages.
   void dispose() { }

lib/src/can/socket_stub.dart

@@ -11,7 +11,7 @@ class CanStub implements CanSocket {
 	}
 
 	@override
-	void init() { }
+	Future<void> init() async { }
 
 	@override
 	void dispose() { }

lib/src/serial/gps.dart

@@ -0,0 +1,68 @@
+import "dart:convert";
+import "dart:io";
+
+import "package:burt_network/burt_network.dart";
+import "package:subsystems/subsystems.dart";
+
+/// The port/device file to listen to the GPS on.
+const serialPort = "/dev/ttyACM0";
+
+/// Listens to the GPS and sends its output to the Dashboard. 
+/// 
+/// Call [init] to start listening and [dispose] to stop.
+class GpsReader {
+  /// Parses an NMEA sentence into a [GpsCoordinates] object.
+  /// 
+  /// See https://shadyelectronics.com/gps-nmea-sentence-structure.
+  static GpsCoordinates? parseNMEA(String nmeaSentence) {
+    final parts = nmeaSentence.split(",");
+    final tag = parts.first;
+    if (tag.endsWith("GGA")) {
+      return GpsCoordinates(
+        latitude: _nmeaToDecimal(double.tryParse(parts[2]) ?? 0.0), 
+        longitude: _nmeaToDecimal(double.tryParse(parts[4]) ?? 0.0),
+        altitude: double.tryParse(parts[9]) ?? 0.0,
+      );
+    } else if (tag.endsWith("RMC")) {
+      return GpsCoordinates(
+        latitude: _nmeaToDecimal(double.tryParse(parts[3]) ?? 0.0), 
+        longitude: _nmeaToDecimal(double.tryParse(parts[5]) ?? 0.0),
+      );
+    } else if (tag.endsWith("GLL")) {
+      return GpsCoordinates(
+        latitude: _nmeaToDecimal(double.tryParse(parts[1]) ?? 0.0),
+        longitude: _nmeaToDecimal(double.tryParse(parts[3]) ?? 0.0),
+      );
+    } else {
+      return null;
+    }
+  }
+
+  static double _nmeaToDecimal(double nmeaValue) {
+    final degrees = nmeaValue ~/ 100;
+    final minutes = nmeaValue % 100;
+    return degrees + minutes / 60.0;
+  }
+
+  /// The `cat` process that's reading from the GPS.
+  Process? cat;
+
+  /// Parses a line of NMEA output and sends the GPS coordinates to the dashboard.
+  void handleLine(String line) {
+    final coordinates = parseNMEA(line);
+    if (coordinates == null) return;
+    logger.debug("GPS Read: $coordinates");
+    final roverPosition = RoverPosition(gps: coordinates);
+    collection.server.sendMessage(roverPosition);
+  }
+
+  /// Starts reading the GPS (on [serialPort]) through the `cat` Linux program.
+  Future<void> init() async {
+    logger.info("Reading GPS on port $serialPort");
+    cat = await Process.start("cat", [serialPort]);
+    cat!.stdout.transform(utf8.decoder).transform(const LineSplitter()).listen(handleLine);
+  }
+
+  /// Closes the [cat] process to stop listening to the GPS.
+  void dispose() => cat?.kill();
+}

lib/subsystems.dart

@@ -2,31 +2,37 @@ import "package:burt_network/logging.dart";
 import "package:subsystems/can.dart";
 
 import "src/server.dart";
+import "src/serial/gps.dart";
 
 export "src/server.dart";
 export "src/serial/imu.dart";
 export "src/serial/serial.dart";
+export "src/serial/gps.dart";
 
 /// Contains all the resources needed by the subsystems program.
 class SubsystemsCollection {
 	/// The CAN bus socket.
 	final can = CanService();
 	/// The UDP server.
 	final server = SubsystemsServer(port: 8001);
+	/// The GPS reader.
+	final gps = GpsReader();
 
 	/// Initializes all the resources needed by the subsystems.
 	Future<void> init() async {
 		BurtLogger.level = LogLevel.debug;
 		logger.debug("Running in debug mode...");
-		can.init();
+		await can.init();
 		await server.init();
+		await gps.init();
 		logger.info("Subsystems initialized");
 	}
 
 	/// Disposes all the resources needed by the subsystems.
 	Future<void> dispose() async {
 		can.dispose();
 		await server.dispose();
+		gps.dispose();
 		logger.info("Subsystems disposed");
 	}
 }

pubspec.lock

@@ -44,11 +44,9 @@ packages:
   burt_network:
     dependency: "direct main"
     description:
-      path: "."
-      ref: HEAD
-      resolved-ref: ab3c4681fddd2cb316cee5cfbbfada2c88a5f622
-      url: "https://github.com/BinghamtonRover/Networking.git"
-    source: git
+      path: "../Networking"
+      relative: true
+    source: path
     version: "1.0.0"
   cli_util:
     dependency: transitive
@@ -190,10 +188,10 @@ packages:
     dependency: transitive
     description:
       name: logger
-      sha256: "6bbb9d6f7056729537a4309bda2e74e18e5d9f14302489cc1e93f33b3fe32cac"
+      sha256: "7ad7215c15420a102ec687bb320a7312afd449bac63bfb1c60d9787c27b9767f"
       url: "https://pub.dev"
     source: hosted
-    version: "2.0.2+1"
+    version: "1.4.0"
   logging:
     dependency: transitive
     description:

pubspec.yaml

@@ -18,3 +18,7 @@ dev_dependencies:
   ffigen: ^9.0.1
   test: ^1.21.0
   very_good_analysis: ^5.0.0+1
+
+dependency_overrides:
+  burt_network:
+    path: ../Networking

src/Makefile

@@ -10,6 +10,7 @@ clean:
 	rm -f burt_can/*.so
 	rm -f libserialport/*.so
 	rm -f ../*.so
+	make -C burt_can clean
 
 SerialCommand = gcc
 SerialCommand += libserialport/serialport.c

src/burt_can/burt_can.cpp

@@ -10,14 +10,15 @@
 
 #include <cstring>
 #include <stdio.h>
+#include <fcntl.h>
 #include <stdlib.h>
 #include <string.h>
 #include <iostream>
 
 #include "burt_can.hpp"
 
 void printError() {
-	std::cout << "Error from C code: " << strerror(errno) << std::endl;
+	std::cout << "Error from C code (" << errno << "): " << strerror(errno) << std::endl;
 }
 
 burt_can::BurtCan::BurtCan(const char* interface, int32_t readTimeout, BurtCanType mode) :
@@ -32,6 +33,7 @@ BurtCanStatus burt_can::BurtCan::open() {
 
 	// Open the socket
 	handle = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+	fcntl(handle, F_SETFL, SOCK_NONBLOCK);
 	if (handle < 0) {
 		printError();
 		return BurtCanStatus::SOCKET_CREATE_ERROR;
@@ -81,6 +83,7 @@ BurtCanStatus burt_can::BurtCan::send(const NativeCanMessage* frame) {
 	// Copy the CanFrame to a can_frame and send it.
 	can_frame raw;
 	raw.can_id = frame->id;
+	raw.can_id |= CAN_EFF_FLAG;
 	raw.len = frame->length;
 	std::memcpy(raw.data, frame->data, 8);
 	int size = sizeof(raw);
@@ -96,12 +99,15 @@ BurtCanStatus burt_can::BurtCan::receive(NativeCanMessage* frame) {
 	can_frame raw;
 	long bytesRead = read(handle, &raw, sizeof(raw));
 	if (bytesRead < 0) {
+		if (errno == 11) return BurtCanStatus::OK;
 		printError();
 		return BurtCanStatus::READ_ERROR;
 	} else if (bytesRead < (long) sizeof(raw)) {
 		return BurtCanStatus::FRAME_NOT_FULLY_READ;
 	} else {
-		frame->id = raw.can_id;
+		// First bit is the EFF flag. Remove it before parsing
+		uint32_t id = (raw.can_id << 1) >> 1;
+		frame->id = id;
 		frame->length = raw.len;
 		std::memcpy(frame->data, raw.data, 8);
 		return BurtCanStatus::OK;

src/burt_can/burt_can_ffi.cpp

@@ -35,7 +35,9 @@ BurtCanStatus BurtCan_close(BurtCan* pointer) {
 }
 
 NativeCanMessage* NativeCanMessage_create() {
-  return new NativeCanMessage;
+  NativeCanMessage* pointer = new NativeCanMessage();
+  pointer->data = new uint8_t[8];
+  return pointer;
 }
 
 void NativeCanMessage_free(NativeCanMessage* pointer) {