From 92bc9eb9022b395fd1b6219cd4a8fb21f0b3988e Mon Sep 17 00:00:00 2001 From: Maxim Marshalov Date: Tue, 10 Sep 2024 15:50:06 +0300 Subject: [PATCH 1/3] Add calibartion --- etc/arduino/arduino.ino | 72 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 70 insertions(+), 2 deletions(-) diff --git a/etc/arduino/arduino.ino b/etc/arduino/arduino.ino index 610f0ff..f176d38 100644 --- a/etc/arduino/arduino.ino +++ b/etc/arduino/arduino.ino @@ -26,6 +26,15 @@ #define TX_PIN 6 #define ECHO_PACKAGE_SIZE 4 +//compass +enum CompassState{ + WORKING, + CALIBRATION +}; +byte READ_DATA_COMMAND[] = { 0x68, 0x04, 0x00, 0x04, 0x08 }; +byte START_CALIBARTION_COMMAND[] = {0x68, 0x04, 0x00, 0x08, 0x0C}; +byte STOP_CALIBRATION_COMMAND [] = {0x68, 0x04, 0x00, 0x0A, 0x0E}; + //Modbus registers #define LEFT_THRUSTER_REG 1 #define RIGHT_THRUSTER_REG 2 @@ -33,6 +42,8 @@ #define CURRENT_REG 4 #define VOLTAGE_REG 5 #define BATTERY_REG 6 +#define HEADING_REG 7 +#define CALIBRATION_REG 8 // ModbusSerial object ModbusSerial mb (MODBUS_SERIAL, SLAVE_ID, -1); @@ -40,6 +51,9 @@ ModbusSerial mb (MODBUS_SERIAL, SLAVE_ID, -1); // Motor objects Servo thruster_left; Servo thruster_right; +enum CompassState state; +uint16_t buffer[14]; +int index; SoftwareSerial softSerial = SoftwareSerial(RX_PIN, TX_PIN); int echoPackage[ECHO_PACKAGE_SIZE]; @@ -63,10 +77,62 @@ void setup() { mb.addHreg(BATTERY_REG, 0); mb.addHreg(CURRENT_REG, 0); mb.addHreg(VOLTAGE_REG, 0); + mb.addHreg(HEADING_REG, 0); + mb.addHreg(CALIBRATION_REG, 0); + state = WORKING; pinMode(CURRENT_PIN, INPUT); pinMode(VOLTAGE_PIN, INPUT); +} +float ConvertFloatValue(uint16_t *buffer) +{ + float wholePart = (buffer[0] & 0x0F) * 100 + (buffer[1] >> 4) * 10 + (buffer[1] & 0x0F) * 1, + fractionalPart = (buffer[2] >> 4) * 10 + (buffer[2] & 0x0F) * 1; + + float result = wholePart + fractionalPart / 100; + + return (buffer[0] >> 4) ? -result : result; +} +void ReadCompassData(){ + if(state == WORKING){ + softSerial.write(READ_DATA_COMMAND, 5); + while(index < 14){ + if (softSerial.available()) { + buffer[index] = softSerial.read(); + } + index ++; + } + int heading = ConvertFloatValue(&buffer[10]); + mb.setHreg(HEADING_REG, heading); + + index = 0; + } + +} +void StartCalibrationCompass(){ + if(state == CALIBRATION){ + softSerial.write(START_CALIBARTION_COMMAND, 5); + } +} +void StopCalibrationCompass(){ + if(state == CALIBRATION){ + softSerial.write(STOP_CALIBRATION_COMMAND, 5); + } +} +void CheckCompassState(){ + if(mb.Hreg(CALIBRATION_REG) == 1 && state == WORKING){ + state = CALIBRATION; + StartCalibrationCompass(); + } + else if(mb.Hreg(CALIBRATION_REG) == 0 && state == CALIBRATION){ + StopCalibrationCompass(); + state = WORKING; + } + else{ + ReadCompassData(); + } + } void ping(){ if(millis() - lastPing > TIMEOUT){ @@ -103,8 +169,10 @@ void BatteryRead(){ void loop() { - ping(); - ReadDepth(); +// ping(); +// ReadDepth(); + CheckCompassState(); + BatteryRead(); mb.task(); thruster_left.writeMicroseconds(constrain(mb.Hreg(LEFT_THRUSTER_REG), PWM_LOW, PWM_HIGH)); -- GitLab From 4bb2f55ae9dfe4b1b54f0dc59be71de778001eff Mon Sep 17 00:00:00 2001 From: Maxim Marshalov Date: Wed, 11 Sep 2024 17:59:06 +0300 Subject: [PATCH 2/3] update compass logic --- etc/arduino/arduino.ino | 27 ++++++++------------------- 1 file changed, 8 insertions(+), 19 deletions(-) diff --git a/etc/arduino/arduino.ino b/etc/arduino/arduino.ino index f176d38..3f86aff 100644 --- a/etc/arduino/arduino.ino +++ b/etc/arduino/arduino.ino @@ -11,6 +11,7 @@ #define MODBUS_SERIAL Serial #define BAUDRATE 115200 #define TIMEOUT 200 +#define LENGTHOF(array) (sizeof(array) / sizeof(*array)) //Battarey parameters #define R_S 0.01 #define R_L 49900 @@ -31,9 +32,9 @@ enum CompassState{ WORKING, CALIBRATION }; -byte READ_DATA_COMMAND[] = { 0x68, 0x04, 0x00, 0x04, 0x08 }; -byte START_CALIBARTION_COMMAND[] = {0x68, 0x04, 0x00, 0x08, 0x0C}; -byte STOP_CALIBRATION_COMMAND [] = {0x68, 0x04, 0x00, 0x0A, 0x0E}; +const byte READ_DATA_COMMAND[] = { 0x68, 0x04, 0x00, 0x04, 0x08 }; +const byte START_CALIBARTION_COMMAND[] = {0x68, 0x04, 0x00, 0x08, 0x0C}; +const byte STOP_CALIBRATION_COMMAND [] = {0x68, 0x04, 0x00, 0x0A, 0x0E}; //Modbus registers #define LEFT_THRUSTER_REG 1 @@ -96,7 +97,7 @@ float ConvertFloatValue(uint16_t *buffer) } void ReadCompassData(){ if(state == WORKING){ - softSerial.write(READ_DATA_COMMAND, 5); + softSerial.write(READ_DATA_COMMAND, LENGTHOF(READ_DATA_COMMAND)); while(index < 14){ if (softSerial.available()) { buffer[index] = softSerial.read(); @@ -110,23 +111,14 @@ void ReadCompassData(){ } } -void StartCalibrationCompass(){ - if(state == CALIBRATION){ - softSerial.write(START_CALIBARTION_COMMAND, 5); - } -} -void StopCalibrationCompass(){ - if(state == CALIBRATION){ - softSerial.write(STOP_CALIBRATION_COMMAND, 5); - } -} + void CheckCompassState(){ if(mb.Hreg(CALIBRATION_REG) == 1 && state == WORKING){ state = CALIBRATION; - StartCalibrationCompass(); + softSerial.write(START_CALIBARTION_COMMAND, LENGTHOF(START_CALIBARTION_COMMAND)); } else if(mb.Hreg(CALIBRATION_REG) == 0 && state == CALIBRATION){ - StopCalibrationCompass(); + softSerial.write(STOP_CALIBRATION_COMMAND, LENGTHOF(STOP_CALIBRATION_COMMAND)); state = WORKING; } else{ @@ -169,10 +161,7 @@ void BatteryRead(){ void loop() { -// ping(); -// ReadDepth(); CheckCompassState(); - BatteryRead(); mb.task(); thruster_left.writeMicroseconds(constrain(mb.Hreg(LEFT_THRUSTER_REG), PWM_LOW, PWM_HIGH)); -- GitLab From 6d0616353e542adb3934ff435a2e7051042db83c Mon Sep 17 00:00:00 2001 From: Maxim Marshalov Date: Wed, 11 Sep 2024 23:05:39 +0300 Subject: [PATCH 3/3] update compass data preparing --- etc/arduino/arduino.ino | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/etc/arduino/arduino.ino b/etc/arduino/arduino.ino index 3f86aff..da50e08 100644 --- a/etc/arduino/arduino.ino +++ b/etc/arduino/arduino.ino @@ -86,10 +86,10 @@ void setup() { } -float ConvertFloatValue(uint16_t *buffer) +static float ConvertFloatValue(uint16_t *buffer) { - float wholePart = (buffer[0] & 0x0F) * 100 + (buffer[1] >> 4) * 10 + (buffer[1] & 0x0F) * 1, - fractionalPart = (buffer[2] >> 4) * 10 + (buffer[2] & 0x0F) * 1; + float wholePart = (buffer[0] & 0x0F) * 100 + (buffer[1] >> 4) * 10 + (buffer[1] & 0x0F) * 1; + float fractionalPart = (buffer[2] >> 4) * 10 + (buffer[2] & 0x0F) * 1; float result = wholePart + fractionalPart / 100; -- GitLab