68 lines
1.2 KiB
C++
68 lines
1.2 KiB
C++
|
|
|
|
#include "./Telemetry.h"
|
|
|
|
|
|
|
|
Telemetry::Telemetry() {
|
|
this->numRegisters = 0;
|
|
};
|
|
|
|
|
|
|
|
Telemetry::~Telemetry(){
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
void Telemetry::setTelemetryRegisters(uint8_t numRegisters, uint8_t* registers, uint8_t* motors){
|
|
if (numRegisters<=TELEMETRY_MAX_REGISTERS) {
|
|
this->numRegisters = numRegisters;
|
|
for (uint8_t i=0; i<numRegisters; i++) {
|
|
this->registers[i] = registers[i];
|
|
if (motors!=NULL)
|
|
this->registers_motor[i] = motors[i];
|
|
else
|
|
this->registers_motor[i] = 0;
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
|
|
void Telemetry::init() {
|
|
headerSent = false;
|
|
};
|
|
|
|
|
|
|
|
void Telemetry::run() {
|
|
if (numRegisters<1)
|
|
return;
|
|
if (!headerSent) {
|
|
sendHeader();
|
|
headerSent = true;
|
|
}
|
|
if (downsampleCnt++ < downsample) return;
|
|
downsampleCnt = 0;
|
|
if (min_elapsed_time > 0) {
|
|
long now = _micros();
|
|
if (now - last_run_time < min_elapsed_time) return;
|
|
last_run_time = now;
|
|
}
|
|
sendTelemetry();
|
|
}
|
|
|
|
|
|
|
|
void Telemetry::addMotor(FOCMotor* motor) {
|
|
if (numMotors < TELEMETRY_MAX_MOTORS) {
|
|
motors[numMotors] = motor;
|
|
numMotors++;
|
|
}
|
|
};
|
|
|