You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
309 lines
9.6 KiB
309 lines
9.6 KiB
/* |
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
*/ |
|
|
|
#include "AP_Torqeedo.h" |
|
|
|
#if HAL_TORQEEDO_ENABLED |
|
|
|
#include <AP_Common/AP_Common.h> |
|
#include <AP_Math/AP_Math.h> |
|
#include <SRV_Channel/SRV_Channel.h> |
|
|
|
#define TORQEEDO_SERIAL_BAUD 19200 // communication is always at 19200 |
|
#define TORQEEDO_PACKET_HEADER 0xAC // communication packet header |
|
#define TORQEEDO_PACKET_FOOTER 0xAD // communication packer footer |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
// parameters |
|
const AP_Param::GroupInfo AP_Torqeedo::var_info[] = { |
|
|
|
// @Param: ENABLE |
|
// @DisplayName: Torqeedo enable |
|
// @Description: Torqeedo enable |
|
// @Values: 0:Disabled, 1:Enabled |
|
// @User: Standard |
|
// @RebootRequired: True |
|
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_Torqeedo, _enable, 0, AP_PARAM_FLAG_ENABLE), |
|
|
|
// @Param: ONOFF_PIN |
|
// @DisplayName: Torqeedo ON/Off pin |
|
// @Description: Pin number connected to Torqeedo's on/off pin. -1 to disable turning motor on/off from autopilot |
|
// @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6 |
|
// @User: Standard |
|
// @RebootRequired: True |
|
AP_GROUPINFO("ONOFF_PIN", 2, AP_Torqeedo, _pin_onoff, -1), |
|
|
|
// @Param: DE_PIN |
|
// @DisplayName: Torqeedo DE pin |
|
// @Description: Pin number connected to RS485 to Serial converter's DE pin. -1 to disable sending commands to motor |
|
// @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6 |
|
// @User: Standard |
|
// @RebootRequired: True |
|
AP_GROUPINFO("DE_PIN", 3, AP_Torqeedo, _pin_de, -1), |
|
|
|
AP_GROUPEND |
|
}; |
|
|
|
AP_Torqeedo::AP_Torqeedo() |
|
{ |
|
_singleton = this; |
|
AP_Param::setup_object_defaults(this, var_info); |
|
} |
|
|
|
// initialise driver |
|
void AP_Torqeedo::init() |
|
{ |
|
// only init once |
|
if (_initialised) { |
|
return; |
|
} |
|
|
|
// create background thread to process serial input and output |
|
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Torqeedo::thread_main, void), "torqeedo", 2048, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) { |
|
return; |
|
} |
|
} |
|
|
|
// initialise serial port and gpio pins (run from background thread) |
|
bool AP_Torqeedo::init_internals() |
|
{ |
|
// find serial driver and initialise |
|
const AP_SerialManager &serial_manager = AP::serialmanager(); |
|
_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Torqeedo, 0); |
|
if (_uart == nullptr) { |
|
return false; |
|
} |
|
_uart->begin(TORQEEDO_SERIAL_BAUD); |
|
_uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); |
|
_uart->set_unbuffered_writes(true); |
|
|
|
// initialise onoff pin and set for 0.5 sec to turn on motor |
|
if (_pin_onoff > -1) { |
|
hal.gpio->pinMode(_pin_onoff, HAL_GPIO_OUTPUT); |
|
hal.gpio->write(_pin_onoff, 1); |
|
|
|
// delay 0.5 sec and then unset pin |
|
hal.scheduler->delay(500); |
|
hal.gpio->write(_pin_onoff, 0); |
|
} |
|
|
|
// initialise RS485 DE pin (when high, allows send to motor) |
|
if (_pin_de > -1) { |
|
hal.gpio->pinMode(_pin_de, HAL_GPIO_OUTPUT); |
|
hal.gpio->write(_pin_de, 0); |
|
} |
|
|
|
return true; |
|
} |
|
|
|
// consume incoming messages from motor, reply with latest motor speed |
|
// runs in background thread |
|
void AP_Torqeedo::thread_main() |
|
{ |
|
// initialisation |
|
if (!init_internals()) { |
|
return; |
|
} |
|
_initialised = true; |
|
|
|
while (true) { |
|
// 1ms loop delay |
|
hal.scheduler->delay_microseconds(1000); |
|
|
|
// check if transmit pin should be unset |
|
check_for_send_end(); |
|
|
|
// parse incoming characters |
|
uint32_t nbytes = MIN(_uart->available(), 1024U); |
|
bool motor_speed_request_received = false; |
|
while (nbytes-- > 0) { |
|
int16_t b = _uart->read(); |
|
if (b >= 0 ) { |
|
if (parse_byte((uint8_t)b)) { |
|
// request received to send updated motor speed |
|
motor_speed_request_received = true; |
|
} |
|
} |
|
} |
|
|
|
// send motor speed |
|
if (motor_speed_request_received) { |
|
send_motor_speed_cmd(); |
|
} |
|
} |
|
} |
|
|
|
// process a single byte received on serial port |
|
// return true if a this driver should send a set-motor-speed message |
|
bool AP_Torqeedo::parse_byte(uint8_t b) |
|
{ |
|
bool motor_speed_requested = false; |
|
|
|
switch (_parse_state) { |
|
case ParseState::WAITING_FOR_HEADER: |
|
if (b == TORQEEDO_PACKET_HEADER) { |
|
_parse_state = ParseState::WAITING_FOR_FOOTER; |
|
} |
|
_received_buff_len = 0; |
|
break; |
|
case ParseState::WAITING_FOR_FOOTER: |
|
if (b == TORQEEDO_PACKET_FOOTER) { |
|
_parse_state = ParseState::WAITING_FOR_HEADER; |
|
|
|
// check message length |
|
if (_received_buff_len == 0) { |
|
_parse_error_count++; |
|
break; |
|
} |
|
|
|
// check crc |
|
const uint8_t crc_expected = crc8_maxim(_received_buff, _received_buff_len-1); |
|
if (_received_buff[_received_buff_len-1] != crc_expected) { |
|
_parse_error_count++; |
|
break; |
|
} |
|
_parse_success_count++; |
|
|
|
// check message id |
|
MsgId msg_id = (MsgId)_received_buff[0]; |
|
if (msg_id == MsgId::REQUEST_MOTOR_SPEED) { |
|
motor_speed_requested = true; |
|
} |
|
|
|
} else { |
|
// add to buffer |
|
_received_buff[_received_buff_len] = b; |
|
_received_buff_len++; |
|
if (_received_buff_len > TORQEEDO_MESSAGE_LEN_MAX) { |
|
// message too long |
|
_parse_state = ParseState::WAITING_FOR_HEADER; |
|
_parse_error_count++; |
|
} |
|
} |
|
break; |
|
} |
|
|
|
return motor_speed_requested; |
|
} |
|
|
|
// set DE Serial CTS pin to enable sending commands to motor |
|
void AP_Torqeedo::send_start() |
|
{ |
|
// set gpio pin or serial port's CTS pin |
|
if (_pin_de > -1) { |
|
hal.gpio->write(_pin_de, 1); |
|
} else { |
|
_uart->set_CTS_pin(true); |
|
} |
|
} |
|
|
|
// check for timeout after sending and unset pin if required |
|
void AP_Torqeedo::check_for_send_end() |
|
{ |
|
if (_send_delay_us == 0) { |
|
// not sending |
|
return; |
|
} |
|
|
|
if (AP_HAL::micros() - _last_send_motor_us < _send_delay_us) { |
|
// return if delay has not yet elapsed |
|
return; |
|
} |
|
_send_delay_us = 0; |
|
|
|
// unset gpio or serial port's CTS pin |
|
if (_pin_de > -1) { |
|
hal.gpio->write(_pin_de, 0); |
|
} else { |
|
_uart->set_CTS_pin(false); |
|
} |
|
} |
|
|
|
// calculate delay require to allow bytes to be sent |
|
uint32_t AP_Torqeedo::calc_send_delay_us(uint8_t num_bytes) |
|
{ |
|
// baud rate of 19200 bits/sec |
|
// total number of bits = 10 x num_bytes |
|
// convert from seconds to micros by multiplying by 1,000,000 |
|
// plus additional 300us safety margin |
|
const uint32_t delay_us = 1e6 * num_bytes * 10 / TORQEEDO_SERIAL_BAUD + 300; |
|
return delay_us; |
|
} |
|
|
|
// format of motor speed command packet |
|
// |
|
// Data Byte Field Definition Example Value Comments |
|
// --------------------------------------------------------------------------------- |
|
// byte 0 Header 0xAC |
|
// byte 1 SourceId? 0x00 0 = tiller? |
|
// byte 2 Destination ID? 0x00 0 = all? |
|
// byte 3 Command Id? 0x05 0=Stop? 4=Don'tTurn? 5=Turn? |
|
// byte 4 Command Id? 0x00 0x20 if byte3=4, 0x0 is byte3=5 |
|
// byte 5 Motor Speed MSB ---- Motor Speed MSB (-1000 to +1000) |
|
// byte 6 Motor Speed LSB ---- Motor Speed LSB (-1000 to +1000) |
|
// byte 7 CRC-Maxim ---- CRC-Maxim value |
|
// byte 8 Footer 0xAD |
|
// |
|
// example message when rotating forwards: "AC 00 00 05 00 00 ED 95 AD" (+237) |
|
// example message when rotating backwards: "AC 00 00 05 00 FF AE 2C 0C AD" (-82) |
|
|
|
|
|
// send a motor speed command as a value from -1000 to +1000 |
|
// value is taken directly from SRV_Channel |
|
void AP_Torqeedo::send_motor_speed_cmd() |
|
{ |
|
// calculate desired motor speed |
|
if (!hal.util->get_soft_armed()) { |
|
_motor_speed = 0; |
|
} else { |
|
// convert throttle output to motor output in range -1000 to +1000 |
|
// ToDo: convert PWM output to motor output so that SERVOx_MIN, MAX and TRIM take effect |
|
_motor_speed = constrain_int16(SRV_Channels::get_output_norm(SRV_Channel::Aux_servo_function_t::k_throttle) * 1000.0, -1000, 1000); |
|
} |
|
|
|
// set send pin |
|
send_start(); |
|
|
|
uint8_t mot_speed_cmd_buff[] = {TORQEEDO_PACKET_HEADER, 0x0, 0x0, 0x5, 0x0, HIGHBYTE(_motor_speed), LOWBYTE(_motor_speed), 0x0, TORQEEDO_PACKET_FOOTER}; |
|
uint8_t buff_size = ARRAY_SIZE(mot_speed_cmd_buff); |
|
|
|
// 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); |
|
|
|
_last_send_motor_us = AP_HAL::micros(); |
|
_send_delay_us = calc_send_delay_us(buff_size); |
|
} |
|
|
|
// get the AP_Torqeedo singleton |
|
AP_Torqeedo *AP_Torqeedo::get_singleton() |
|
{ |
|
return _singleton; |
|
} |
|
|
|
AP_Torqeedo *AP_Torqeedo::_singleton = nullptr; |
|
|
|
namespace AP { |
|
AP_Torqeedo *torqeedo() |
|
{ |
|
return AP_Torqeedo::get_singleton(); |
|
} |
|
}; |
|
|
|
#endif // HAL_TORQEEDO_ENABLED
|
|
|