diff --git a/bin/gps.dart b/bin/gps.dart new file mode 100644 index 0000000..631258e --- /dev/null +++ b/bin/gps.dart @@ -0,0 +1,6 @@ +import "package:subsystems/subsystems.dart"; + +void main() async { + final reader = GpsReader(); + await reader.init(); +} diff --git a/bin/subsystems.dart b/bin/subsystems.dart index ae5e453..2534c93 100644 --- a/bin/subsystems.dart +++ b/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(); } diff --git a/lib/can.dart b/lib/can.dart index d7c565f..9030992 100644 --- a/lib/can.dart +++ b/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 dataCanIDs = {1234: "ScienceData"}; +final Map 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 commandCanIDs = {"ScienceCommand": 0x1234}; +final Map 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 _subscription; /// Initializes the CAN library. - void init() { - can.init(); + Future 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. diff --git a/lib/src/can/socket_ffi.dart b/lib/src/can/socket_ffi.dart index 3a52070..5f9c45f 100644 --- a/lib/src/can/socket_ffi.dart +++ b/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 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 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); } } diff --git a/lib/src/can/socket_interface.dart b/lib/src/can/socket_interface.dart index e2da13b..43a8306 100644 --- a/lib/src/can/socket_interface.dart +++ b/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 init(); /// Disposes of native resources allocated to this object, and stops listening for CAN messages. void dispose() { } diff --git a/lib/src/can/socket_stub.dart b/lib/src/can/socket_stub.dart index dc93e46..b5d6f7e 100644 --- a/lib/src/can/socket_stub.dart +++ b/lib/src/can/socket_stub.dart @@ -11,7 +11,7 @@ class CanStub implements CanSocket { } @override - void init() { } + Future init() async { } @override void dispose() { } diff --git a/lib/src/serial/gps.dart b/lib/src/serial/gps.dart new file mode 100644 index 0000000..9bce7b8 --- /dev/null +++ b/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 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(); +} diff --git a/lib/subsystems.dart b/lib/subsystems.dart index 4f9a514..55679d2 100644 --- a/lib/subsystems.dart +++ b/lib/subsystems.dart @@ -2,10 +2,12 @@ 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 { @@ -13,13 +15,16 @@ class SubsystemsCollection { 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 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"); } @@ -27,6 +32,7 @@ class SubsystemsCollection { Future dispose() async { can.dispose(); await server.dispose(); + gps.dispose(); logger.info("Subsystems disposed"); } } diff --git a/pubspec.lock b/pubspec.lock index 9789c88..8e3d379 100644 --- a/pubspec.lock +++ b/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: diff --git a/pubspec.yaml b/pubspec.yaml index 9f2027d..f44f94b 100644 --- a/pubspec.yaml +++ b/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 diff --git a/src/Makefile b/src/Makefile index 9e3d082..796cce5 100644 --- a/src/Makefile +++ b/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 diff --git a/src/burt_can/burt_can.cpp b/src/burt_can/burt_can.cpp index c727be8..6a97983 100755 --- a/src/burt_can/burt_can.cpp +++ b/src/burt_can/burt_can.cpp @@ -10,6 +10,7 @@ #include #include +#include #include #include #include @@ -17,7 +18,7 @@ #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; diff --git a/src/burt_can/burt_can_ffi.cpp b/src/burt_can/burt_can_ffi.cpp index 8daea26..77608fc 100644 --- a/src/burt_can/burt_can_ffi.cpp +++ b/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) {