Skip to content

Commit

Permalink
Merge pull request #7 from MichaKersloot/main
Browse files Browse the repository at this point in the history 
Clarify about (auto) calibration and using the standard radians / degrees functions from arduino.h
  • Loading branch information
@htool
htool authored Aug 20, 2024
2 parents 0c51074 + cddfac2 commit 6f8c311
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 20 deletions.
32 changes: 13 additions & 19 deletions ESP32_Precision-9_compass_CMPS14.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -254,21 +254,21 @@ void loop()
compassError = true; // no heading for now
if (n2kConnected && send_heading && compassError == false) {
if (send_heading_true) {
SetN2kPGN127250(N2kMsg, SID, heading_true * DEG_TO_RAD, N2kDoubleNA, N2kDoubleNA, N2khr_true);
SetN2kPGN127250(N2kMsg, SID, radians(heading_true), N2kDoubleNA, N2kDoubleNA, N2khr_true);
nmea2000->SendMsg(N2kMsg);
} else {
SetN2kPGN127250(N2kMsg, SID, heading_mag * DEG_TO_RAD, N2kDoubleNA, N2kDoubleNA, N2khr_magnetic);
SetN2kPGN127250(N2kMsg, SID, radians(heading_mag), N2kDoubleNA, N2kDoubleNA, N2khr_magnetic);
nmea2000->SendMsg(N2kMsg);
}
}
if (gyroError == false) {
SetN2kRateOfTurn(N2kMsg, SID, gyroZ * DEG_TO_RAD * -1); // radians
SetN2kRateOfTurn(N2kMsg, SID, radians(gyroZ) * -1); // radians
nmea2000->SendMsg(N2kMsg);
}
send_heading = !send_heading; // Toggle to do 10hz

if (n2kConnected && pitchrollError == false) {
SetN2kAttitude(N2kMsg, SID, N2kDoubleNA, pitch * DEG_TO_RAD, roll * DEG_TO_RAD);
SetN2kAttitude(N2kMsg, SID, N2kDoubleNA, radians(pitch), radians(roll));
nmea2000->SendMsg(N2kMsg);
}

Expand Down Expand Up @@ -378,7 +378,7 @@ void HandleNMEA2000Msg(const tN2kMsg &N2kMsg) {
uint16_t DaysSince1970;
double variation;
ParseN2kMagneticVariation(N2kMsg, SID, source, DaysSince1970, variation);
heading_variation = (float)variation * RAD_TO_DEG;
heading_variation = degrees((float)variation);
send_heading_true = true;
break;
case 130845L:
Expand Down Expand Up @@ -441,7 +441,7 @@ bool ParseN2kPGN130845(const tN2kMsg &N2kMsg, uint16_t &Key, uint16_t &Command,
switch (Key) {
case 0x0000: // Heading offset
heading_offset_rad = N2kMsg.Get2ByteDouble(0.0001, Index);
heading_offset_deg = (int8_t)round(heading_offset_rad * RAD_TO_DEG);
heading_offset_deg = (int8_t)round(degrees(heading_offset_rad));
if (heading_offset_deg > 127) {
heading_offset_deg = heading_offset_deg - 360;
}
Expand All @@ -451,7 +451,7 @@ bool ParseN2kPGN130845(const tN2kMsg &N2kMsg, uint16_t &Key, uint16_t &Command,
break;
case 0x0039: // Heel offset
heel_offset_rad = N2kMsg.Get2ByteDouble(0.0001, Index);
heel_offset_deg = (int8_t)round(heel_offset_rad * RAD_TO_DEG);
heel_offset_deg = (int8_t)round(degrees(heel_offset_rad));
if (heel_offset_deg > 127) {
heel_offset_deg = heel_offset_deg - 360;
}
Expand All @@ -461,7 +461,7 @@ bool ParseN2kPGN130845(const tN2kMsg &N2kMsg, uint16_t &Key, uint16_t &Command,
break;
case 0x0031: // Trim offset
trim_offset_rad = N2kMsg.Get2ByteDouble(0.0001, Index);
trim_offset_deg = (int8_t)round(trim_offset_rad * RAD_TO_DEG);
trim_offset_deg = (int8_t)round(degrees(trim_offset_rad));
if (trim_offset_deg > 127) {
trim_offset_deg = trim_offset_deg - 360;
}
Expand Down Expand Up @@ -604,10 +604,10 @@ bool SetN2kPGN130824(tN2kMsg &N2kMsg) {
N2kMsg.AddByte(0x99); // Reserved
N2kMsg.AddByte(0x9e); // 9e,20 Pitch rate
N2kMsg.AddByte(0x20); // 9e,20 Pitch rate
N2kMsg.Add2ByteDouble(pitch * DEG_TO_RAD, 0.0001);
N2kMsg.Add2ByteDouble(radians(pitch), 0.0001);
N2kMsg.AddByte(0x3c); // 3c,20 Roll rate
N2kMsg.AddByte(0x20); // 3c,20 Roll rate
N2kMsg.Add2ByteDouble(roll * DEG_TO_RAD, 0.0001);
N2kMsg.Add2ByteDouble(radians(roll), 0.0001);
return true;
}

Expand Down Expand Up @@ -739,36 +739,30 @@ void readCalibration() {
Serial.println("\n[Saved settings]");
Serial.print("heading_offset_deg : ");
heading_offset_deg = (int8_t)EEPROM.readByte(EEP_HEADING_OFFSET);
heading_offset_rad = degToRad(heading_offset_deg);
heading_offset_rad = radians(heading_offset_deg);
Serial.printf("%d %f\n", heading_offset_deg, heading_offset_rad);

Serial.print("heel_offset_deg : ");
heel_offset_deg = (int8_t)(EEPROM.readByte(EEP_HEEL_OFFSET));
heel_offset_rad = degToRad(heel_offset_deg);
heel_offset_rad = radians(heel_offset_deg);
Serial.printf("%d %f\n", heel_offset_deg, heel_offset_rad);

Serial.print("trim_offset_deg : ");
trim_offset_deg = (int8_t)(EEPROM.readByte(EEP_TRIM_OFFSET));
trim_offset_rad = degToRad(trim_offset_deg);
trim_offset_rad = radians(trim_offset_deg);
Serial.printf("%d %f\n", trim_offset_deg, trim_offset_rad);
Serial.print("autocalibration : ");
compass_autocalibration = EEPROM.readByte(EEP_AUTOCALIBRATION);
Serial.println(compass_autocalibration);
configureAutoCalibration(compass_autocalibration);
}

float degToRad (int deg) {
float rad = deg * DEG_TO_RAD;
return rad;
}

void printBytes()
{
for (size_t i = 0; i < EEPROM_SIZE; ++i)
Serial.println(EEPROM.readByte(i), HEX);
}


float getPitch16()
{
// Begin communication with CMPS14
Expand Down
27 changes: 26 additions & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@ Digital compass acting as B&amp;G Precision 9 based on ESP32 and CMPS14

## ESP32 board
To have easy access to N2k [this board is recommended](https://hatlabs.github.io/sh-esp32/).
There is a part on the board for prototyping. You can use this to add the CMPS14 directly on the board

## Goal
- A highly accurate compass which also reports roll, pitch, rate of turn and possibly heave.
Expand All @@ -12,6 +13,7 @@ To have easy access to N2k [this board is recommended](https://hatlabs.github.io
After trying the MPU9250 first, Iwasn't happy with the noise level. The CMPS14 seems more stable and makes the code way easier.

## Status

### What's working
- Recognised as B&G Precision-9.
- Heading send out as Vessel Heading PGN 127250 as Magnetic at 10Hz.
Expand All @@ -20,10 +22,33 @@ After trying the MPU9250 first, Iwasn't happy with the noise level. The CMPS14 s
- Rate of turn is send out as PGN 127251 at 10Hz.
- MFD configuration of heading, trim and heel offset.
- Offsets are stored in EEPROM when set.
- Auto calibration mode is recognised,
- Off, means.... Off, no autocalibration
- On, means Autocalibration on for Gyro, Accelerometer and Magnetometer. Deletes stored calibration profile
- Locked, means Off, no autocalibration. Save calibration profile
- Auto, means Autocalibration off for Gyro and Accelerometer. On for Magnetometer

### calibration

Normaly the CMPS14 is calibrated in the factory. But it can be usefull to calibrate the device on (or near) the spot you want to use it.
The procedure is:

- Connect your device to your computer and watch the serial port
- Set Autocalibration mode on 'On' with the 'c' key
- Type 'o' to enable output
- Watch: [Calibration] 11001111 . In the ideal situation it wil look like written. But before calibration there could be more 0 and less 1'Save
- Put your device on different sides, each for about 1 second. Keep your device steady for that second.
- Set the device on minimal 4 sides.
- Watch the Calibration and continue untill the fifth and sixth position shows a 1 (Accelerometer)
- Then slowly rotate the device in all three axis. 180 degrees and back in about 2 to 3 seconds.
- Watch the Calibration and continue untill the seventh and eigth position shows a 1 (Magnetometer)
- When done calibrating, press 'o' to stop the output
- Then press 'c' to change calibration to 'Locked' to save the calibration profile.
- Then press 'c' again to change calibration to 'auto'
- You can now place your device on it's final spot.

### What's half working
- Deviation calibration routine. The stop/finsihed signal works, but processing the outcome is still missing. This routine process should build a boat specific deviation table.
- Auto calibration mode is recognised, but not used yet.

### What's not working
- Actual creation of the deviation table, which from what I understand can be done using Fourier transformation on the data from the 390 deg circle.
Expand Down

0 comments on commit 6f8c311

Please sign in to comment.