Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Torqeedo/AP_Torqeedo.cpp

1156 lines
46 KiB
C++
Raw Blame History

/*
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>
#include <AP_Logger/AP_Logger.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_SerialManager/AP_SerialManager.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 packet footer
#define TORQEEDO_PACKET_ESCAPE 0xAE // escape character for handling occurrences of header, footer and this escape bytes in original message
#define TORQEEDO_PACKET_ESCAPE_MASK 0x80 // byte after ESCAPE character should be XOR'd with this value
#define TORQEEDO_LOG_TRQD_INTERVAL_MS 5000// log TRQD message at this interval in milliseconds
#define TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_MS 100 // motor speed sent at 10hz if connected to motor
#define TORQEEDO_SEND_MOTOR_STATUS_REQUEST_INTERVAL_MS 400 // motor status requested every 0.4sec if connected to motor
#define TORQEEDO_SEND_MOTOR_PARAM_REQUEST_INTERVAL_MS 400 // motor param requested every 0.4sec if connected to motor
#define TORQEEDO_BATT_TIMEOUT_MS 5000 // battery info timeouts after 5 seconds
#define TORQEEDO_REPLY_TIMEOUT_MS 25 // stop waiting for replies after 25ms
#define TORQEEDO_ERROR_REPORT_INTERVAL_MAX_MS 10000 // errors reported to user at no less than once every 10 seconds
extern const AP_HAL::HAL& hal;
// parameters
const AP_Param::GroupInfo AP_Torqeedo::var_info[] = {
// @Param: TYPE
// @DisplayName: Torqeedo connection type
// @Description: Torqeedo connection type
// @Values: 0:Disabled, 1:Tiller, 2:Motor
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO_FLAGS("TYPE", 1, AP_Torqeedo, _type, (int8_t)ConnectionType::TYPE_DISABLED, AP_PARAM_FLAG_ENABLE),
// @Param: ONOFF_PIN
// @DisplayName: Torqeedo ON/Off pin
// @Description: Pin number connected to Torqeedo's on/off pin. -1 to use serial port's RTS pin if available
// @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 use serial port's CTS pin if available
// @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),
// @Param: OPTIONS
// @DisplayName: Torqeedo Options
// @Description: Torqeedo Options Bitmask
// @Bitmask: 0:Log,1:Send debug to GCS
// @User: Advanced
AP_GROUPINFO("OPTIONS", 4, AP_Torqeedo, _options, (int8_t)options::LOG),
// @Param: POWER
// @DisplayName: Torqeedo Motor Power
// @Description: Torqeedo motor power. Only applied when using motor connection type (e.g. TRQD_TYPE=2)
// @Units: %
// @Range: 0 100
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("POWER", 5, AP_Torqeedo, _motor_power, 100),
// @Param: SLEW_TIME
// @DisplayName: Torqeedo Throttle Slew Time
// @Description: Torqeedo slew rate specified as the minimum number of seconds required to increase the throttle from 0 to 100%. Higher values cause a slower response, lower values cause a faster response. A value of zero disables the limit
// @Units: s
// @Range: 0 5
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("SLEW_TIME", 6, AP_Torqeedo, _slew_time, 2.0),
// @Param: DIR_DELAY
// @DisplayName: Torqeedo Direction Change Delay
// @Description: Torqeedo direction change delay. Output will remain at zero for this many seconds when transitioning between forward and backwards rotation
// @Units: s
// @Range: 0 5
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("DIR_DELAY", 7, AP_Torqeedo, _dir_delay, 1.0),
AP_GROUPEND
};
AP_Torqeedo::AP_Torqeedo()
{
_singleton = this;
AP_Param::setup_object_defaults(this, var_info);
}
// initialise driver
void AP_Torqeedo::init()
{
// exit immediately if not enabled
if (!enabled()) {
return;
}
// only init once
// Note: a race condition exists here if init is called multiple times quickly before thread_main has a chance to set _initialise
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);
// if using tiller connection set on/off pin for 0.5 sec to turn on battery
if (_type == ConnectionType::TYPE_TILLER) {
if (_pin_onoff > -1) {
hal.gpio->pinMode(_pin_onoff, HAL_GPIO_OUTPUT);
hal.gpio->write(_pin_onoff, 1);
hal.scheduler->delay(500);
hal.gpio->write(_pin_onoff, 0);
} else {
// use serial port's RTS pin to turn on battery
_uart->set_RTS_pin(true);
hal.scheduler->delay(500);
_uart->set_RTS_pin(false);
}
}
// 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);
} else {
_uart->set_CTS_pin(false);
}
return true;
}
// returns true if the driver is enabled
bool AP_Torqeedo::enabled() const
{
switch ((ConnectionType)_type) {
case ConnectionType::TYPE_DISABLED:
return false;
case ConnectionType::TYPE_TILLER:
case ConnectionType::TYPE_MOTOR:
return true;
}
return false;
}
// 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();
// check for timeout waiting for reply
check_for_reply_timeout();
// parse incoming characters
uint32_t nbytes = MIN(_uart->available(), 1024U);
while (nbytes-- > 0) {
int16_t b = _uart->read();
if (b >= 0 ) {
if (parse_byte((uint8_t)b)) {
// complete message received, parse it!
parse_message();
// clear wait-for-reply because if we are waiting for a reply, this message must be it
set_reply_received();
}
}
}
// send motor speed
bool log_update = false;
if (safe_to_send()) {
uint32_t now_ms = AP_HAL::millis();
// if connected to motor
if (_type == ConnectionType::TYPE_MOTOR) {
if (now_ms - _last_send_motor_ms > TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_MS) {
// send motor speed every 0.1sec
_send_motor_speed = true;
} else if (now_ms - _last_send_motor_status_request_ms > TORQEEDO_SEND_MOTOR_STATUS_REQUEST_INTERVAL_MS) {
// send request for motor status
send_motor_msg_request(MotorMsgId::STATUS);
_last_send_motor_status_request_ms = now_ms;
} else if (now_ms - _last_send_motor_param_request_ms > TORQEEDO_SEND_MOTOR_PARAM_REQUEST_INTERVAL_MS) {
// send request for motor params
send_motor_msg_request(MotorMsgId::PARAM);
_last_send_motor_param_request_ms = now_ms;
}
}
// send motor speed
if (_send_motor_speed) {
send_motor_speed_cmd();
_send_motor_speed = false;
log_update = true;
}
}
// log high level status and motor speed
log_TRQD(log_update);
}
}
// returns true if communicating with the motor
bool AP_Torqeedo::healthy()
{
if (!_initialised) {
return false;
}
{
// healthy if both receive and send have occurred in the last 3 seconds
WITH_SEMAPHORE(_last_healthy_sem);
const uint32_t now_ms = AP_HAL::millis();
return ((now_ms - _last_received_ms < 3000) && (now_ms - _last_send_motor_ms < 3000));
}
}
// run pre-arm check. returns false on failure and fills in failure_msg
// any failure_msg returned will not include a prefix
bool AP_Torqeedo::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
{
// exit immediately if not enabled
if (!enabled()) {
return true;
}
if (!_initialised) {
strncpy(failure_msg, "not initialised", failure_msg_len);
return false;
}
if (!healthy()) {
strncpy(failure_msg, "not healthy", failure_msg_len);
return false;
}
return true;
}
// returns a human-readable string corresponding the passed-in
// master error code (see page 93 of https://media.torqeedo.com/downloads/manuals/torqeedo-Travel-manual-DE-EN.pdf)
// If no conversion is available then nullptr is returned
const char * AP_Torqeedo::map_master_error_code_to_string(uint8_t code) const
{
switch (code) {
case 2:
return "stator high temp";
case 5:
return "propeller blocked";
case 6:
return "motor low voltage";
case 7:
return "motor high current";
case 8:
return "pcb temp high";
case 21:
return "tiller cal bad";
case 22:
return "mag bad";
case 23:
return "range incorrect";
case 30:
return "motor comm error";
case 32:
return "tiller comm error";
case 33:
return "general comm error";
case 41:
case 42:
return "charge voltage bad";
case 43:
return "battery flat";
case 45:
return "battery high current";
case 46:
return "battery temp error";
case 48:
return "charging temp error";
}
return nullptr;
}
// report changes in error codes to user
void AP_Torqeedo::report_error_codes()
{
// skip reporting if we have already reported status very recently
const uint32_t now_ms = AP_HAL::millis();
// skip reporting if no changes in flags and already reported within 10 seconds
const bool flags_changed = (_display_system_state_flags_prev.value != _display_system_state.flags.value) ||
(_display_system_state_master_error_code_prev != _display_system_state.master_error_code) ||
(_motor_status_prev.status_flags_value != _motor_status.status_flags_value) ||
(_motor_status_prev.error_flags_value != _motor_status.error_flags_value);
if (!flags_changed && ((now_ms - _last_error_report_ms) < TORQEEDO_ERROR_REPORT_INTERVAL_MAX_MS)) {
return;
}
// report display system errors
const char* msg_prefix = "Torqeedo:";
if (_display_system_state.flags.set_throttle_stop) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s zero throttle required", msg_prefix);
}
if (_display_system_state.flags.temp_warning) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high temp", msg_prefix);
}
if (_display_system_state.flags.temp_warning) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s batt nearly empty", msg_prefix);
}
if (_display_system_state.master_error_code > 0) {
const char *error_string = map_master_error_code_to_string(_display_system_state.master_error_code);
if (error_string != nullptr) {
GCS_SEND_TEXT(
MAV_SEVERITY_CRITICAL, "%s err:%u %s",
msg_prefix,
_display_system_state.master_error_code,
error_string);
} else {
GCS_SEND_TEXT(
MAV_SEVERITY_CRITICAL, "%s err:%u",
msg_prefix,
_display_system_state.master_error_code);
}
}
// report motor status errors
if (_motor_status.error_flags.overcurrent) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s overcurrent", msg_prefix);
}
if (_motor_status.error_flags.blocked) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s prop blocked", msg_prefix);
}
if (_motor_status.error_flags.overvoltage_static || _motor_status.error_flags.overvoltage_current) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high voltage", msg_prefix);
}
if (_motor_status.error_flags.undervoltage_static || _motor_status.error_flags.undervoltage_current) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s low voltage", msg_prefix);
}
if (_motor_status.error_flags.overtemp_motor || _motor_status.error_flags.overtemp_pcb) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high temp", msg_prefix);
}
if (_motor_status.error_flags.timeout_rs485) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s comm timeout", msg_prefix);
}
if (_motor_status.error_flags.temp_sensor_error) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s temp sensor err", msg_prefix);
}
if (_motor_status.error_flags.tilt) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s tilted", msg_prefix);
}
// display OK if all errors cleared
const bool prev_errored = (_display_system_state_flags_prev.value != 0) ||
(_display_system_state_master_error_code_prev != 0) ||
(_motor_status_prev.error_flags_value != 0);
const bool now_errored = (_display_system_state.flags.value != 0) ||
(_display_system_state.master_error_code != 0) ||
(_motor_status.error_flags_value != 0);
if (!now_errored && prev_errored) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s OK", msg_prefix);
}
// record change in state and reporting time
_display_system_state_flags_prev.value = _display_system_state.flags.value;
_display_system_state_master_error_code_prev = _display_system_state.master_error_code;
_motor_status_prev = _motor_status;
_last_error_report_ms = now_ms;
}
// get latest battery status info. returns true on success and populates arguments
bool AP_Torqeedo::get_batt_info(float &voltage, float &current_amps, float &temp_C, uint8_t &pct_remaining) const
{
// use battery info from display_system_state if available (tiller connection)
if ((AP_HAL::millis() - _display_system_state.last_update_ms) <= TORQEEDO_BATT_TIMEOUT_MS) {
voltage = _display_system_state.batt_voltage;
current_amps = _display_system_state.batt_current;
temp_C = MAX(_display_system_state.temp_sw, _display_system_state.temp_rp);
pct_remaining = _display_system_state.batt_charge_pct;
return true;
}
// use battery info from motor_param if available (motor connection)
if ((AP_HAL::millis() - _motor_param.last_update_ms) <= TORQEEDO_BATT_TIMEOUT_MS) {
voltage = _motor_param.voltage;
current_amps = _motor_param.current;
temp_C = MAX(_motor_param.pcb_temp, _motor_param.stator_temp);
pct_remaining = 0; // motor does not know percent remaining
return true;
}
return false;
}
// get battery capacity. returns true on success and populates argument
bool AP_Torqeedo::get_batt_capacity_Ah(uint16_t &amp_hours) const
{
if (_display_system_setup.batt_capacity == 0) {
return false;
}
amp_hours = _display_system_setup.batt_capacity;
return true;
}
// process a single byte received on serial port
// return true if a complete message has been received (the message will be held in _received_buff)
bool AP_Torqeedo::parse_byte(uint8_t b)
{
bool complete_msg_received = false;
switch (_parse_state) {
case ParseState::WAITING_FOR_HEADER:
if (b == TORQEEDO_PACKET_HEADER) {
_parse_state = ParseState::WAITING_FOR_FOOTER;
}
_received_buff_len = 0;
_parse_escape_received = false;
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++;
{
// record time of successful receive for health reporting
WITH_SEMAPHORE(_last_healthy_sem);
_last_received_ms = AP_HAL::millis();
}
complete_msg_received = true;
} else {
// escape character handling
if (_parse_escape_received) {
b ^= TORQEEDO_PACKET_ESCAPE_MASK;
_parse_escape_received = false;
} else if (b == TORQEEDO_PACKET_ESCAPE) {
// escape character received, record this and ignore this byte
_parse_escape_received = true;
break;
}
// 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 complete_msg_received;
}
// process message held in _received_buff
void AP_Torqeedo::parse_message()
{
// message address (i.e. target of message)
const MsgAddress msg_addr = (MsgAddress)_received_buff[0];
// handle messages sent to "remote" (aka tiller)
if ((_type == ConnectionType::TYPE_TILLER) && (msg_addr == MsgAddress::REMOTE1)) {
RemoteMsgId msg_id = (RemoteMsgId)_received_buff[1];
if (msg_id == RemoteMsgId::REMOTE) {
// request received to send updated motor speed
_send_motor_speed = true;
}
return;
}
// handle messages sent to Display
if ((_type == ConnectionType::TYPE_TILLER) && (msg_addr == MsgAddress::DISPLAY)) {
DisplayMsgId msg_id = (DisplayMsgId)_received_buff[1];
switch (msg_id) {
case DisplayMsgId::SYSTEM_STATE :
if (_received_buff_len == 30) {
// fill in _display_system_state
_display_system_state.flags.value = UINT16_VALUE(_received_buff[2], _received_buff[3]);
_display_system_state.master_state = _received_buff[4]; // deprecated
_display_system_state.master_error_code = _received_buff[5];
_display_system_state.motor_voltage = UINT16_VALUE(_received_buff[6], _received_buff[7]) * 0.01;
_display_system_state.motor_current = UINT16_VALUE(_received_buff[8], _received_buff[9]) * 0.1;
_display_system_state.motor_power = UINT16_VALUE(_received_buff[10], _received_buff[11]);
_display_system_state.motor_rpm = (int16_t)UINT16_VALUE(_received_buff[12], _received_buff[13]);
_display_system_state.motor_pcb_temp = _received_buff[14];
_display_system_state.motor_stator_temp = _received_buff[15];
_display_system_state.batt_charge_pct = _received_buff[16];
_display_system_state.batt_voltage = UINT16_VALUE(_received_buff[17], _received_buff[18]) * 0.01;
_display_system_state.batt_current = UINT16_VALUE(_received_buff[19], _received_buff[20]) * 0.1;
_display_system_state.gps_speed = UINT16_VALUE(_received_buff[21], _received_buff[22]);
_display_system_state.range_miles = UINT16_VALUE(_received_buff[23], _received_buff[24]);
_display_system_state.range_minutes = UINT16_VALUE(_received_buff[25], _received_buff[26]);
_display_system_state.temp_sw = _received_buff[27];
_display_system_state.temp_rp = _received_buff[28];
_display_system_state.last_update_ms = AP_HAL::millis();
// update esc telem sent to ground station
const uint8_t esc_temp = MAX(_display_system_state.temp_sw, _display_system_state.temp_rp);
const uint8_t motor_temp = MAX(_display_system_state.motor_pcb_temp, _display_system_state.motor_stator_temp);
update_esc_telem(_display_system_state.motor_rpm,
_display_system_state.motor_voltage,
_display_system_state.motor_current,
esc_temp,
motor_temp);
// log data
if ((_options & options::LOG) != 0) {
// @LoggerMessage: TRST
// @Description: Torqeedo System State
// @Field: TimeUS: Time since system startup
// @Field: F: Flags bitmask
// @Field: Err: Master error code
// @Field: MVolt: Motor voltage
// @Field: MCur: Motor current
// @Field: Pow: Motor power
// @Field: RPM: Motor RPM
// @Field: MTemp: Motor Temp (higher of pcb or stator)
// @Field: BPct: Battery charge percentage
// @Field: BVolt: Battery voltage
// @Field: BCur: Battery current
AP::logger().Write("TRST", "TimeUS,F,Err,MVolt,MCur,Pow,RPM,MTemp,BPct,BVolt,BCur", "QHBffHhBBff",
AP_HAL::micros64(),
_display_system_state.flags.value,
_display_system_state.master_error_code,
_display_system_state.motor_voltage,
_display_system_state.motor_current,
_display_system_state.motor_power,
_display_system_state.motor_rpm,
motor_temp,
_display_system_state.batt_charge_pct,
_display_system_state.batt_voltage,
_display_system_state.batt_current);
}
// send to GCS
if ((_options & options::DEBUG_TO_GCS) != 0) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRST F:%u Err:%u MV:%4.1f MC:%4.1f P:%u MT:%d B%%:%d BV:%4.1f BC:%4.1f",
(unsigned)_display_system_state.flags.value,
(unsigned)_display_system_state.master_error_code,
(double)_display_system_state.motor_voltage,
(double)_display_system_state.motor_current,
(unsigned)_display_system_state.motor_power,
(int)motor_temp,
(unsigned)_display_system_state.batt_charge_pct,
(double)_display_system_state.batt_voltage,
(double)_display_system_state.batt_current);
}
// report any errors
report_error_codes();
} else {
// unexpected length
_parse_error_count++;
}
break;
case DisplayMsgId::SYSTEM_SETUP:
if (_received_buff_len == 13) {
// fill in display system setup
_display_system_setup.flags = _received_buff[2];
_display_system_setup.motor_type = _received_buff[3];
_display_system_setup.motor_sw_version = UINT16_VALUE(_received_buff[4], _received_buff[5]);
_display_system_setup.batt_capacity = UINT16_VALUE(_received_buff[6], _received_buff[7]);
_display_system_setup.batt_charge_pct = _received_buff[8];
_display_system_setup.batt_type = _received_buff[9];
_display_system_setup.master_sw_version = UINT16_VALUE(_received_buff[10], _received_buff[11]);
// log data
if ((_options & options::LOG) != 0) {
// @LoggerMessage: TRSE
// @Description: Torqeedo System Setup
// @Field: TimeUS: Time since system startup
// @Field: Flag: Flags
// @Field: MotType: Motor type
// @Field: MotVer: Motor software version
// @Field: BattCap: Battery capacity
// @Field: BattPct: Battery charge percentage
// @Field: BattType: Battery type
// @Field: SwVer: Master software version
AP::logger().Write("TRSE", "TimeUS,Flag,MotType,MotVer,BattCap,BattPct,BattType,SwVer", "QBBHHBBH",
AP_HAL::micros64(),
_display_system_setup.flags,
_display_system_setup.motor_type,
_display_system_setup.motor_sw_version,
_display_system_setup.batt_capacity,
_display_system_setup.batt_charge_pct,
_display_system_setup.batt_type,
_display_system_setup.master_sw_version);
}
// send to GCS
if ((_options & options::DEBUG_TO_GCS) != 0) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRSE:%u F:%u Mot:%u/%u Bat:%u/%u/%u%%",
(unsigned)_display_system_setup.master_sw_version,
(unsigned)_display_system_setup.flags,
(unsigned)_display_system_setup.motor_type,
(unsigned)_display_system_setup.motor_sw_version,
(unsigned)_display_system_setup.batt_type,
(unsigned)_display_system_setup.batt_capacity,
(unsigned)_display_system_setup.batt_charge_pct);
}
} else {
// unexpected length
_parse_error_count++;
}
break;
default:
// ignore message
break;
}
return;
}
// handle reply from motor
if ((_type == ConnectionType::TYPE_MOTOR) && (msg_addr == MsgAddress::BUS_MASTER)) {
// replies strangely do not return the msgid so we must have stored it
MotorMsgId msg_id = (MotorMsgId)_reply_msgid;
switch (msg_id) {
case MotorMsgId::PARAM:
if (_received_buff_len == 15) {
_motor_param.rpm = (int16_t)UINT16_VALUE(_received_buff[2], _received_buff[3]);
_motor_param.power = UINT16_VALUE(_received_buff[4], _received_buff[5]);
_motor_param.voltage = UINT16_VALUE(_received_buff[6], _received_buff[7]) * 0.01;
_motor_param.current = UINT16_VALUE(_received_buff[8], _received_buff[9]) * 0.1;
_motor_param.pcb_temp = (int16_t)UINT16_VALUE(_received_buff[10], _received_buff[11]) * 0.1;
_motor_param.stator_temp = (int16_t)UINT16_VALUE(_received_buff[12], _received_buff[13]) * 0.1;
_motor_param.last_update_ms = AP_HAL::millis();
// update esc telem sent to ground station
update_esc_telem(_motor_param.rpm,
_motor_param.voltage,
_motor_param.current,
_motor_param.pcb_temp, // esc temp
_motor_param.stator_temp); // motor temp
// log data
if ((_options & options::LOG) != 0) {
// @LoggerMessage: TRMP
// @Description: Torqeedo Motor Param
// @Field: TimeUS: Time since system startup
// @Field: RPM: Motor RPM
// @Field: Pow: Motor power
// @Field: Volt: Motor voltage
// @Field: Cur: Motor current
// @Field: ETemp: ESC Temp
// @Field: MTemp: Motor Temp
AP::logger().Write("TRMP", "TimeUS,RPM,Pow,Volt,Cur,ETemp,MTemp", "QhHffff",
AP_HAL::micros64(),
_motor_param.rpm,
_motor_param.power,
_motor_param.voltage,
_motor_param.current,
_motor_param.pcb_temp,
_motor_param.stator_temp);
}
// send to GCS
if ((_options & options::DEBUG_TO_GCS) != 0) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TRMP: rpm:%d p:%u V:%4.1f C:%4.1f PT:%4.1f MT:%4.1f",
(int)_motor_param.rpm,
(unsigned)_motor_param.power,
(double)_motor_param.voltage,
(double)_motor_param.current,
(double)_motor_param.pcb_temp,
(double)_motor_param.stator_temp);
}
} else {
// unexpected length
_parse_error_count++;
}
break;
case MotorMsgId::STATUS:
if (_received_buff_len == 6) {
_motor_status.status_flags_value = _received_buff[2];
_motor_status.error_flags_value = UINT16_VALUE(_received_buff[3], _received_buff[4]);
// log data
if ((_options & options::LOG) != 0) {
// @LoggerMessage: TRMS
// @Description: Torqeedo Motor Status
// @Field: TimeUS: Time since system startup
// @Field: State: Motor status flags
// @Field: Err: Motor error flags
AP::logger().Write("TRMS", "TimeUS,State,Err", "QBHH",
AP_HAL::micros64(),
_motor_status.status_flags_value,
_motor_status.error_flags_value);
}
// send to GCS
if ((_options & options::DEBUG_TO_GCS) != 0) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRMS S:%d Err:%d",
_motor_status.status_flags_value,
_motor_status.error_flags_value);
}
// report any errors
report_error_codes();
} else {
// unexpected length
_parse_error_count++;
}
break;
case MotorMsgId::INFO:
case MotorMsgId::DRIVE:
case MotorMsgId::CONFIG:
// we do not process these replies
break;
default:
// ignore unknown messages
break;
}
}
}
// 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() - _send_start_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;
}
// record msgid of message to wait for and set timer for timeout handling
void AP_Torqeedo::set_expected_reply_msgid(uint8_t msg_id)
{
_reply_msgid = msg_id;
_reply_wait_start_ms = AP_HAL::millis();
}
// check for timeout waiting for reply message
void AP_Torqeedo::check_for_reply_timeout()
{
// return immediately if not waiting for reply
if (_reply_wait_start_ms == 0) {
return;
}
if (AP_HAL::millis() - _reply_wait_start_ms > TORQEEDO_REPLY_TIMEOUT_MS) {
_reply_wait_start_ms = 0;
_parse_error_count++;
}
}
// mark reply received. should be called whenever a message is received regardless of whether we are actually waiting for a reply
void AP_Torqeedo::set_reply_received()
{
_reply_wait_start_ms = 0;
}
// send a message to the motor with the specified message contents
// msg_contents should not include the header, footer or CRC
// returns true on success
bool AP_Torqeedo::send_message(const uint8_t msg_contents[], uint8_t num_bytes)
{
// buffer for outgoing message
uint8_t send_buff[TORQEEDO_MESSAGE_LEN_MAX];
uint8_t send_buff_num_bytes = 0;
// calculate crc
const uint8_t crc = crc8_maxim(msg_contents, num_bytes);
// add header
send_buff[send_buff_num_bytes++] = TORQEEDO_PACKET_HEADER;
// add contents
for (uint8_t i=0; i<num_bytes; i++) {
if (!add_byte_to_message(msg_contents[i], send_buff, ARRAY_SIZE(send_buff), send_buff_num_bytes)) {
_parse_error_count++;
return false;
}
}
// add crc
if (!add_byte_to_message(crc, send_buff, ARRAY_SIZE(send_buff), send_buff_num_bytes)) {
_parse_error_count++;
return false;
}
// add footer
if (send_buff_num_bytes >= ARRAY_SIZE(send_buff)) {
_parse_error_count++;
return false;
}
send_buff[send_buff_num_bytes++] = TORQEEDO_PACKET_FOOTER;
// set send pin
send_start();
// write message
_uart->write(send_buff, send_buff_num_bytes);
// record start and expected delay to send message
_send_start_us = AP_HAL::micros();
_send_delay_us = calc_send_delay_us(send_buff_num_bytes);
return true;
}
// add a byte to a message buffer including adding the escape character (0xAE) if necessary
// this should only be used when adding the contents to the buffer, not the header and footer
// num_bytes is updated to the next free byte
bool AP_Torqeedo::add_byte_to_message(uint8_t byte_to_add, uint8_t msg_buff[], uint8_t msg_buff_size, uint8_t &num_bytes) const
{
bool escape_required = (byte_to_add == TORQEEDO_PACKET_HEADER ||
byte_to_add == TORQEEDO_PACKET_FOOTER ||
byte_to_add == TORQEEDO_PACKET_ESCAPE);
// check if we have enough space
if (num_bytes + (escape_required ? 2 : 1) >= msg_buff_size) {
return false;
}
// add byte
if (escape_required) {
msg_buff[num_bytes++] = TORQEEDO_PACKET_ESCAPE;
msg_buff[num_bytes++] = byte_to_add ^ TORQEEDO_PACKET_ESCAPE_MASK;
} else {
msg_buff[num_bytes++] = byte_to_add;
}
return true;
}
// Example "Remote (0x01)" reply message to allow tiller to control motor speed
// Byte Field Definition Example Value Comments
// ---------------------------------------------------------------------------------
// byte 0 Header 0xAC
// byte 1 TargetAddress 0x00 see MsgAddress enum
// byte 2 Message ID 0x00 only master populates this. replies have this set to zero
// byte 3 Flags 0x05 bit0=pin present, bit2=motor speed valid
// byte 4 Status 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 tiller handle forwards: "AC 00 00 05 00 00 ED 95 AD" (+237)
// example message when rotating tiller handle 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
// for tiller connection this sends the "Remote (0x01)" message
// for motor connection this sends the "Motor Drive (0x82)" message
void AP_Torqeedo::send_motor_speed_cmd()
{
// calculate desired motor speed
if (!hal.util->get_soft_armed()) {
_motor_speed_desired = 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_desired = constrain_int16(SRV_Channels::get_output_norm(SRV_Channel::Aux_servo_function_t::k_throttle) * 1000.0, -1000, 1000);
}
// updated limited motor speed
int16_t mot_speed_limited = calc_motor_speed_limited(_motor_speed_desired);
// by default use tiller connection command
uint8_t mot_speed_cmd_buff[] = {(uint8_t)MsgAddress::BUS_MASTER, 0x0, 0x5, 0x0, HIGHBYTE(mot_speed_limited), LOWBYTE(mot_speed_limited)};
// update message if using motor connection
if (_type == ConnectionType::TYPE_MOTOR) {
const uint8_t motor_power = (uint8_t)constrain_int16(_motor_power, 0, 100);
mot_speed_cmd_buff[0] = (uint8_t)MsgAddress::MOTOR;
mot_speed_cmd_buff[1] = (uint8_t)MotorMsgId::DRIVE;
mot_speed_cmd_buff[2] = (mot_speed_limited == 0 ? 0 : 0x01) | (_motor_clear_error ? 0x04 : 0); // 1:enable motor, 2:fast off, 4:clear error
mot_speed_cmd_buff[3] = mot_speed_limited == 0 ? 0 : motor_power; // motor power from 0 to 100
// set expected reply message id
set_expected_reply_msgid((uint8_t)MotorMsgId::DRIVE);
// reset motor clear error request
_motor_clear_error = false;
}
// send a message
if (send_message(mot_speed_cmd_buff, ARRAY_SIZE(mot_speed_cmd_buff))) {
// record time of send for health reporting
WITH_SEMAPHORE(_last_healthy_sem);
_last_send_motor_ms = AP_HAL::millis();
}
}
// send request to motor to reply with a particular message
// msg_id can be INFO, STATUS or PARAM
void AP_Torqeedo::send_motor_msg_request(MotorMsgId msg_id)
{
// prepare message
uint8_t mot_status_request_buff[] = {(uint8_t)MsgAddress::MOTOR, (uint8_t)msg_id};
// send a message
if (send_message(mot_status_request_buff, ARRAY_SIZE(mot_status_request_buff))) {
// record waiting for reply
set_expected_reply_msgid((uint8_t)msg_id);
}
}
// calculate the limited motor speed that is sent to the motors
// desired_motor_speed argument and returned value are in the range -1000 to 1000
int16_t AP_Torqeedo::calc_motor_speed_limited(int16_t desired_motor_speed)
{
const uint32_t now_ms = AP_HAL::millis();
// update dir_limit flag for forward-reverse transition delay
const bool dir_delay_active = is_positive(_dir_delay);
if (!dir_delay_active) {
// allow movement in either direction
_dir_limit = 0;
} else {
// by default limit motor direction to previous iteration's direction
if (is_positive(_motor_speed_limited)) {
_dir_limit = 1;
} else if (is_negative(_motor_speed_limited)) {
_dir_limit = -1;
} else {
// motor speed is zero
if ((_motor_speed_zero_ms != 0) && ((now_ms - _motor_speed_zero_ms) > (_dir_delay * 1000))) {
// delay has passed so allow movement in either direction
_dir_limit = 0;
_motor_speed_zero_ms = 0;
}
}
}
// calculate upper and lower limits for forward-reverse transition delay
int16_t lower_limit = -1000;
int16_t upper_limit = 1000;
if (_dir_limit < 0) {
upper_limit = 0;
}
if (_dir_limit > 0) {
lower_limit = 0;
}
// calculate dt since last update
float dt = (now_ms - _motor_speed_limited_ms) * 0.001f;
if (dt > 1.0) {
// after a long delay limit motor output to zero to avoid sudden starts
lower_limit = 0;
upper_limit = 0;
}
_motor_speed_limited_ms = now_ms;
// apply slew limit
if (_slew_time > 0) {
const float chg_max = 1000.0 * dt / _slew_time;
_motor_speed_limited = constrain_float(desired_motor_speed, _motor_speed_limited - chg_max, _motor_speed_limited + chg_max);
} else {
// no slew limit
_motor_speed_limited = desired_motor_speed;
}
// apply upper and lower limits
_motor_speed_limited = constrain_float(_motor_speed_limited, lower_limit, upper_limit);
// record time motor speed becomes zero
if (is_zero(_motor_speed_limited)) {
if (_motor_speed_zero_ms == 0) {
_motor_speed_zero_ms = now_ms;
}
} else {
// clear timer
_motor_speed_zero_ms = 0;
}
return (int16_t)_motor_speed_limited;
}
// output logging and debug messages (if required)
// force_logging should be true if caller wants to ensure the latest status is logged
void AP_Torqeedo::log_TRQD(bool force_logging)
{
// exit immediately if options are all unset
if (_options == 0) {
return;
}
// return if not enough time has passed since last output
const uint32_t now_ms = AP_HAL::millis();
if (!force_logging && (now_ms - _last_log_TRQD_ms < TORQEEDO_LOG_TRQD_INTERVAL_MS)) {
return;
}
_last_log_TRQD_ms = now_ms;
const bool health = healthy();
int16_t actual_motor_speed = get_motor_speed_limited();
if ((_options & options::LOG) != 0) {
// @LoggerMessage: TRQD
// @Description: Torqeedo Status
// @Field: TimeUS: Time since system startup
// @Field: Health: Health
// @Field: DesMotSpeed: Desired Motor Speed (-1000 to 1000)
// @Field: MotSpeed: Motor Speed (-1000 to 1000)
// @Field: SuccCnt: Success Count
// @Field: ErrCnt: Error Count
AP::logger().Write("TRQD", "TimeUS,Health,DesMotSpeed,MotSpeed,SuccCnt,ErrCnt", "QBhhII",
AP_HAL::micros64(),
health,
_motor_speed_desired,
actual_motor_speed,
_parse_success_count,
_parse_error_count);
}
if ((_options & options::DEBUG_TO_GCS) != 0) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRQD h:%u dspd:%d spd:%d succ:%ld err:%ld",
(unsigned)health,
(int)_motor_speed_desired,
(int)actual_motor_speed,
(unsigned long)_parse_success_count,
(unsigned long)_parse_error_count);
}
}
// send ESC telemetry
void AP_Torqeedo::update_esc_telem(float rpm, float voltage, float current_amps, float esc_tempC, float motor_tempC)
{
#if HAL_WITH_ESC_TELEM
// find servo output channel
uint8_t telem_esc_index = 0;
IGNORE_RETURN(SRV_Channels::find_channel(SRV_Channel::Aux_servo_function_t::k_throttle, telem_esc_index));
// fill in telemetry data structure
AP_ESC_Telem_Backend::TelemetryData telem_dat {};
telem_dat.temperature_cdeg = esc_tempC * 100; // temperature in centi-degrees
telem_dat.voltage = voltage; // voltage in volts
telem_dat.current = current_amps; // current in amps
telem_dat.motor_temp_cdeg = motor_tempC * 100; // motor temperature in centi-degrees
// send telem and rpm data
update_telem_data(telem_esc_index, telem_dat, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE |
AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE |
AP_ESC_Telem_Backend::TelemetryType::CURRENT |
AP_ESC_Telem_Backend::TelemetryType::VOLTAGE);
update_rpm(telem_esc_index, rpm);
#endif
}
// 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
Reference in New Issue View Git Blame Copy Permalink