mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
AP_Volz_Protocol: add CMD union helper
This commit is contained in:
parent
b34d0c9c87
commit
871e9101ff
libraries/AP_Volz_Protocol
@ -56,8 +56,8 @@ void AP_Volz_Protocol::update()
|
||||
}
|
||||
|
||||
uint32_t now = AP_HAL::micros();
|
||||
if (now - last_volz_update_time < volz_time_frame_micros ||
|
||||
port->txspace() < VOLZ_DATA_FRAME_SIZE) {
|
||||
if (((now - last_volz_update_time) < volz_time_frame_micros) ||
|
||||
(port->txspace() < sizeof(CMD))) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -93,31 +93,28 @@ void AP_Volz_Protocol::update()
|
||||
}
|
||||
|
||||
// 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;
|
||||
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);
|
||||
send_command(cmd);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
// 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.
|
||||
crc = ((data[i] << 8) ^ crc);
|
||||
|
||||
for(j=0; j<8; j++) {
|
||||
crc = (cmd.data[i] << 8) ^ crc;
|
||||
|
||||
for(uint8_t j=0; j<8; j++) {
|
||||
if (crc & 0x8000) {
|
||||
crc = (crc << 1) ^ 0x8005;
|
||||
} else {
|
||||
@ -127,9 +124,9 @@ void AP_Volz_Protocol::send_command(uint8_t data[VOLZ_DATA_FRAME_SIZE])
|
||||
}
|
||||
|
||||
// add CRC result to the message
|
||||
data[4] = HIGHBYTE(crc);
|
||||
data[5] = LOWBYTE(crc);
|
||||
port->write(data, VOLZ_DATA_FRAME_SIZE);
|
||||
cmd.crc1 = HIGHBYTE(crc);
|
||||
cmd.crc2 = LOWBYTE(crc);
|
||||
port->write(cmd.data, sizeof(cmd));
|
||||
}
|
||||
|
||||
void AP_Volz_Protocol::update_volz_bitmask(uint32_t new_bitmask)
|
||||
|
@ -64,14 +64,28 @@ public:
|
||||
CLASS_NO_COPY(AP_Volz_Protocol);
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
|
||||
void update();
|
||||
|
||||
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;
|
||||
|
||||
|
||||
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);
|
||||
|
||||
uint32_t last_volz_update_time;
|
||||
|
Loading…
Reference in New Issue
Block a user