mirror of https://github.com/ArduPilot/ardupilot
AP_Torqeedo: simple driver to control torqeedo 1003 motor
includes fixes from peer review
This commit is contained in:
Randy Mackay
parent
7e68ce5c86
commit
50ce79bcf6
|
|
@ -0,0 +1,309 @@
|
|||
/*
|
||||
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
|
||||
|
|
@ -0,0 +1,110 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#ifndef HAL_TORQEEDO_ENABLED
|
||||
#define HAL_TORQEEDO_ENABLED (BOARD_FLASH_SIZE > 1024)
|
||||
#endif
|
||||
|
||||
#if HAL_TORQEEDO_ENABLED
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
|
||||
#define TORQEEDO_MESSAGE_LEN_MAX 30 // messages are no more than 30 bytes
|
||||
|
||||
class AP_Torqeedo {
|
||||
public:
|
||||
AP_Torqeedo();
|
||||
|
||||
CLASS_NO_COPY(AP_Torqeedo);
|
||||
|
||||
static AP_Torqeedo* get_singleton();
|
||||
|
||||
// initialise driver
|
||||
void init();
|
||||
|
||||
// consume incoming messages from motor, reply with latest motor speed
|
||||
// runs in background thread
|
||||
void thread_main();
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
private:
|
||||
|
||||
// message ids
|
||||
enum class MsgId : uint8_t {
|
||||
SET_MOTOR_SPEED = 0x0,
|
||||
UNKNOWN_0x01 = 0x1,
|
||||
REQUEST_MOTOR_SPEED = 0x14,
|
||||
UNKNOWN_0x20 = 0x20,
|
||||
UNKNOWN_0x30 = 0x30,
|
||||
};
|
||||
|
||||
enum class ParseState {
|
||||
WAITING_FOR_HEADER = 0,
|
||||
WAITING_FOR_FOOTER,
|
||||
};
|
||||
|
||||
// initialise serial port and gpio pins (run from background thread)
|
||||
// returns true on success
|
||||
bool init_internals();
|
||||
|
||||
// process a single byte received on serial port
|
||||
// return true if a this driver should send a set-motor-speed message
|
||||
bool parse_byte(uint8_t b);
|
||||
|
||||
// set pin to enable sending commands to motor
|
||||
void send_start();
|
||||
|
||||
// check for timeout after sending and unset pin if required
|
||||
void check_for_send_end();
|
||||
|
||||
// calculate delay require to allow bytes to be sent
|
||||
uint32_t calc_send_delay_us(uint8_t num_bytes);
|
||||
|
||||
// send a motor speed command as a value from -1000 to +1000
|
||||
// value is taken directly from SRV_Channel
|
||||
void send_motor_speed_cmd();
|
||||
|
||||
// parameters
|
||||
AP_Int8 _enable; // 1 if torqeedo feature is enabled
|
||||
AP_Int8 _pin_onoff; // Pin number connected to Torqeedo's on/off pin. -1 to disable turning motor on/off from autopilot
|
||||
AP_Int8 _pin_de; // Pin number connected to RS485 to Serial converter's DE pin. -1 to disable sending commands to motor
|
||||
|
||||
// members
|
||||
AP_HAL::UARTDriver *_uart; // serial port to communicate with motor
|
||||
bool _initialised; // true once driver has been initialised
|
||||
int16_t _motor_speed; // desired motor speed (set from within update method)
|
||||
uint32_t _last_send_motor_us; // system time (in micros) last motor speed command was sent
|
||||
uint32_t _send_delay_us; // delay (in micros) to allow bytes to be sent after which pin can be unset. 0 if not delaying
|
||||
|
||||
// message parsing members
|
||||
ParseState _parse_state; // current state of parsing
|
||||
uint32_t _parse_error_count; // total number of parsing errors (for reporting)
|
||||
uint32_t _parse_success_count; // number of messages successfully parsed (for reporting)
|
||||
uint8_t _received_buff[TORQEEDO_MESSAGE_LEN_MAX]; // characters received
|
||||
uint8_t _received_buff_len; // number of characters received
|
||||
|
||||
static AP_Torqeedo *_singleton;
|
||||
};
|
||||
|
||||
namespace AP {
|
||||
AP_Torqeedo *torqeedo();
|
||||
};
|
||||
|
||||
#endif // HAL_TORQEEDO_ENABLED
|
||||
Loading…
Reference in New Issue