Telemetry is going again via Firmata.

firmware/src/firmata_commands.hpp

@@ -11,6 +11,7 @@ enum FirmataTelemetryType
 {
   GPS = 0,
   IMU,
+  POWER
 };
 
 #endif // __FIRMATA_COMMANDS_HPP__

firmware/src/main.cpp

@@ -15,6 +15,7 @@ extern "C"
 #include "tasks/firmata_task.hpp"
 #include "tasks/gps_task.hpp"
 #include "tasks/imu_task.hpp"
+#include "tasks/power_task.hpp"
 
 #define SERVO_ENABLE_PORT_PIN 2
 #define SERVO_ENABLE_STARBOARD_PIN 3
@@ -33,11 +34,13 @@ void setup()
   FirmataTask firmataTask;
   GpsTask gpsTask(firmataTask);
   ImuTask imuTask(i2c0Mutex, firmataTask);
+  PowerTask powerTask(i2c0Mutex, firmataTask);
 
   xTaskCreate(BlinkerTask::startTask, "Blinker", 2048, (void *)&blinkerTask, 1, NULL);
   xTaskCreate(FirmataTask::startTask, "Firmata", 2048, (void *)&firmataTask, 2, NULL);
   xTaskCreate(GpsTask::startTask, "GPS", 2048, (void *)&gpsTask, 3, NULL);
-  xTaskCreate(ImuTask::startTask, "IMU", 2048, (void *)&imuTask, 4, NULL);
+  xTaskCreate(PowerTask::startTask, "Power", 2048, (void *)&powerTask, 4, NULL);
+  xTaskCreate(ImuTask::startTask, "IMU", 2048, (void *)&imuTask, 5, NULL);
   vTaskStartScheduler();
 }
 

firmware/src/tasks/gps_task.cpp

@@ -32,8 +32,15 @@ void GpsTask::run()
       this->lastSample = newSample;
       this->sampleReady = true;
 
-      byte *data = (byte *)&this->lastSample;
-      this->firmata.sendTelemetry(FirmataTelemetryType::GPS, sizeof(this->lastSample), data);
+      byte *data[22];
+      data[0] = (byte *)&newSample.latitude;
+      data[4] = (byte *)&newSample.longitude;
+      data[8] = (byte *)&newSample.altitude_cm;
+      data[12] = (byte *)&newSample.speed_kph;
+      data[16] = (byte *)&newSample.heading;
+      data[20] = (byte *)&newSample.satellites;
+      data[21] = (byte *)&newSample.status;
+      this->firmata.sendTelemetry(FirmataTelemetryType::GPS, 22, *data);
     }
     vTaskDelay(delay);
   }

firmware/src/tasks/imu_task.cpp

@@ -29,7 +29,7 @@ void ImuTask::run()
       imu::Vector<3> gyroscope = this->imu.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
       imu::Vector<3> gravity = this->imu.getVector(Adafruit_BNO055::VECTOR_GRAVITY);
       imu::Vector<3> linearAcceleration = this->imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
-      byte temperature = this->imu.getTemp();
+      int8_t temperature = this->imu.getTemp();
       xSemaphoreGive(this->busMutex);
 
       this->lastSample.orientation = {orientation.x(), orientation.y(), orientation.z(), orientation.w()};
@@ -41,8 +41,14 @@ void ImuTask::run()
       this->lastSample.temperature = temperature;
       this->sampleReady = true;
 
-      byte *data = (byte *)&this->lastSample;
-      this->firmata.sendTelemetry(FirmataTelemetryType::IMU, sizeof(this->lastSample), data);
+      // Only sending the orientation and temperature to the telemetry for now.
+      byte *data[33];
+      data[0] = (byte *)&this->lastSample.orientation.x;
+      data[8] = (byte *)&this->lastSample.orientation.y;
+      data[16] = (byte *)&this->lastSample.orientation.z;
+      data[24] = (byte *)&this->lastSample.orientation.w;
+      data[32] = (byte *)&this->lastSample.temperature;
+      this->firmata.sendTelemetry(FirmataTelemetryType::IMU, 33, *data);
 
       vTaskDelay(delay);
     }

firmware/src/tasks/power_sample.hpp

@@ -0,0 +1,12 @@
+#ifndef __POWER_SAMPLE__HPP__
+#define __POWER_SAMPLE__HPP__
+
+typedef struct
+{
+  float busVoltage;
+  float shuntVoltage;
+  float current;
+  float power;
+} PowerSample;
+
+#endif // __POWER_SAMPLE__HPP__

firmware/src/tasks/power_task.cpp

@@ -0,0 +1,71 @@
+#include <FreeRTOS_TEENSY4.h>
+#include "power_task.hpp"
+#include "ticks.hpp"
+
+PowerTask::PowerTask(SemaphoreHandle_t &busMutex, FirmataTask &firmata) : busMutex(busMutex), firmata(firmata), sensor(POWER_ADDRESS)
+{
+  this->lastSample = {0.0, 0.0, 0.0, 0.0};
+  this->sampleReady = false;
+}
+
+void PowerTask::init()
+{
+  TickType_t delay = Ticks::fromMillis(100L);
+  while (1)
+  {
+    if (xSemaphoreTake(this->busMutex, delay) == pdTRUE)
+    {
+      this->sensor.begin();
+      this->sensor.setCalibration_32V_2A();
+      xSemaphoreGive(this->busMutex);
+      break;
+    }
+    vTaskDelay(delay);
+  }
+}
+
+void PowerTask::run()
+{
+  long delay = Ticks::fromHz((long)POWER_UPDATE_HZ);
+  long lockWaitDelay = Ticks::fromMillis(100L);
+  while (1)
+  {
+    if (xSemaphoreTake(this->busMutex, lockWaitDelay) == pdTRUE)
+    {
+      float busVoltage = this->sensor.getBusVoltage_V();
+      float shuntVoltage = this->sensor.getShuntVoltage_mV();
+      float current = this->sensor.getCurrent_mA();
+      float power = this->sensor.getPower_mW();
+      xSemaphoreGive(this->busMutex);
+      this->lastSample = {busVoltage, shuntVoltage, current, power};
+      this->sampleReady = true;
+
+      byte *data[16];
+      data[0] = (byte *)&busVoltage;
+      data[4] = (byte *)&shuntVoltage;
+      data[8] = (byte *)&current;
+      data[12] = (byte *)&power;
+      this->firmata.sendTelemetry(FirmataTelemetryType::POWER, 16, *data);
+
+      vTaskDelay(delay);
+    }
+  }
+}
+
+void PowerTask::startTask(void *arg)
+{
+  PowerTask *powerTask = (PowerTask *)arg;
+  powerTask->init();
+  vTaskDelay(Ticks::fromMillis(100L));
+  powerTask->run();
+}
+
+bool PowerTask::hasSample()
+{
+  return this->sampleReady;
+}
+
+PowerSample PowerTask::getSample()
+{
+  return this->lastSample;
+}

firmware/src/tasks/power_task.hpp

@@ -0,0 +1,34 @@
+#ifndef __POWER_HPP__
+#define __POWER_HPP__
+
+#define POWER_ADDRESS 0x41
+#define POWER_UPDATE_HZ 2
+
+#include <FreeRTOS_TEENSY4.h>
+#include <Arduino.h>
+#include <Adafruit_INA219.h>
+#include "task.hpp"
+#include "firmata_task.hpp"
+#include "firmata_commands.hpp"
+#include "power_sample.hpp"
+
+class PowerTask : public Task
+{
+public:
+  PowerTask(SemaphoreHandle_t &busMutex, FirmataTask &firmata);
+  void run();
+  void init();
+  static void startTask(void *arg);
+
+  bool hasSample();
+  PowerSample getSample();
+
+private:
+  SemaphoreHandle_t &busMutex;
+  FirmataTask &firmata;
+  Adafruit_INA219 sensor;
+  bool sampleReady;
+  PowerSample lastSample;
+};
+
+#endif // __POWER_HPP__