From 58b2154e77141ceb07148b69c8e5599aa583acb4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 21 Oct 2021 09:47:49 +0900 Subject: [PATCH] AP_Torqeedo: handle outgoing esc characters --- libraries/AP_Torqeedo/AP_Torqeedo.cpp | 125 ++++++++++++++++++-------- libraries/AP_Torqeedo/AP_Torqeedo.h | 10 +++ 2 files changed, 96 insertions(+), 39 deletions(-) diff --git a/libraries/AP_Torqeedo/AP_Torqeedo.cpp b/libraries/AP_Torqeedo/AP_Torqeedo.cpp index cb17eac899..13ff3d7f5b 100644 --- a/libraries/AP_Torqeedo/AP_Torqeedo.cpp +++ b/libraries/AP_Torqeedo/AP_Torqeedo.cpp @@ -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= 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 diff --git a/libraries/AP_Torqeedo/AP_Torqeedo.h b/libraries/AP_Torqeedo/AP_Torqeedo.h index 61dbda7ecc..fad1f9fdd7 100644 --- a/libraries/AP_Torqeedo/AP_Torqeedo.h +++ b/libraries/AP_Torqeedo/AP_Torqeedo.h @@ -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();