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.
110 lines
3.6 KiB
110 lines
3.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/>. |
|
*/ |
|
|
|
#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
|
|
|