zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Torqeedo: simple driver to control torqeedo 1003 motor 

includes fixes from peer review
gps-1.3.1
Randy Mackay 4 years ago
parent
7e68ce5c86
commit
50ce79bcf6
2 changed files with 419 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 309
      libraries/AP_Torqeedo/AP_Torqeedo.cpp
  2. 110
      libraries/AP_Torqeedo/AP_Torqeedo.h

309
libraries/AP_Torqeedo/AP_Torqeedo.cpp
Unescape View File

@ -0,0 +1,309 @@ @@ -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

110
libraries/AP_Torqeedo/AP_Torqeedo.h
Unescape View File

@ -0,0 +1,110 @@ @@ -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
Write Preview
Loading…
Cancel
Save