Telemetry is going again via Firmata.
This commit is contained in:
parent
59ce49b3c9
commit
1ea1d5477f
7 changed files with 140 additions and 6 deletions
|
@ -11,6 +11,7 @@ enum FirmataTelemetryType
|
||||||
{
|
{
|
||||||
GPS = 0,
|
GPS = 0,
|
||||||
IMU,
|
IMU,
|
||||||
|
POWER
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // __FIRMATA_COMMANDS_HPP__
|
#endif // __FIRMATA_COMMANDS_HPP__
|
||||||
|
|
|
@ -15,6 +15,7 @@ extern "C"
|
||||||
#include "tasks/firmata_task.hpp"
|
#include "tasks/firmata_task.hpp"
|
||||||
#include "tasks/gps_task.hpp"
|
#include "tasks/gps_task.hpp"
|
||||||
#include "tasks/imu_task.hpp"
|
#include "tasks/imu_task.hpp"
|
||||||
|
#include "tasks/power_task.hpp"
|
||||||
|
|
||||||
#define SERVO_ENABLE_PORT_PIN 2
|
#define SERVO_ENABLE_PORT_PIN 2
|
||||||
#define SERVO_ENABLE_STARBOARD_PIN 3
|
#define SERVO_ENABLE_STARBOARD_PIN 3
|
||||||
|
@ -33,11 +34,13 @@ void setup()
|
||||||
FirmataTask firmataTask;
|
FirmataTask firmataTask;
|
||||||
GpsTask gpsTask(firmataTask);
|
GpsTask gpsTask(firmataTask);
|
||||||
ImuTask imuTask(i2c0Mutex, firmataTask);
|
ImuTask imuTask(i2c0Mutex, firmataTask);
|
||||||
|
PowerTask powerTask(i2c0Mutex, firmataTask);
|
||||||
|
|
||||||
xTaskCreate(BlinkerTask::startTask, "Blinker", 2048, (void *)&blinkerTask, 1, NULL);
|
xTaskCreate(BlinkerTask::startTask, "Blinker", 2048, (void *)&blinkerTask, 1, NULL);
|
||||||
xTaskCreate(FirmataTask::startTask, "Firmata", 2048, (void *)&firmataTask, 2, NULL);
|
xTaskCreate(FirmataTask::startTask, "Firmata", 2048, (void *)&firmataTask, 2, NULL);
|
||||||
xTaskCreate(GpsTask::startTask, "GPS", 2048, (void *)&gpsTask, 3, 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();
|
vTaskStartScheduler();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -32,8 +32,15 @@ void GpsTask::run()
|
||||||
this->lastSample = newSample;
|
this->lastSample = newSample;
|
||||||
this->sampleReady = true;
|
this->sampleReady = true;
|
||||||
|
|
||||||
byte *data = (byte *)&this->lastSample;
|
byte *data[22];
|
||||||
this->firmata.sendTelemetry(FirmataTelemetryType::GPS, sizeof(this->lastSample), data);
|
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);
|
vTaskDelay(delay);
|
||||||
}
|
}
|
||||||
|
|
|
@ -29,7 +29,7 @@ void ImuTask::run()
|
||||||
imu::Vector<3> gyroscope = this->imu.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
|
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> gravity = this->imu.getVector(Adafruit_BNO055::VECTOR_GRAVITY);
|
||||||
imu::Vector<3> linearAcceleration = this->imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
|
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);
|
xSemaphoreGive(this->busMutex);
|
||||||
|
|
||||||
this->lastSample.orientation = {orientation.x(), orientation.y(), orientation.z(), orientation.w()};
|
this->lastSample.orientation = {orientation.x(), orientation.y(), orientation.z(), orientation.w()};
|
||||||
|
@ -41,8 +41,14 @@ void ImuTask::run()
|
||||||
this->lastSample.temperature = temperature;
|
this->lastSample.temperature = temperature;
|
||||||
this->sampleReady = true;
|
this->sampleReady = true;
|
||||||
|
|
||||||
byte *data = (byte *)&this->lastSample;
|
// Only sending the orientation and temperature to the telemetry for now.
|
||||||
this->firmata.sendTelemetry(FirmataTelemetryType::IMU, sizeof(this->lastSample), data);
|
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);
|
vTaskDelay(delay);
|
||||||
}
|
}
|
||||||
|
|
12
firmware/src/tasks/power_sample.hpp
Normal file
12
firmware/src/tasks/power_sample.hpp
Normal file
|
@ -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__
|
71
firmware/src/tasks/power_task.cpp
Normal file
71
firmware/src/tasks/power_task.cpp
Normal file
|
@ -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 *)¤t;
|
||||||
|
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;
|
||||||
|
}
|
34
firmware/src/tasks/power_task.hpp
Normal file
34
firmware/src/tasks/power_task.hpp
Normal file
|
@ -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__
|
Reference in a new issue