improve serial to 500000 baud

barometer calibration
berryerlouis · Oct 6, 2024 · a6fb708 · a6fb708
1 parent ec122bc
commit a6fb708
Show file tree

Hide file tree

Showing 22 changed files with 216 additions and 66 deletions.
diff --git a/.gitignore b/.gitignore
@@ -2,3 +2,8 @@
 build
 HMI/node_modules
 HMI/coverage
+*.o
+*.d
+*.atsuo
+src/debug/
+node_modules/
diff --git a/HMI/js/clusters/db.js b/HMI/js/clusters/db.js
@@ -29,10 +29,13 @@ export let clustersDatabase = {
         GYR: {x: 0, y: 0, z: 0},
         MAG: {x: 0, y: 0, z: 0},
         TMP: "0",
-        YAWPITCHROLL: {x: 0, y: 0, z: 0},
+        YAWPITCHROLL: {yaw: 0, pitch: 0, roll: 0},
         PRESSURE: "0",
         ALTITUDE: "0",
-        TMPBAR: "0"
+        TMPBAR: "0",
+        CALIB_MAG_MIN: {x: 0, y: 0, z: 0},
+        CALIB_MAG_MAX: {x: 0, y: 0, z: 0},
+        STARTSTOPMAGCALIB: false,
     },
     PROXIMITY: {
         US_LEFT: 0,

diff --git a/HMI/js/clusters/imu/ClusterImu.js b/HMI/js/clusters/imu/ClusterImu.js
@@ -157,20 +157,20 @@ export class ClusterImu {
                 this.databaseManager.updateDb({
                     cluster: 'IMU',
                     command: 'YAWPITCHROLL',
-                    item: 'x',
+                    item: 'roll',
                     value: message.fetchInt16S()
                 });
                 this.databaseManager.updateDb({
                     cluster: 'IMU',
                     command: 'YAWPITCHROLL',
-                    item: 'y',
+                    item: 'pitch',
                     value: message.fetchInt16S()
                 });
                 let angle = message.fetchInt16S();
                 this.databaseManager.updateDb({
                     cluster: 'IMU',
                     command: 'YAWPITCHROLL',
-                    item: 'z',
+                    item: 'yaw',
                     value: angle
                 });
                 this.compass.rotate(angle);
@@ -210,6 +210,41 @@ export class ClusterImu {
                 this.temperatureHMI[0].innerText = val;
             }
         });
+        this.messageManager.addCallbackNotifyOnSpecificCommand(ClusterName.IMU, CommandImu.CALIB_MAG_MIN_MAX, (message) => {
+            if (message.size > 0) {
+                let min = message.fetchInt8U();
+                let x = (message.fetchInt32S() / 100).toFixed(2);
+                let y = (message.fetchInt32S() / 100).toFixed(2);
+                let z = (message.fetchInt32S() / 100).toFixed(2);
+                this.databaseManager.updateDb({
+                    cluster: 'IMU',
+                    command: min === 0 ? 'CALIB_MAG_MIN' : 'CALIB_MAG_MAX',
+                    item: 'x',
+                    value: x
+                });
+                this.databaseManager.updateDb({
+                    cluster: 'IMU',
+                    command: min === 0 ? 'CALIB_MAG_MIN' : 'CALIB_MAG_MAX',
+                    item: 'y',
+                    value: y
+                });
+                this.databaseManager.updateDb({
+                    cluster: 'IMU',
+                    command: min === 0 ? 'CALIB_MAG_MIN' : 'CALIB_MAG_MAX',
+                    item: 'z',
+                    value: z
+                });
+            }
+        });
+        this.messageManager.addCallbackNotifyOnSpecificCommand(ClusterName.IMU, CommandImu.START_STOP_MAG_CALIB, (message) => {
+            if (message.size > 0) {
+                this.databaseManager.updateDb({
+                    cluster: 'IMU',
+                    command: 'STARTSTOPMAGCALIB',
+                    value: message.fetchInt8U() === 1
+                });
+            }
+        });
     }
 }
 
diff --git a/HMI/js/main.js b/HMI/js/main.js
@@ -8,9 +8,9 @@ import {DatabaseManager} from "./clusters/DatabaseManager.js";
 import Hexapod from "./ui/hexapod/hexapod.js";
 import Controls from "./ui/gui/Controls.js";
 import Walk from "./ui/walk/Walk.js";
-import {Message} from "./protocol/Message.js";
-import {ClusterName, CommandBattery, CommandGeneral, CommandImu, CommandServo} from "./protocol/Cluster.js";
 import Compass from "./ui/Compass.js";
+import {ClusterName, CommandBattery, CommandGeneral, CommandImu, CommandServo} from "./protocol/Cluster.js";
+import {Message} from "./protocol/Message.js";
 
 
 const serialInterface = new SerialInterface();

diff --git a/HMI/js/protocol/Cluster.js b/HMI/js/protocol/Cluster.js
@@ -52,7 +52,9 @@ const CommandImu = {
     YAWPITCHROLL: 'YAWPITCHROLL',
     PRESSURE: 'PRESSURE',
     ALTITUDE: 'ALTITUDE',
-    TMPBAR: 'TMPBAR'
+    TMPBAR: 'TMPBAR',
+    CALIB_MAG_MIN_MAX: 'CALIBMAGMINMAX',
+    START_STOP_MAG_CALIB: 'STARTSTOPMAGCALIB',
 };
 
 const CommandBody = {
@@ -112,7 +114,9 @@ export default class Clusters {
                 new Command(CommandImu.YAWPITCHROLL, '05'),
                 new Command(CommandImu.PRESSURE, '06'),
                 new Command(CommandImu.ALTITUDE, '07'),
-                new Command(CommandImu.TMPBAR, '08')
+                new Command(CommandImu.TMPBAR, '08'),
+                new Command(CommandImu.CALIB_MAG_MIN_MAX, '09'),
+                new Command(CommandImu.START_STOP_MAG_CALIB, '0A')
             ]
         ),
         new Cluster(

diff --git a/HMI/js/protocol/Serial.js b/HMI/js/protocol/Serial.js
@@ -17,7 +17,7 @@ export class SerialInterface {
             const filters = [{usbVendorId: 0x10C4, usbProductId: 0xEA60}];
             try {
                 this.port = await nav.serial.requestPort({filters});
-                await this.port.open({baudRate: 115200});
+                await this.port.open({baudRate: 500000});
 
                 const textEncoder = new TextEncoderStream();
                 this.writableStreamClosed = textEncoder.readable.pipeTo(this.port.writable);
@@ -57,6 +57,9 @@ export class SerialInterface {
 
     async write(message) {
         if (this.writer) {
+            let date = new Date();
+            date = new Date(date.getTime() - date.getTimezoneOffset() * 60000).toISOString().replace('T', ' ').replace('Z', ' ');
+            console.log(date + " => " + message.raw)
             await this.writer.write(message.raw);
             this.notifyWrite(message);
             return true;
@@ -124,6 +127,9 @@ export class SerialInterface {
                     }
 
                     if (value) {
+                        let date = new Date();
+                        date = new Date(date.getTime() - date.getTimezoneOffset() * 60000).toISOString().replace('T', ' ').replace('Z', ' ');
+                        console.log(date + " => " + value)
                         this.catchIncomingMessage(value.replace('\n', ''));
                     }
                 }

diff --git a/HMI/js/ui/gui/ClusterCommand.js b/HMI/js/ui/gui/ClusterCommand.js
@@ -38,10 +38,12 @@ export default class ClusterCommand {
         this.addClusterManyCommands(clusterFolder, ClusterName.IMU, CommandImu.GYR, ['x', 'y', 'z']);
         this.addClusterManyCommands(clusterFolder, ClusterName.IMU, CommandImu.MAG, ['x', 'y', 'z']);
         this.addClusterSimpleCommand(clusterFolder, ClusterName.IMU, CommandImu.TMP);
-        this.addClusterManyCommands(clusterFolder, ClusterName.IMU, CommandImu.YAWPITCHROLL, ['x', 'y', 'z']);
+        this.addClusterManyCommands(clusterFolder, ClusterName.IMU, CommandImu.YAWPITCHROLL, ['yaw', 'pitch', 'roll']);
         this.addClusterSimpleCommand(clusterFolder, ClusterName.IMU, CommandImu.PRESSURE);
         this.addClusterSimpleCommand(clusterFolder, ClusterName.IMU, CommandImu.ALTITUDE);
         this.addClusterSimpleCommand(clusterFolder, ClusterName.IMU, CommandImu.TMPBAR);
+        this.addClusterManyCommands(clusterFolder, ClusterName.IMU, "CALIB_MAG_MIN", ['x', 'y', 'z']);
+        this.addClusterManyCommands(clusterFolder, ClusterName.IMU, "CALIB_MAG_MAX", ['x', 'y', 'z']);
 
 
         //ClusterName.PROXIMITY

diff --git a/src/Cluster/ClusterBase.h b/src/Cluster/ClusterBase.h
@@ -41,6 +41,7 @@ namespace Cluster
                 if (this->mClusterCommands[i].commandId == request.commandId
                     && this->mClusterCommands[i].expectedSize == request.nbParams) {
                     success = this->ExecuteFrame(request, response);
+					break;
                 }
             }
             return success;

diff --git a/src/Cluster/Constants.h b/src/Cluster/Constants.h
@@ -51,10 +51,12 @@ namespace Cluster
         YAW_PITCH_ROLL,
         PRESSURE,
         ALTITUDE,
-        TMP_BAR
+        TMP_BAR,
+		CALIB_MAG_MIN_MAX,
+		START_STOP_MAG_CALIB,
     };
 
-#define NB_COMMANDS_IMU 9U
+#define NB_COMMANDS_IMU 11U
 
     enum EServoCommands {
         GET_ALL = 0,

diff --git a/src/Cluster/Decoding/Protocol.cpp b/src/Cluster/Decoding/Protocol.cpp
@@ -47,7 +47,7 @@ namespace Cluster
         }
 
         uint8_t Protocol::Encode(const Frame &response, const char *buffer) {
-            if (buffer == nullptr || response.nbParams == 0) {
+            if (buffer == nullptr) {
                 return (0U);
             }
             const uint8_t *params = response.params;

diff --git a/src/Cluster/Imu/ClusterImu.h b/src/Cluster/Imu/ClusterImu.h
@@ -29,6 +29,10 @@ namespace Cluster
                 this->AddClusterItem((ClusterItem){.commandId = EImuCommands::PRESSURE, .expectedSize = 0U});
                 this->AddClusterItem((ClusterItem){.commandId = EImuCommands::ALTITUDE, .expectedSize = 0U});
                 this->AddClusterItem((ClusterItem){.commandId = EImuCommands::TMP_BAR, .expectedSize = 0U});
+                this->AddClusterItem((ClusterItem){
+                    .commandId = EImuCommands::START_STOP_MAG_CALIB, .expectedSize = 1U
+                });
+                this->AddClusterItem((ClusterItem){.commandId = EImuCommands::CALIB_MAG_MIN_MAX, .expectedSize = 1U});
             }
 
             ~ClusterImu() = default;
@@ -65,6 +69,14 @@ namespace Cluster
                 } else if (request.commandId == EImuCommands::TMP_BAR) {
                     const int16_t temp = this->mBarometer.GetTemp();
                     success = this->BuildFrameTmpBar(temp, response);
+                } else if (request.commandId == EImuCommands::CALIB_MAG_MIN_MAX) {
+                    const bool min = request.Get1ByteParam(0U);
+                    const Vector3F calib = this->mImu.ReadCalibrationMag(min);
+                    success = this->BuildFrameCalibMag(min, calib, response);
+                } else if (request.commandId == EImuCommands::START_STOP_MAG_CALIB) {
+                    const bool start = request.Get1ByteParam(0U);
+                    this->mImu.StartCalibrationMag(start);
+                    success = this->BuildFrameStartCalibMag(response);
                 }
                 return success;
             }
@@ -167,6 +179,23 @@ namespace Cluster
                 return (success);
             }
 
+            inline Core::CoreStatus BuildFrameCalibMag(const bool min, const Vector3F &calib, Frame &response) const {
+                const Core::CoreStatus success = response.Build(
+                    EClusters::IMU,
+                    EImuCommands::YAW_PITCH_ROLL);
+                if (success == Core::CoreStatus::CORE_OK) {
+                    response.Set1ByteParam(min);
+                    response.SetxBytesParam(12U, (uint8_t *) &calib);
+                }
+                return (success);
+            }
+
+            inline Core::CoreStatus BuildFrameStartCalibMag(Frame &response) const {
+                return response.Build(
+                    EClusters::IMU,
+                    EImuCommands::START_STOP_MAG_CALIB);
+            }
+
         private:
             Mpu9150Interface &mImu;
             BarometerInterface &mBarometer;

diff --git a/src/Component/Communication/Communication.cpp b/src/Component/Communication/Communication.cpp
@@ -78,7 +78,7 @@ namespace Component
         }
 
         bool Communication::ReceivedStringFrame(void) {
-            if (this->mUart.DataAvailable() > 0U) {
+            while (this->mUart.DataAvailable() > 0U) {
                 const uint8_t rc = this->mUart.Read();
                 if (REJECT_UNKNOWN_CHARACTER(rc)) {
                     if (rc == '<') {

diff --git a/src/Component/Imu/Ahrs.cpp b/src/Component/Imu/Ahrs.cpp
@@ -23,7 +23,8 @@ namespace Component
                 this->mQuaternion[0] * this->mQuaternion[0] - this->mQuaternion[1] * this->mQuaternion[1] - this->
                 mQuaternion[2] * this->mQuaternion[2] + this->mQuaternion[3] * this->mQuaternion[3]);
 
-            ypr.yaw *= 180.0F / M_PI - 2.46F; // Declination Chirens
+            ypr.yaw *= 180.0F / M_PI - 2.76F;
+            //2° 47' E  ± 0° 22 Declination Chirens declination_offset = degrees   minutes / 60
             ypr.pitch *= 180.0F / M_PI;
             ypr.roll *= 180.0F / M_PI;
         }