Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 4 additions & 14 deletions src/comms/SimpleFOCRegisters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,10 +4,8 @@
#include "communication/SimpleFOCDebug.h"
#include "./telemetry/Telemetry.h"


SimpleFOCRegisters::SimpleFOCRegisters(){};
SimpleFOCRegisters::~SimpleFOCRegisters(){};

CustomRegisterHandler* customRegisters[MAX_CUSTOM_REGISTERS] = {nullptr};
uint8_t customRegisterCount = 0;

// write the register value(s) for the given motor to the given comms object
bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMotor* motor){
Expand Down Expand Up @@ -75,12 +73,7 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto
case SimpleFOCRegister::REG_TELEMETRY_REG:
if (Telemetry::num_telemetry > 0){
Telemetry* telemetry = Telemetry::telemetries[Telemetry::telemetry_ctrl];
comms << telemetry->numRegisters;
for (uint8_t i=0; i<telemetry->numRegisters; i++) {
comms << telemetry->registers_motor[i];
comms << telemetry->registers[i];
}
telemetry->headerSent = false;
telemetry->registerToComms(comms);
}
else {
comms << (uint32_t)0;
Expand Down Expand Up @@ -700,7 +693,7 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){
case SimpleFOCRegister::REG_TELEMETRY_REG:
if (Telemetry::num_telemetry > 0) {
Telemetry* telemetry = Telemetry::telemetries[Telemetry::telemetry_ctrl];
return 2*telemetry->numRegisters + 1;
return telemetry->sizeOfRegister();
}
else
return 1;
Expand Down Expand Up @@ -743,6 +736,3 @@ bool SimpleFOCRegisters::addCustomRegister(uint8_t reg, uint8_t size, RegisterRe
customRegisters[customRegisterCount++] = new CustomRegisterHandler{reg, size, readHandler, writeHandler};
return true;
}


SimpleFOCRegisters* SimpleFOCRegisters::regs = new SimpleFOCRegisters();
16 changes: 4 additions & 12 deletions src/comms/SimpleFOCRegisters.h
Original file line number Diff line number Diff line change
Expand Up @@ -117,19 +117,11 @@ struct CustomRegisterHandler {
};


class SimpleFOCRegisters {
public:
SimpleFOCRegisters();
virtual ~SimpleFOCRegisters();
virtual uint8_t sizeOfRegister(uint8_t reg);
virtual bool registerToComms(RegisterIO& comms, uint8_t reg, FOCMotor* motor);
virtual bool commsToRegister(RegisterIO& comms, uint8_t reg, FOCMotor* motor);

static SimpleFOCRegisters* regs;
namespace SimpleFOCRegisters {
uint8_t sizeOfRegister(uint8_t reg);
bool registerToComms(RegisterIO& comms, uint8_t reg, FOCMotor* motor);
bool commsToRegister(RegisterIO& comms, uint8_t reg, FOCMotor* motor);

// Method to add custom register handlers
bool addCustomRegister(uint8_t reg, uint8_t size, RegisterReadHandler readHandler, RegisterWriteHandler writeHandler);
protected:
CustomRegisterHandler* customRegisters[MAX_CUSTOM_REGISTERS] = {nullptr};
uint8_t customRegisterCount = 0;
};
4 changes: 2 additions & 2 deletions src/comms/can/CANCommander.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ void CANCommander::sendRegisterResponse(uint8_t reg, uint8_t dest_addr) {
}

bool CANCommander::commsToRegister(uint8_t reg) {
return SimpleFOCRegisters::regs->commsToRegister(*this, reg, motors[curMotor]);
return SimpleFOCRegisters::commsToRegister(*this, reg, motors[curMotor]);
}

bool CANCommander::registerToComms(uint8_t reg) {
Expand All @@ -136,7 +136,7 @@ bool CANCommander::registerToComms(uint8_t reg) {
commanderror = true;
return false;
}
return SimpleFOCRegisters::regs->registerToComms(*this, reg, motors[curMotor]);
return SimpleFOCRegisters::registerToComms(*this, reg, motors[curMotor]);
}
}

Expand Down
2 changes: 1 addition & 1 deletion src/comms/can/CANCommander.h
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ class CANCommander : public RegisterIO
bool addCustomRegister(uint8_t reg, uint8_t size,
RegisterReadHandler readHandler,
RegisterWriteHandler writeHandler) {
return SimpleFOCRegisters::regs->addCustomRegister(reg, size, readHandler, writeHandler);
return SimpleFOCRegisters::addCustomRegister(reg, size, readHandler, writeHandler);
}

protected:
Expand Down
6 changes: 3 additions & 3 deletions src/comms/i2c/I2CCommander.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -129,7 +129,7 @@ bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int nu
(val>0)?motors[i]->enable():motors[i]->disable();
break;
default: // unknown register
return SimpleFOCRegisters::regs->commsToRegister(*this, registerNum, motors[motorNum]);
return SimpleFOCRegisters::commsToRegister(*this, registerNum, motors[motorNum]);
}
return true;
}
Expand Down Expand Up @@ -171,7 +171,7 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) {
break;
case I2CCOMMANDER_REG_REPORT:
for (int i=0;i<numReportRegisters;i++)
SimpleFOCRegisters::regs->registerToComms(*this, reportRegisters[i], motors[reportMotors[i]]); // send any normal register
SimpleFOCRegisters::registerToComms(*this, reportRegisters[i], motors[reportMotors[i]]); // send any normal register
break;
case REG_NUM_MOTORS:
_wire->write(numMotors);
Expand All @@ -181,7 +181,7 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) {
return false;
// unknown register - not handled here
default:
return SimpleFOCRegisters::regs->registerToComms(*this, registerNum, motors[motorNum]);
return SimpleFOCRegisters::registerToComms(*this, registerNum, motors[motorNum]);
}
return true;
}
6 changes: 3 additions & 3 deletions src/comms/streams/PacketCommander.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ void PacketCommander::handleRegisterPacket(bool write, uint8_t reg) {
commanderror = commanderror || !ok;
}
if (!write || echo) {
uint8_t size = SimpleFOCRegisters::regs->sizeOfRegister(reg);
uint8_t size = SimpleFOCRegisters::sizeOfRegister(reg);
if (size > 0) { // sendable register
*_io << START_PACKET(PacketType::RESPONSE, size+1) << reg << Separator('=');
// TODO status?
Expand All @@ -98,7 +98,7 @@ bool PacketCommander::commsToRegister(uint8_t reg){
curMotor = val;
return true;
default:
return SimpleFOCRegisters::regs->commsToRegister(*_io, reg, motors[curMotor]);
return SimpleFOCRegisters::commsToRegister(*_io, reg, motors[curMotor]);
}
};

Expand All @@ -116,6 +116,6 @@ bool PacketCommander::registerToComms(uint8_t reg){
*_io << numMotors;
return true;
default:
return SimpleFOCRegisters::regs->registerToComms(*_io, reg, motors[curMotor]);
return SimpleFOCRegisters::registerToComms(*_io, reg, motors[curMotor]);
}
};
2 changes: 1 addition & 1 deletion src/comms/telemetry/SimpleTelemetry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ void SimpleTelemetry::init(TextIO& _comms){
void SimpleTelemetry::sendTelemetry(){
if (numRegisters > 0) {
for (uint8_t i = 0; i < numRegisters; i++) {
SimpleFOCRegisters::regs->registerToComms(*comms, registers[i], motors[registers_motor[i]]);
SimpleFOCRegisters::registerToComms(*comms, registers[i], motors[registers_motor[i]]);
if (i<numRegisters-1) *comms << '\t';
};
*comms << '\n';
Expand Down
21 changes: 16 additions & 5 deletions src/comms/telemetry/Telemetry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +42,6 @@ void Telemetry::init(PacketIO& _comms) {
}
this->id = Telemetry::num_telemetry++;
headerSent = false;
if (SimpleFOCRegisters::regs == NULL) {
SimpleFOCRegisters::regs = new SimpleFOCRegisters();
}
};


Expand Down Expand Up @@ -87,7 +84,21 @@ void Telemetry::addMotor(FOCMotor* motor) {
}
};

uint8_t Telemetry::sizeOfRegister() const
{
return 2*numRegisters + 1;
}

bool Telemetry::registerToComms(RegisterIO& comms)
{
comms << numRegisters;
for (uint8_t i=0; i<numRegisters; i++) {
comms << registers_motor[i];
comms << registers[i];
}
headerSent = false;
return true;
}

void Telemetry::sendHeader() {
if (numRegisters > 0) {
Expand All @@ -105,11 +116,11 @@ void Telemetry::sendTelemetry(){
if (numRegisters > 0) {
uint8_t size = 1;
for (uint8_t i = 0; i < numRegisters; i++) {
size += SimpleFOCRegisters::regs->sizeOfRegister(registers[i]);
size += SimpleFOCRegisters::sizeOfRegister(registers[i]);
}
*comms << START_PACKET(PacketType::TELEMETRY, size) << id << Separator('=');
for (uint8_t i = 0; i < numRegisters; i++) {
SimpleFOCRegisters::regs->registerToComms(*comms, registers[i], motors[registers_motor[i]]);
SimpleFOCRegisters::registerToComms(*comms, registers[i], motors[registers_motor[i]]);
};
*comms << END_PACKET;
}
Expand Down
3 changes: 2 additions & 1 deletion src/comms/telemetry/Telemetry.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,6 @@ typedef enum : uint8_t {


class Telemetry {
friend class SimpleFOCRegisters;
public:
Telemetry();
virtual ~Telemetry();
Expand All @@ -47,6 +46,8 @@ class Telemetry {
static uint8_t num_telemetry;
static uint8_t telemetry_ctrl;
static Telemetry* telemetries[];
uint8_t sizeOfRegister() const;
bool registerToComms(RegisterIO& comms);
protected:
virtual void sendTelemetry();
virtual void sendHeader();
Expand Down
2 changes: 1 addition & 1 deletion src/comms/telemetry/TeleplotTelemetry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ void TeleplotTelemetry::sendTelemetry(){
*comms << registers_motor[i] << '_';
}
*comms << registers[i] << ':';
SimpleFOCRegisters::regs->registerToComms(*comms, registers[i], motors[registers_motor[i]]);
SimpleFOCRegisters::registerToComms(*comms, registers[i], motors[registers_motor[i]]);
*comms << '\n';
};
}
Expand Down
4 changes: 2 additions & 2 deletions src/settings/SettingsStorage.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ SettingsStatus SettingsStorage::loadSettings() {
for (int i = 0; i < numRegisters; i++) {
SimpleFOCRegister reg = registers[i];
startLoadRegister(reg);
SimpleFOCRegisters::regs->commsToRegister(*_io, reg, motors[m]);
SimpleFOCRegisters::commsToRegister(*_io, reg, motors[m]);
endLoadRegister();
}
endLoadMotor();
Expand All @@ -94,7 +94,7 @@ SettingsStatus SettingsStorage::saveSettings() {
for (int i = 0; i < numRegisters; i++) {
SimpleFOCRegister reg = registers[i];
startSaveRegister(reg);
SimpleFOCRegisters::regs->registerToComms(*_io, reg, motors[m]);
SimpleFOCRegisters::registerToComms(*_io, reg, motors[m]);
endSaveRegister();
}
endSaveMotor();
Expand Down
Loading