Add magnetometer startup

src/main.cpp

@@ -8,6 +8,7 @@ ServoController controller;
 
 void setup() {
     controller.initServos();
+    controller.initMag();
     LittleFS.begin();
     setup_comm();
 

src/servo.cpp

@@ -1,29 +1,51 @@
 #include "servo.h"
+#include <WebMonitor>
 
 
 void ServoController::initServos() {
     HrzSrv.attach(12,  500, 2500);
-    delay(50);
+    delay(50); // Espera 50ms para o motor se mover
     VrtSrv.attach(13,  500, 2500);
 }
 
-/*void ServoController::initMag() {
+void ServoController::initMag() {
+    while (!mag.begin()) {
+        WebMonitor.println("ERRO: Não foi possível iniciar o magnetometro.");
+        WebMonitor.println("Tentando novamente em 500ms");
+        delay(500);
+    }
 
-}*/
+    if (mag.isQMC()) {
+        WebMonitor.println("Iniciando magnetometro QMC5883L");
         
-void ServoController::updateHeading() {
-    for (int i = 0; i <= 180; i+=5) {
-        HrzSrv.write(i);
-        delay(20);
-        VrtSrv.write(i);
-        delay(200);
+        mag.setRange(QMC5883_RANGE_8GA);
+        WebMonitor.print("Range: ");
+        WebMonitor.println(mag.getRange());
+
+        mag.setMeasurementMode(QMC5883_CONTINOUS);
+        WebMonitor.print("Modo de leitura: ");
+        WebMonitor.println(mag.getMeasurementMode());
+
+        mag.setDataRate(QMC5883_DATARATE_200HZ);
+        WebMonitor.print("Velocidade: ");
+        WebMonitor.println(mag.getDataRate());
+
+        mag.setSamples(QMC5883_SAMPLES_8);
+        WebMonitor.print("Sample: ");
+        WebMonitor.println(mag.getSamples());
+
+        mag.setDeclination(DECLINATION);
+        WebMonitor.print("Declinação: ");
+        WebMonitor.println(DECLINATION);
+    } else {
+        WebMonitor.println("ERRO: Magnetometro encontrado não é suportado.");
+        WebMonitor.println("Suspendendo operação.");
+        while (true);
     }
-    for (int i = 180; i >= 0; i-=5) {
-        HrzSrv.write(i);
-        delay(20);
-        VrtSrv.write(i);
-        delay(200);
 }
+
+void ServoController::updateHeading() {
+    
 }
 
 void ServoController::moveToAzAlt() {

src/servo.h

@@ -1,5 +1,7 @@
-#include <Servo.h>
-#include <DFRobot_QMC5883.h>
+#include <Servo>
+#include <DFRobot_QMC5883>
+
+#define DECLINATION -0.353429173528852
 
 #ifndef SERVO_H
 #define SERVO_H
@@ -10,11 +12,11 @@ class ServoController {
 
         Servo HrzSrv;
         Servo VrtSrv;
-        DFRobot_QMC5883 mag;
+        DFRobot_QMC5883 mag{&Wire, 0xD};
 
     public:
         void initServos();
-        //void initMag(): mag~{&Wire, 0xD};
+        void initMag();
         void moveToAzAlt();
         void moveToRADec();
         void updateHeading();