65 lines
1.4 KiB
C++
65 lines
1.4 KiB
C++
|
|
#include "./SerialASCIITelemetry.h"
|
|
|
|
SerialASCIITelemetry::SerialASCIITelemetry(int floatPrecision) : Telemetry() {
|
|
this->floatPrecision = floatPrecision;
|
|
};
|
|
|
|
SerialASCIITelemetry::~SerialASCIITelemetry(){
|
|
|
|
};
|
|
|
|
void SerialASCIITelemetry::init(Print* print){
|
|
this->_print = print;
|
|
this->Telemetry::init();
|
|
};
|
|
|
|
|
|
|
|
void SerialASCIITelemetry::sendHeader() {
|
|
if (numRegisters > 0) {
|
|
writeChar('H');
|
|
writeChar(' ');
|
|
for (uint8_t i = 0; i < numRegisters; i++) {
|
|
writeByte(registers_motor[i]);
|
|
writeChar(':');
|
|
writeByte(registers[i]);
|
|
if (i < numRegisters-1)
|
|
writeChar(' ');
|
|
};
|
|
writeChar('\n');
|
|
};
|
|
};
|
|
|
|
|
|
|
|
void SerialASCIITelemetry::sendTelemetry(){
|
|
if (numRegisters > 0) {
|
|
for (uint8_t i = 0; i < numRegisters; i++) {
|
|
sendRegister(static_cast<SimpleFOCRegister>(registers[i]), motors[registers_motor[i]]);
|
|
if (i < numRegisters-1)
|
|
writeChar(' ');
|
|
};
|
|
writeChar('\n');
|
|
}
|
|
};
|
|
|
|
|
|
|
|
uint8_t SerialASCIITelemetry::writeChar(char value){
|
|
return _print->print(value);
|
|
};
|
|
|
|
uint8_t SerialASCIITelemetry::writeByte(uint8_t value){
|
|
return _print->print(value);
|
|
};
|
|
|
|
|
|
uint8_t SerialASCIITelemetry::writeFloat(float value){
|
|
return _print->print(value, floatPrecision);
|
|
};
|
|
|
|
|
|
uint8_t SerialASCIITelemetry::writeInt(uint32_t value){
|
|
return _print->print(value);
|
|
}; |