Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Torqeedo: handle outgoing esc characters

Browse Source
This commit is contained in:
Randy Mackay 2021-10-21 09:47:49 +09:00
parent 096f459148
commit 58b2154e77
2 changed files with 96 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
libraries/AP_Torqeedo/AP_Torqeedo.cpp
View File

@ -790,6 +790,79 @@ void AP_Torqeedo::set_reply_received()
_reply_wait_start_ms = 0;
}
// send a message to the motor with the specified message contents
// msg_contents should not include the header, footer or CRC
// returns true on success
bool AP_Torqeedo::send_message(const uint8_t msg_contents[], uint8_t num_bytes)
{
// buffer for outgoing message
uint8_t send_buff[TORQEEDO_MESSAGE_LEN_MAX];
uint8_t send_buff_num_bytes = 0;
// calculate crc
const uint8_t crc = crc8_maxim(msg_contents, num_bytes);
// add header
send_buff[send_buff_num_bytes++] = TORQEEDO_PACKET_HEADER;
// add contents
for (uint8_t i=0; i<num_bytes; i++) {
if (!add_byte_to_message(msg_contents[i], send_buff, ARRAY_SIZE(send_buff), send_buff_num_bytes)) {
_parse_error_count++;
return false;
}
}
// add crc
if (!add_byte_to_message(crc, send_buff, ARRAY_SIZE(send_buff), send_buff_num_bytes)) {
_parse_error_count++;
return false;
}
// add footer
if (send_buff_num_bytes >= ARRAY_SIZE(send_buff)) {
_parse_error_count++;
return false;
}
send_buff[send_buff_num_bytes++] = TORQEEDO_PACKET_FOOTER;
// set send pin
send_start();
// write message
_uart->write(send_buff, send_buff_num_bytes);
// record start and expected delay to send message
_send_start_us = AP_HAL::micros();
_send_delay_us = calc_send_delay_us(send_buff_num_bytes);
return true;
}
// add a byte to a message buffer including adding the escape character (0xAE) if necessary
// this should only be used when adding the contents to the buffer, not the header and footer
// num_bytes is updated to the next free byte
bool AP_Torqeedo::add_byte_to_message(uint8_t byte_to_add, uint8_t msg_buff[], uint8_t msg_buff_size, uint8_t &num_bytes) const
{
bool escape_required = (byte_to_add == TORQEEDO_PACKET_HEADER ||
byte_to_add == TORQEEDO_PACKET_FOOTER ||
byte_to_add == TORQEEDO_PACKET_ESCAPE);
// check if we have enough space
if (num_bytes + (escape_required ? 2 : 1) >= msg_buff_size) {
return false;
}
// add byte
if (escape_required) {
msg_buff[num_bytes++] = TORQEEDO_PACKET_ESCAPE;
msg_buff[num_bytes++] = byte_to_add ^ TORQEEDO_PACKET_ESCAPE_MASK;
} else {
msg_buff[num_bytes++] = byte_to_add;
}
return true;
}
// Example "Remote (0x01)" reply message to allow tiller to control motor speed
// Byte Field Definition Example Value Comments
// ---------------------------------------------------------------------------------
@ -824,20 +897,16 @@ void AP_Torqeedo::send_motor_speed_cmd()
// updated limited motor speed
int16_t mot_speed_limited = calc_motor_speed_limited(_motor_speed_desired);
// set send pin
send_start();
// by default use tiller connection command
uint8_t mot_speed_cmd_buff[] = {TORQEEDO_PACKET_HEADER, (uint8_t)MsgAddress::BUS_MASTER, 0x0, 0x5, 0x0, HIGHBYTE(mot_speed_limited), LOWBYTE(mot_speed_limited), 0x0, TORQEEDO_PACKET_FOOTER};
uint8_t buff_size = ARRAY_SIZE(mot_speed_cmd_buff);
uint8_t mot_speed_cmd_buff[] = {(uint8_t)MsgAddress::BUS_MASTER, 0x0, 0x5, 0x0, HIGHBYTE(mot_speed_limited), LOWBYTE(mot_speed_limited)};
// update message if using motor connection
if (_type == ConnectionType::TYPE_MOTOR) {
const uint8_t motor_power = (uint8_t)constrain_int16(_motor_power, 0, 100);
mot_speed_cmd_buff[1] = (uint8_t)MsgAddress::MOTOR;
mot_speed_cmd_buff[2] = (uint8_t)MotorMsgId::DRIVE;
mot_speed_cmd_buff[3] = (mot_speed_limited == 0 ? 0 : 0x01) | (_motor_clear_error ? 0x04 : 0); // 1:enable motor, 2:fast off, 4:clear error
mot_speed_cmd_buff[4] = mot_speed_limited == 0 ? 0 : motor_power; // motor power from 0 to 100
mot_speed_cmd_buff[0] = (uint8_t)MsgAddress::MOTOR;
mot_speed_cmd_buff[1] = (uint8_t)MotorMsgId::DRIVE;
mot_speed_cmd_buff[2] = (mot_speed_limited == 0 ? 0 : 0x01) | (_motor_clear_error ? 0x04 : 0); // 1:enable motor, 2:fast off, 4:clear error
mot_speed_cmd_buff[3] = mot_speed_limited == 0 ? 0 : motor_power; // motor power from 0 to 100
// set expected reply message id
set_expected_reply_msgid((uint8_t)MotorMsgId::DRIVE);
@ -846,18 +915,8 @@ void AP_Torqeedo::send_motor_speed_cmd()
_motor_clear_error = false;
}
// calculate crc and add to buffer
const uint8_t crc = crc8_maxim(&mot_speed_cmd_buff[1], buff_size-3);
mot_speed_cmd_buff[buff_size-2] = crc;
// write message
_uart->write(mot_speed_cmd_buff, buff_size);
// record start and expected delay to send message
_send_start_us = AP_HAL::micros();
_send_delay_us = calc_send_delay_us(buff_size);
{
// send a message
if (send_message(mot_speed_cmd_buff, ARRAY_SIZE(mot_speed_cmd_buff))) {
// record time of send for health reporting
WITH_SEMAPHORE(_last_healthy_sem);
_last_send_motor_ms = AP_HAL::millis();
@ -868,26 +927,14 @@ void AP_Torqeedo::send_motor_speed_cmd()
// msg_id can be INFO, STATUS or PARAM
void AP_Torqeedo::send_motor_msg_request(MotorMsgId msg_id)
{
// set send pin
send_start();
// prepare message
uint8_t mot_status_request_buff[] = {TORQEEDO_PACKET_HEADER, (uint8_t)MsgAddress::MOTOR, (uint8_t)msg_id, 0x0, TORQEEDO_PACKET_FOOTER};
uint8_t buff_size = ARRAY_SIZE(mot_status_request_buff);
uint8_t mot_status_request_buff[] = {(uint8_t)MsgAddress::MOTOR, (uint8_t)msg_id};
// calculate crc and add to buffer
const uint8_t crc = crc8_maxim(&mot_status_request_buff[1], buff_size-3);
mot_status_request_buff[buff_size-2] = crc;
// write message
_uart->write(mot_status_request_buff, buff_size);
// record start and expected delay to send message
_send_start_us = AP_HAL::micros();
_send_delay_us = calc_send_delay_us(buff_size);
// record waiting for reply
set_expected_reply_msgid((uint8_t)msg_id);
// send a message
if (send_message(mot_status_request_buff, ARRAY_SIZE(mot_status_request_buff))) {
// record waiting for reply
set_expected_reply_msgid((uint8_t)msg_id);
}
}
// calculate the limited motor speed that is sent to the motors

10
libraries/AP_Torqeedo/AP_Torqeedo.h
View File

@ -178,6 +178,16 @@ private:
// mark reply received. should be called whenever a message is received regardless of whether we are actually waiting for a reply
void set_reply_received();
// send a message to the motor with the specified message contents
// msg_contents should not include the header, footer or CRC
// returns true on success
bool send_message(const uint8_t msg_contents[], uint8_t num_bytes);
// add a byte to a message buffer including adding the escape character (0xAE) if necessary
// this should only be used when adding the contents to the buffer, not the header and footer
// num_bytes is updated to the next free byte
bool add_byte_to_message(uint8_t byte_to_add, uint8_t msg_buff[], uint8_t msg_buff_size, uint8_t &num_bytes) const;
// send a motor speed command as a value from -1000 to +1000
// value is taken directly from SRV_Channel
void send_motor_speed_cmd();