mirror of https://github.com/ArduPilot/ardupilot
111 lines
3.6 KiB
C++
111 lines
3.6 KiB
C++
/*
|
|
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
|