From 9c848e83680362e61de33a491311ce5a946378c6 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Thu, 5 Jul 2018 20:22:51 -0700 Subject: [PATCH] Added first version of tower firmware. Spits out GPS and IMU data on port RS485-1 and talks over modbus on RS585-2. Modbus side doesn't do anything atm. --- software/firmware/tower/tower.ino | 250 +++++++++++++++++++----------- 1 file changed, 159 insertions(+), 91 deletions(-) diff --git a/software/firmware/tower/tower.ino b/software/firmware/tower/tower.ino index b331f82..4fd25d2 100644 --- a/software/firmware/tower/tower.ino +++ b/software/firmware/tower/tower.ino @@ -1,9 +1,7 @@ -#include -#include -#include +////////// Includes ////////// +#include #include - /* Imu/data (Imu) Imu/raw (Imu) @@ -26,60 +24,88 @@ temp (deg c) */ -Adafruit_BNO055 bno = Adafruit_BNO055(WIRE_BUS, -1, BNO055_ADDRESS_A, I2C_MASTER, I2C_PINS_29_30, I2C_PULLUP_INT, I2C_RATE_100, I2C_OP_MODE_ISR); +////////// Hardware / Data Enumerations ////////// +enum HARDWARE { + GPS_IMU_RS485_EN = 3, + GPS_IMU_RS485_RX = 9, + GPS_IMU_RS485_TX = 10, + COMMS_RS485_EN = 2, + COMMS_RS485_RX = 0, + COMMS_RS485_TX = 1, -#define gpsPort Serial2 -#define commsPort Serial3 -#define GPS_PORT_NAME "Serial2" + GPS_UART_RX = 7, + GPS_UART_TX = 8, -const unsigned char ubxRate5Hz[] = { 0x06, 0x08, 0x06, 0x00, 200, 0x00, 0x01, 0x00, 0x01, 0x00 }; -const unsigned char ubxRate10Hz[] = { 0x06, 0x08, 0x06, 0x00, 100, 0x00, 0x01, 0x00, 0x01, 0x00 }; + IMU_SDA = 18, + IMU_SCL = 19, -const char baud38400 [] = "PUBX,41,1,3,3,38400,0"; -const char baud57600 [] = "PUBX,41,1,3,3,57600,0"; -const char baud115200[] = "PUBX,41,1,3,3,115200,0"; + WS2812_DATA = 11, + C02_SENSOR = A7, + MISC_PIN = A8, + LED_BLUE_EXTRA = 13 +}; + +enum MODBUS_REGISTERS { + DIRECTION = 0, // Input + SPEED = 1, // Input + SLEEP = 2, // Input + + CURRENT = 3, // Output + FAULT = 4, // Output + + TEMPERATURE = 5 // Output +}; + +#define GPS_SERIAL_PORT Serial3 +#define GPS_IMU_STREAMING_PORT Serial2 + +////////// Global Variables ////////// +///// Modbus +const uint8_t node_id = 1; +const uint8_t mobus_serial_port_number = 1; + +uint16_t modbus_data[] = {0, 0}; +uint8_t num_modbus_registers = 0; + +int8_t poll_state = 0; +bool communication_good = false; +uint8_t message_count = 0; + +///// IMU imu::Vector<3> linear_accel; imu::Vector<3> angular_vel; imu::Quaternion quat; -char current_byte = '$'; -String nmea_sentence = ""; -String output_sentence; - char float_decimal_places = 8; -void sendUBX( const unsigned char *progmemBytes, size_t len ) -{ - gpsPort.write( 0xB5 ); // SYNC1 - gpsPort.write( 0x62 ); // SYNC2 +///// GPS +char current_byte = '$'; +String nmea_sentence = ""; - uint8_t a = 0, b = 0; - while (len-- > 0) { - uint8_t c = pgm_read_byte( progmemBytes++ ); - a += c; - b += a; - gpsPort.write( c ); - } +////////// Class Instantiations ////////// +Modbus slave(node_id, mobus_serial_port_number, HARDWARE::COMMS_RS485_EN); - gpsPort.write( a ); // CHECKSUM A - gpsPort.write( b ); // CHECKSUM B - -} +Adafruit_BNO055 bno = Adafruit_BNO055(WIRE_BUS, -1, BNO055_ADDRESS_A, I2C_MASTER, I2C_PINS_18_19, I2C_PULLUP_INT, I2C_RATE_100, I2C_OP_MODE_ISR); void setup() { + // Debugging Serial.begin(9600); - while (!Serial); - Serial.println("Booting up"); - // put your setup code here, to run once: - commsPort.begin(115200); - commsPort.transmitterEnable(3); - delay(250); - gpsPort.begin(9600); - // sendUBX( ubxRate5Hz, sizeof(ubxRate5Hz) ); + // Raw pin setup + setup_hardware(); + // Setup modbus serial communication + num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); + slave.begin(115200); // baud-rate at 19200 + slave.setTimeOut(150); + + // GPS & IMU serial streaming setup + GPS_IMU_STREAMING_PORT.begin(115200); + GPS_IMU_STREAMING_PORT.transmitterEnable(HARDWARE::GPS_IMU_RS485_EN); + + // IMU Setup Serial.println("Setting up IMU"); if (!bno.begin()) { /* There was a problem detecting the BNO055 ... check your connections */ @@ -90,75 +116,117 @@ void setup() { bno.setExtCrystalUse(true); Serial.println("IMU Configured."); + // GPS Setup + GPS_SERIAL_PORT.begin(9600); } void loop() { + poll_modbus(); + set_leds(); + send_imu_stream_line(); + process_gps_and_send_if_ready(); - if (gpsPort.available() > 0 ) { - nmea_sentence = ""; - do { - if (gpsPort.available() > 0) { - current_byte = gpsPort.read(); +} - if (current_byte != '\r' and current_byte != '\n') { - nmea_sentence += current_byte; - } - } - } while (current_byte != '\r'); - commsPort.println(nmea_sentence); - } +void setup_hardware() { + // Setup pins as inputs / outputs + pinMode(HARDWARE::WS2812_DATA, OUTPUT); + pinMode(HARDWARE::C02_SENSOR, INPUT); + pinMode(HARDWARE::MISC_PIN, OUTPUT); + pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT); + + // Set default pin states + digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW); + + // Set teensy to increased analog resolution + analogReadResolution(13); + +} + +void send_imu_stream_line() { quat = bno.getQuat(); linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); angular_vel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); - commsPort.print("ox:"); - commsPort.print(quat.x(), float_decimal_places); - commsPort.print(","); - commsPort.print("oy:"); - commsPort.print(quat.y(), float_decimal_places); - commsPort.print(","); - commsPort.print("oz:"); - commsPort.print(quat.z(), float_decimal_places); - commsPort.print(","); - commsPort.print("ow:"); - commsPort.print(quat.w(), float_decimal_places); + GPS_IMU_STREAMING_PORT.print("ox:"); + GPS_IMU_STREAMING_PORT.print(quat.x(), float_decimal_places); + GPS_IMU_STREAMING_PORT.print(","); + GPS_IMU_STREAMING_PORT.print("oy:"); + GPS_IMU_STREAMING_PORT.print(quat.y(), float_decimal_places); + GPS_IMU_STREAMING_PORT.print(","); + GPS_IMU_STREAMING_PORT.print("oz:"); + GPS_IMU_STREAMING_PORT.print(quat.z(), float_decimal_places); + GPS_IMU_STREAMING_PORT.print(","); + GPS_IMU_STREAMING_PORT.print("ow:"); + GPS_IMU_STREAMING_PORT.print(quat.w(), float_decimal_places); - commsPort.print(","); - commsPort.print("lax:"); - commsPort.print(linear_accel.x(), float_decimal_places); - commsPort.print(","); - commsPort.print("lay:"); - commsPort.print(linear_accel.y(), float_decimal_places); - commsPort.print(","); - commsPort.print("laz:"); - commsPort.print(linear_accel.z(), float_decimal_places); + GPS_IMU_STREAMING_PORT.print(","); + GPS_IMU_STREAMING_PORT.print("lax:"); + GPS_IMU_STREAMING_PORT.print(linear_accel.x(), float_decimal_places); + GPS_IMU_STREAMING_PORT.print(","); + GPS_IMU_STREAMING_PORT.print("lay:"); + GPS_IMU_STREAMING_PORT.print(linear_accel.y(), float_decimal_places); + GPS_IMU_STREAMING_PORT.print(","); + GPS_IMU_STREAMING_PORT.print("laz:"); + GPS_IMU_STREAMING_PORT.print(linear_accel.z(), float_decimal_places); - commsPort.print(","); - commsPort.print("avx:"); - commsPort.print(angular_vel.x(), float_decimal_places); - commsPort.print(","); - commsPort.print("avy:"); - commsPort.print(angular_vel.y(), float_decimal_places); - commsPort.print(","); - commsPort.print("avz:"); - commsPort.print(angular_vel.z(), float_decimal_places); + GPS_IMU_STREAMING_PORT.print(","); + GPS_IMU_STREAMING_PORT.print("avx:"); + GPS_IMU_STREAMING_PORT.print(angular_vel.x(), float_decimal_places); + GPS_IMU_STREAMING_PORT.print(","); + GPS_IMU_STREAMING_PORT.print("avy:"); + GPS_IMU_STREAMING_PORT.print(angular_vel.y(), float_decimal_places); + GPS_IMU_STREAMING_PORT.print(","); + GPS_IMU_STREAMING_PORT.print("avz:"); + GPS_IMU_STREAMING_PORT.print(angular_vel.z(), float_decimal_places); - commsPort.println(); + GPS_IMU_STREAMING_PORT.println(); // // /* Display calibration status for each sensor. */ // uint8_t system, gyro, accel, mag = 0; // bno.getCalibration(&system, &gyro, &accel, &mag); - // commsPort.print("CALIBRATION: Sys="); - // commsPort.print(system, DEC); - // commsPort.print(" Gyro="); - // commsPort.print(gyro, DEC); - // commsPort.print(" Accel="); - // commsPort.print(accel, DEC); - // commsPort.print(" Mag="); - // commsPort.print(mag, DEC); - + // Serial.print("CALIBRATION: Sys="); + // Serial.print(system, DEC); + // Serial.print(" Gyro="); + // Serial.print(gyro, DEC); + // Serial.print(" Accel="); + // Serial.print(accel, DEC); + // Serial.print(" Mag="); + // Serial.print(mag, DEC); +} + +void process_gps_and_send_if_ready() { + if (GPS_SERIAL_PORT.available() > 0 ) { + current_byte = GPS_SERIAL_PORT.read(); + + if (current_byte != '\r' && current_byte != '\n') { + nmea_sentence += current_byte; + } + + if (current_byte == '\r') { + GPS_IMU_STREAMING_PORT.println(nmea_sentence); + nmea_sentence = ""; + } + } +} + +void poll_modbus() { + poll_state = slave.poll(modbus_data, num_modbus_registers); + communication_good = !slave.getTimeOutState(); +} + +void set_leds() { + if (poll_state > 4) { + message_count++; + if (message_count > 2) { + digitalWrite(HARDWARE::LED_BLUE_EXTRA, !digitalRead(HARDWARE::LED_BLUE_EXTRA)); + message_count = 0; + } + } else if (!communication_good) { + digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW); + } }