Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Volz_Protocol: add CMD union helper

Browse Source
This commit is contained in:
Iampete1 2024-04-04 14:05:19 +01:00 committed by Peter Barker
parent b34d0c9c87
commit 871e9101ff
2 changed files with 32 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp
View File

@ -56,8 +56,8 @@ void AP_Volz_Protocol::update()
} }
uint32_t now = AP_HAL::micros(); uint32_t now = AP_HAL::micros();
if (now - last_volz_update_time < volz_time_frame_micros || if (((now - last_volz_update_time) < volz_time_frame_micros) ||
port->txspace() < VOLZ_DATA_FRAME_SIZE) { (port->txspace() < sizeof(CMD))) {
return; return;
} }
@ -93,31 +93,28 @@ void AP_Volz_Protocol::update()
} }
// prepare Volz protocol data. // prepare Volz protocol data.
uint8_t data[VOLZ_DATA_FRAME_SIZE]; CMD cmd;
cmd.ID = SET_EXTENDED_POSITION_CMD;
cmd.actuator_id = i + 1; // send actuator id as 1 based index so ch1 will have id 1, ch2 will have id 2 ....
cmd.arg1 = HIGHBYTE(value);
cmd.arg2 = LOWBYTE(value);
data[0] = VOLZ_SET_EXTENDED_POSITION_CMD; send_command(cmd);
data[1] = i + 1; // send actuator id as 1 based index so ch1 will have id 1, ch2 will have id 2 ....
data[2] = HIGHBYTE(value);
data[3] = LOWBYTE(value);
send_command(data);
} }
} }
} }
// calculate CRC for volz serial protocol and send the data. // calculate CRC for volz serial protocol and send the data.
void AP_Volz_Protocol::send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE]) void AP_Volz_Protocol::send_command(CMD &cmd)
{ {
uint8_t i,j;
uint16_t crc = 0xFFFF; uint16_t crc = 0xFFFF;
// calculate Volz CRC value according to protocol definition // calculate Volz CRC value according to protocol definition
for(i=0; i<4; i++) { for(uint8_t i=0; i<4; i++) {
// take input data into message that will be transmitted. // take input data into message that will be transmitted.
crc = ((data[i] << 8) ^ crc); crc = (cmd.data[i] << 8) ^ crc;
for(j=0; j<8; j++) {
for(uint8_t j=0; j<8; j++) {
if (crc & 0x8000) { if (crc & 0x8000) {
crc = (crc << 1) ^ 0x8005; crc = (crc << 1) ^ 0x8005;
} else { } else {
@ -127,9 +124,9 @@ void AP_Volz_Protocol::send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE])
} }
// add CRC result to the message // add CRC result to the message
data[4] = HIGHBYTE(crc); cmd.crc1 = HIGHBYTE(crc);
data[5] = LOWBYTE(crc); cmd.crc2 = LOWBYTE(crc);
port->write(data, VOLZ_DATA_FRAME_SIZE); port->write(cmd.data, sizeof(cmd));
} }
void AP_Volz_Protocol::update_volz_bitmask(uint32_t new_bitmask) void AP_Volz_Protocol::update_volz_bitmask(uint32_t new_bitmask)

20
libraries/AP_Volz_Protocol/AP_Volz_Protocol.h
View File

@ -64,14 +64,28 @@ public:
CLASS_NO_COPY(AP_Volz_Protocol); CLASS_NO_COPY(AP_Volz_Protocol);
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
void update(); void update();
private: private:
// Command frame
union CMD {
struct PACKED {
uint8_t ID; // CMD ID
uint8_t actuator_id; // actuator send to or receiving from
uint8_t arg1; // CMD dependant argument 1
uint8_t arg2; // CMD dependant argument 2
uint8_t crc1;
uint8_t crc2;
};
uint8_t data[6];
};
AP_HAL::UARTDriver *port; AP_HAL::UARTDriver *port;
void init(void); void init(void);
void send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE]); void send_command(CMD &cmd);
void update_volz_bitmask(uint32_t new_bitmask); void update_volz_bitmask(uint32_t new_bitmask);
uint32_t last_volz_update_time; uint32_t last_volz_update_time;