robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

Fix current address

Browse source
This commit is contained in:
lulko 2025-05-05 14:53:37 +03:00
parent b36103201c
commit 8ecb1aca43
2 changed files with 16 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

2
controller/fw/embed/src/flash.cpp
View file

@ -94,7 +94,7 @@ void flash_write(uint32_t addr, FLASH_RECORD* record){
FLASH->CR |= FLASH_CR_PG; FLASH->CR |= FLASH_CR_PG;
for(int i = 0;i < size;i++){ for(int i = 0;i < size;i++){
*(volatile uint32_t*)(addr + i) = data[i]; *(volatile uint32_t*)(addr + (i * 4)) = data[i];
write_ptr++; write_ptr++;
} }

32
controller/fw/embed/src/main.cpp
View file

@ -119,36 +119,35 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, size_t data_length) { void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, size_t data_length) {
// Create CAN message // Create CAN message
CAN_message_t msg; CAN_message_t msg_l;
msg.id = id; msg_l.id = id;
msg.len = 8; // Protocol-defined message length msg_l.len = 8; // Protocol-defined message length
msg.buf[0] = message_type; msg_l.buf[0] = message_type;
memcpy(&msg.buf[1], data, data_length); memcpy(&msg_l.buf[1], data, data_length);
// Prepare CRC calculation buffer (ID + data) // Prepare CRC calculation buffer (ID + data)
size_t crc_data_size = sizeof(msg.id) + data_length; size_t crc_data_size = sizeof(msg_l.id) + data_length;
uint8_t crc_data[crc_data_size]; uint8_t crc_data[crc_data_size];
// Copy message ID // Copy message ID
memcpy(crc_data, &msg.id, sizeof(msg.id)); memcpy(crc_data, &msg_l.id, sizeof(msg_l.id));
// Copy all data bytes // Copy all data bytes
memcpy(crc_data + sizeof(msg.id), data, data_length); memcpy(crc_data + sizeof(msg_l.id), data, data_length);
// Calculate CRC // Calculate CRC
uint16_t crc_value = validate_crc16(crc_data, crc_data_size); uint16_t crc_value = validate_crc16(crc_data, crc_data_size);
// Insert CRC into buffer // Insert CRC into buffer
msg.buf[6] = crc_value & 0xFF; msg_l.buf[6] = crc_value & 0xFF;
msg.buf[7] = (crc_value >> 8) & 0xFF; msg_l.buf[7] = (crc_value >> 8) & 0xFF;
// Send message // Send message
Can.write(msg); Can.write(msg_l);
__NOP(); __NOP();
} }
void send_velocity() { void send_velocity() {
float current_velocity = motor.shaftVelocity(); float current_velocity = motor.shaftVelocity();
flash_rec = load_params();
if (flash_rec == nullptr) { // Null check if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc. // Error handling: logging, alerts, etc.
return; return;
@ -160,7 +159,6 @@ void send_velocity() {
void send_angle() { void send_angle() {
float current_angle = motor.shaftAngle(); float current_angle = motor.shaftAngle();
flash_rec = load_params();
if (flash_rec == nullptr) { // Null check if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc. // Error handling: logging, alerts, etc.
return; return;
@ -180,7 +178,6 @@ void send_motor_enabled() {
void send_foc_state() { void send_foc_state() {
/* Firmware data reading */ /* Firmware data reading */
flash_rec = load_params();
if (flash_rec == nullptr) { // Null check if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc. // Error handling: logging, alerts, etc.
return; return;
@ -192,7 +189,6 @@ void send_foc_state() {
void send_id() { void send_id() {
/* Firmware data reading */ /* Firmware data reading */
flash_rec = load_params();
if (flash_rec == nullptr) { // Null check if (flash_rec == nullptr) { // Null check
// Error handling: logging, alerts, etc. // Error handling: logging, alerts, etc.
return; return;
@ -206,7 +202,6 @@ void send_motor_torque() {
float i_q = motor.current.q; // Q-axis current (A) float i_q = motor.current.q; // Q-axis current (A)
float torque = kt * i_q; // Torque calculation float torque = kt * i_q; // Torque calculation
torque *= 100; torque *= 100;
flash_rec = load_params();
CAN_TX_msg.id = flash_rec->value; CAN_TX_msg.id = flash_rec->value;
CAN_TX_msg.buf[0] = 'T'; CAN_TX_msg.buf[0] = 'T';
CAN_TX_msg.len = 5; CAN_TX_msg.len = 5;
@ -215,7 +210,6 @@ void send_motor_torque() {
} }
void send_pid(uint8_t param_pid){ void send_pid(uint8_t param_pid){
flash_rec = load_params();
if (flash_rec == nullptr) { // Null check if (flash_rec == nullptr) { // Null check
return; return;
} }
@ -294,11 +288,11 @@ void listen_can(const CAN_message_t &msg) {
if (calculated_crc != received_crc) { if (calculated_crc != received_crc) {
return; // Ignore message on CRC mismatch return; // Ignore message on CRC mismatch
} }
flash_rec = load_params();
/* Message Structure: 0x691 /* Message Structure: 0x691
69 - Device address 69 - Device address
1 - Action code */ 1 - Action code */
if(id_x == flash_rec->value){ if(id_x == flash_rec[addr_id].value){
if(msg_ch == REG_WRITE){ if(msg_ch == REG_WRITE){
switch(msg.buf[0]) { switch(msg.buf[0]) {
case REG_ID: case REG_ID: