diff --git a/pico/inc/devices/compass.hpp b/pico/inc/devices/compass.hpp
index 6632229eb903ccbb1e9d6a87e6a0a4e4c8f3297f..2cd29c8c0d5bcf4cefb0e3b372c6083e7dc73f59 100644
--- a/pico/inc/devices/compass.hpp
+++ b/pico/inc/devices/compass.hpp
@@ -7,21 +7,6 @@
#include "debug.hpp"
-// Compass I2C configuration
-#define COMPASS_ADDR 0x1E
-#define CONFIG_A 0x00
-#define CONFIG_B 0x01
-#define MODE_REG 0x02
-#define DATA_REG 0x03
-
-// Conversion from raw value to microteslas (uT)
-#define TO_UT (100.0 / 1090.0)
-
-// I2C Pins and Instance
-#define I2C_SCL_PIN 17
-#define I2C_SDA_PIN 16
-#define I2C_PORT i2c0
-
struct CalibrationMaxValue {
float X;
float Y;
@@ -36,13 +21,16 @@ struct CalibrationMinValue {
class Compass {
public:
- Compass();
+ Compass(uint SCL_PIN, uint SDA_PIN, i2c_inst_t* I2C_PORT);
void init();
void readRawData(int16_t &x, int16_t &y, int16_t &z);
void calibrate();
float getHeading();
private:
+ uint SCL_PIN;
+ uint SDA_PIN;
+ i2c_inst_t* I2C_PORT;
float xRawValueOffset;
float yRawValueOffset;
float zRawValueOffset;
-};
\ No newline at end of file
+};
diff --git a/pico/src/devices/compass.cpp b/pico/src/devices/compass.cpp
index 61682d2a1e57e9445f17e2a3180545affa471f16..32cf864cf27d002654e6d02ed267ce778514bed8 100644
--- a/pico/src/devices/compass.cpp
+++ b/pico/src/devices/compass.cpp
@@ -1,17 +1,30 @@
#include "compass.hpp"
#include "pico/stdlib.h"
-Compass::Compass() {
- // Initialize I2C communication
- i2c_init(I2C_PORT, 400000); // 400 kHz
- gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
- gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
- gpio_pull_up(I2C_SDA_PIN);
- gpio_pull_up(I2C_SCL_PIN);
-}
+#include "debug.hpp"
+
+// Compass I2C configuration
+#define COMPASS_ADDR 0x1E
+#define CONFIG_A 0x00
+#define CONFIG_B 0x01
+#define MODE_REG 0x02
+#define DATA_REG 0x03
+
+// Conversion from raw value to microteslas (uT)
+#define TO_UT (100.0 / 1090.0)
+
+Compass::Compass(uint SCL_PIN_VAL, uint SDL_PIN_VAL, i2c_inst_t *I2C_PORT_VAL)
+ : SCL_PIN(SCL_PIN_VAL), SDA_PIN(SDL_PIN_VAL), I2C_PORT(I2C_PORT_VAL) {}
// Initialize the compass
void Compass::init() {
+ // Initialize I2C communication
+ i2c_init(I2C_PORT, 400000); // 400 kHz
+ gpio_set_function(SDA_PIN, GPIO_FUNC_I2C);
+ gpio_set_function(SCL_PIN, GPIO_FUNC_I2C);
+ gpio_pull_up(SDA_PIN);
+ gpio_pull_up(SCL_PIN);
+
uint8_t config_a[2] = {CONFIG_A, 0x70}; // Configuration for CONFIG_A
uint8_t config_b[2] = {CONFIG_B, 0xa0}; // Configuration for CONFIG_B
@@ -23,14 +36,14 @@ void Compass::init() {
// Read raw data from the compass
void Compass::readRawData(int16_t &x, int16_t &y, int16_t &z) {
- uint8_t buf[2] = {0x02, 0x01};
+ uint8_t buf[2] = {MODE_REG, CONFIG_B};
uint8_t data[6];
i2c_write_blocking(I2C_PORT, COMPASS_ADDR, buf, 2, false);
sleep_ms(10);
// Request data
- buf[0] = 0x03;
+ buf[0] = DATA_REG;
i2c_write_blocking(I2C_PORT, COMPASS_ADDR, buf, 1, false);
i2c_read_blocking(I2C_PORT, COMPASS_ADDR, data, 6, false);
@@ -53,56 +66,55 @@ void Compass::calibrate() {
int16_t x, y, z;
- while(xCount < 3 || yCount < 3 || zCount < 3) {
+ DEBUG("Calibrate the compass\r\n");
+
+ while (xCount < 3 || yCount < 3 || zCount < 3) {
readRawData(x, y, z);
- if ((fabs(x) > 654) || (fabs(x) > 654) || (fabs(x) > 654))
- continue;
+ if ((std::fabs(x) > 600) || (std::fabs(y) > 600) || (std::fabs(z) > 600)) continue;
if (minValue.X > x) {
minValue.X = x;
- } else if (maxValue.X < x)
+ } else if (maxValue.X < x) {
maxValue.X = x;
+ }
if (minValue.Y > y) {
minValue.Y = y;
- } else if (maxValue.Y < y)
+ } else if (maxValue.Y < y) {
maxValue.Y = y;
+ }
if (minValue.Z > z) {
minValue.Z = z;
- } else if (maxValue.Z < z)
+ } else if (maxValue.Z < z) {
maxValue.Z = z;
-
-
+ }
if (xRotationFlag) {
- if (fabs(x) > 55) {
+ if (std::fabs(x) > 50) {
xRotationFlag = false;
xCount++;
}
} else {
- if (fabs(x) < 44)
- xRotationFlag = true;
+ if (std::fabs(x) < 40) { xRotationFlag = true; }
}
if (yRotationFlag) {
- if (fabs(y) > 55) {
+ if (std::fabs(y) > 50) {
yRotationFlag = false;
yCount++;
}
} else {
- if (fabs(y) < 44)
- yRotationFlag = true;
+ if (std::fabs(y) < 40) { yRotationFlag = true; }
}
if (zRotationFlag) {
- if (fabs(z) > 55) {
+ if (std::fabs(z) > 50) {
zRotationFlag = false;
zCount++;
}
} else {
- if (fabs(z) < 44)
- zRotationFlag = true;
+ if (std::fabs(z) < 40) { zRotationFlag = true; }
}
sleep_ms(30);
@@ -127,27 +139,21 @@ float Compass::getHeading() {
// Convert raw values to microtesla
float x_uT = x * TO_UT;
float y_uT = y * TO_UT;
- float z_uT = z * TO_UT;
+ //float z_uT = z * TO_UT;
// Calculate heading
float heading = atan2(y_uT, x_uT);
- //This is the declination angle in radians, converted from +10* 16'. It is taken around Helsinki / Vantaa.
+ // This is the declination angle in radians, converted from +10* 16'. It is taken around Helsinki / Vantaa.
float declinationAngle = 0.18;
heading += declinationAngle;
- if (heading < 0)
- heading += 2*M_PI;
-
- if (heading > 2*M_PI) {
- heading -= 2*M_PI;
- }
+ if (heading < 0) heading += 2 * M_PI;
- //TODO replace *heading* with this on the *headingDegrees* for vertical angle stuff
- float test = atan2(z, sqrt(x * x + y * y));
+ if (heading > 2 * M_PI) { heading -= 2 * M_PI; }
- //convert radians to degrees
- float headingDegrees = heading * 180/M_PI;
+ // convert radians to degrees
+ float headingDegrees = heading * 180 / M_PI;
return headingDegrees;
-}
\ No newline at end of file
+}
diff --git a/pico/src/test_main.cpp b/pico/src/test_main.cpp
index c66213b21651bcd66ba5230929d680efff04f591..4b1030330abcffa87e9ec4c31469de074085dce6 100644
--- a/pico/src/test_main.cpp
+++ b/pico/src/test_main.cpp
@@ -2,6 +2,7 @@
#include "convert.hpp"
#include "devices/gps.hpp"
#include "hardware/clock.hpp"
+#include "devices/compass.hpp"
#include "message.hpp"
#include "pico/stdlib.h"
#include "uart/PicoUart.hpp"
@@ -17,12 +18,15 @@
int main() {
stdio_init_all();
sleep_ms(5000);
+ DEBUG("Start\r\n");
auto queue = std::make_shared<std::queue<msg::Message>>();
auto uart_0 = std::make_shared<PicoUart>(0, 0, 1, 9600);
auto uart_1 = std::make_shared<PicoUart>(1, 4, 5, 9600);
auto clock = std::make_shared<Clock>();
auto gps = std::make_unique<GPS>(uart_1, false, true);
+ Compass compass(21, 20, i2c0);
+ compass.init();
CommBridge bridge(uart_0, queue);
@@ -30,6 +34,7 @@ int main() {
gps->set_mode(GPS::Mode::FULL_ON);
bool fix = false;
+ float heading = 0;
for (;;) {
Coordinates coords = gps->get_coordinates();
if (coords.status) {
@@ -40,9 +45,12 @@ int main() {
gps->set_mode(GPS::Mode::STANDBY);
}
} else if (!fix) {
- gps->locate_position(15);
+ gps->locate_position(5);
}
+ heading = compass.getHeading();
+ DEBUG("Heading: ", heading);
+
DEBUG("Received wakeup signal\r\n");
bridge.read_and_parse(10000, true);
@@ -63,6 +71,10 @@ int main() {
break;
case msg::INSTRUCTIONS:
DEBUG("Received instructions");
+ for (auto it = msg.content.begin(); it != msg.content.end(); ++it) {
+ DEBUG("Instruction: ", *it);
+ }
+ bridge.send(msg::response(true));
break;
case msg::PICTURE: // Pico should not receive these
DEBUG("Received picture msg for some reason");