diff --git a/libraries/AP_Torqeedo/AP_Torqeedo.cpp b/libraries/AP_Torqeedo/AP_Torqeedo.cpp index 5ff6b672b9..1aac8d689c 100644 --- a/libraries/AP_Torqeedo/AP_Torqeedo.cpp +++ b/libraries/AP_Torqeedo/AP_Torqeedo.cpp @@ -19,86 +19,25 @@ #include #include -#include -#include #include -#include - -#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 +#include "AP_Torqeedo_Backend.h" +#include "AP_Torqeedo_TQBus.h" 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), + // 1 to 7 were used for parameters before frontend/backend split - // @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), + // @Group: 1_ + // @Path: AP_Torqeedo_Params.cpp + AP_SUBGROUPINFO(_params[0], "1_", 8, AP_Torqeedo, AP_Torqeedo_Params), - // @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), +#if AP_TORQEEDO_MAX_INSTANCES > 1 + // @Group: 2_ + // @Path: AP_Torqeedo_Params.cpp + AP_SUBGROUPINFO(_params[1], "2_", 9, AP_Torqeedo, AP_Torqeedo_Params), +#endif AP_GROUPEND }; @@ -112,156 +51,87 @@ AP_Torqeedo::AP_Torqeedo() // 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); + // check init has not been called before + for (uint8_t i = 1; i < AP_TORQEEDO_MAX_INSTANCES; i++) { + if (get_instance(i) != nullptr) { + return; } } - // 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); + // create each instance + uint8_t instance; + for (instance = 0; instance < AP_TORQEEDO_MAX_INSTANCES; instance++) { + switch ((ConnectionType)_params[instance].type.get()) { + case ConnectionType::TYPE_DISABLED: + // do nothing + break; + case ConnectionType::TYPE_TILLER: + case ConnectionType::TYPE_MOTOR: + _backends[instance] = new AP_Torqeedo_TQBus(_params[instance], instance); + break; + } } - return true; + // init each instance, do it after all instances were created, so that they all know things + for (instance = 0; instance < AP_TORQEEDO_MAX_INSTANCES; instance++) { + if (_backends[instance] != nullptr) { + _backends[instance]->init(); + } + } } -// returns true if the driver is enabled +// returns true if at least one backend has been configured (e.g. TYPE param has been set) bool AP_Torqeedo::enabled() const { - switch ((ConnectionType)_type) { - case ConnectionType::TYPE_DISABLED: - return false; - case ConnectionType::TYPE_TILLER: - case ConnectionType::TYPE_MOTOR: - return true; + for (uint8_t instance = 0; instance < AP_TORQEEDO_MAX_INSTANCES; instance++) { + if (enabled(instance)) { + return true; + } } - return false; } -// consume incoming messages from motor, reply with latest motor speed -// runs in background thread -void AP_Torqeedo::thread_main() +// returns true if the instance has been configured (e.g. TYPE param has been set) +bool AP_Torqeedo::enabled(uint8_t instance) const { - // 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(); - } - } + if (instance < AP_TORQEEDO_MAX_INSTANCES) { + switch ((ConnectionType)_params[instance].type.get()) { + case ConnectionType::TYPE_DISABLED: + return false; + case ConnectionType::TYPE_TILLER: + case ConnectionType::TYPE_MOTOR: + return true; } - - // 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); } + return false; } -// returns true if communicating with the motor +// returns true if all backends are communicating with the motor bool AP_Torqeedo::healthy() { - if (!_initialised) { + uint8_t num_backends = 0; + uint8_t num_healthy = 0; + for (uint8_t instance = 0; instance < AP_TORQEEDO_MAX_INSTANCES; instance++) { + auto *backend = get_instance(instance); + if (backend != nullptr) { + num_backends++; + if (backend->healthy()) { + num_healthy++; + } + } + } + + return ((num_backends > 0) && (num_healthy == num_backends)); +} + +// returns true if instance is healthy +bool AP_Torqeedo::healthy(uint8_t instance) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { 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)); - } + return backend->healthy(); } // run pre-arm check. returns false on failure and fills in failure_msg @@ -273,10 +143,6 @@ bool AP_Torqeedo::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) 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; @@ -284,870 +150,58 @@ bool AP_Torqeedo::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) 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 +// clear motor errors +void AP_Torqeedo::clear_motor_error() { - 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:"; - (void)msg_prefix; // sometimes unused when HAL_GCS_ENABLED is false - 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.batt_nearly_empty) { - 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); + for (uint8_t instance = 0; instance < AP_TORQEEDO_MAX_INSTANCES; instance++) { + auto *backend = get_instance(instance); + if (backend != nullptr) { + backend->clear_motor_error(); } } - - // 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 ¤t_amps, float &temp_C, uint8_t &pct_remaining) const +// instance is normally 0 or 1, if invalid instances are provided the first instance is used +bool AP_Torqeedo::get_batt_info(uint8_t instance, float &voltage, float ¤t_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; + // for invalid instances use the first instance + if (instance > AP_TORQEEDO_MAX_INSTANCES-1) { + instance = 0; } - // 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 battery info for specified instance + auto *backend = get_instance(instance); + if (backend != nullptr) { + return backend->get_batt_info(voltage, current_amps, temp_C, pct_remaining); } - return false; } // get battery capacity. returns true on success and populates argument -bool AP_Torqeedo::get_batt_capacity_Ah(uint16_t &_hours) const +// instance is normally 0 or 1, if invalid instances are provided the first instance is used +bool AP_Torqeedo::get_batt_capacity_Ah(uint8_t instance, uint16_t &_hours) const { - if (_display_system_setup.batt_capacity == 0) { - return false; + // for invalid instances use the first instance + if (instance > AP_TORQEEDO_MAX_INSTANCES-1) { + instance = 0; } - amp_hours = _display_system_setup.batt_capacity; - return true; + + // return battery capacity from specified instance + auto *backend = get_instance(instance); + if (backend != nullptr) { + return backend->get_batt_capacity_Ah(amp_hours); + } + return false; } -// 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) +// return pointer to backend given an instance number +AP_Torqeedo_Backend *AP_Torqeedo::get_instance(uint8_t instance) const { - 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; + if (instance < AP_TORQEEDO_MAX_INSTANCES) { + return _backends[instance]; } - - 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); - -#if HAL_LOGGING_ENABLED - // 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); - } -#endif - - // 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]); - -#if HAL_LOGGING_ENABLED - // 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); - } -#endif - - // 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 - -#if HAL_LOGGING_ENABLED - // 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); - } -#endif - - // 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]); - -#if HAL_LOGGING_ENABLED - // 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", "QBH", - AP_HAL::micros64(), - _motor_status.status_flags_value, - _motor_status.error_flags_value); - } -#endif - - // 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= 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; - -#if HAL_LOGGING_ENABLED || HAL_GCS_ENABLED - const bool health = healthy(); - int16_t actual_motor_speed = get_motor_speed_limited(); - -#if HAL_LOGGING_ENABLED - 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); - } -#endif - - 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); - } -#endif // HAL_LOGGING_ENABLED || HAL_GCS_ENABLED -} - -// 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 + return nullptr; } // get the AP_Torqeedo singleton @@ -1159,10 +213,11 @@ AP_Torqeedo *AP_Torqeedo::get_singleton() AP_Torqeedo *AP_Torqeedo::_singleton = nullptr; namespace AP { + AP_Torqeedo *torqeedo() { return AP_Torqeedo::get_singleton(); } -}; +} #endif // HAL_TORQEEDO_ENABLED diff --git a/libraries/AP_Torqeedo/AP_Torqeedo.h b/libraries/AP_Torqeedo/AP_Torqeedo.h index 32d2a002c2..f5c4eb1cb8 100644 --- a/libraries/AP_Torqeedo/AP_Torqeedo.h +++ b/libraries/AP_Torqeedo/AP_Torqeedo.h @@ -15,32 +15,7 @@ /* This driver supports communicating with Torqeedo motors that implement the "TQ Bus" protocol - which includes the Ultralight, Cruise 2.0 R, Cruise 4.0 R, Travel 503, Travel 1003 and Cruise 10kW - - The autopilot should be connected either to the battery's tiller connector or directly to the motor - as described on the ArduPilot wiki. https://ardupilot.org/rover/docs/common-torqeedo.html - TQ Bus is a serial protocol over RS-485 meaning that a serial to RS-485 converter is required. - - Tiller connection: Autopilot <-> Battery (master) <-> Motor - Motor connection: Autopilot (master) <-> Motor - - Communication between the components is always initiated by the master with replies sent within 25ms - - 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 - - More details of the TQ Bus protocol are available from Torqeedo after signing an NDA. - */ +*/ #pragma once @@ -48,84 +23,30 @@ #if HAL_TORQEEDO_ENABLED -#include #include +#include "AP_Torqeedo_Params.h" -#define TORQEEDO_MESSAGE_LEN_MAX 35 // messages are no more than 35 bytes +#define AP_TORQEEDO_MAX_INSTANCES 2 // maximum number of Torqeedo backends + +// declare backend classes +class AP_Torqeedo_Backend; +class AP_Torqeedo_TQBus; + +class AP_Torqeedo { + + // declare backends as friends + friend class AP_Torqeedo_Backend; + friend class AP_Torqeedo_TQBus; -class AP_Torqeedo : public AP_ESC_Telem_Backend { public: AP_Torqeedo(); + /* Do not allow copies */ CLASS_NO_COPY(AP_Torqeedo); + // get singleton instance 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(); - - // returns true if communicating with the motor - bool healthy(); - - // run pre-arm check. returns false on failure and fills in failure_msg - // any failure_msg returned will not include a prefix - bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len); - - // report changes in error codes to user - void report_error_codes(); - - // clear motor errors - void clear_motor_error() { _motor_clear_error = true; } - - // get latest battery status info. returns true on success and populates arguments - bool get_batt_info(float &voltage, float ¤t_amps, float &temp_C, uint8_t &pct_remaining) const WARN_IF_UNUSED; - bool get_batt_capacity_Ah(uint16_t &_hours) const; - - static const struct AP_Param::GroupInfo var_info[]; - -private: - - // message addresses - enum class MsgAddress : uint8_t { - BUS_MASTER = 0x00, - REMOTE1 = 0x14, - DISPLAY = 0x20, - MOTOR = 0x30, - BATTERY = 0x80 - }; - - // Remote specific message ids - enum class RemoteMsgId : uint8_t { - INFO = 0x00, - REMOTE = 0x01, - SETUP = 0x02 - }; - - // Display specific message ids - enum class DisplayMsgId : uint8_t { - INFO = 0x00, - SYSTEM_STATE = 0x41, - SYSTEM_SETUP = 0x42 - }; - - // Motor specific message ids - enum class MotorMsgId : uint8_t { - INFO = 0x00, - STATUS = 0x01, - PARAM = 0x03, - CONFIG = 0x04, - DRIVE = 0x82 - }; - - enum class ParseState { - WAITING_FOR_HEADER = 0, - WAITING_FOR_FOOTER, - }; - // TYPE parameter values enum class ConnectionType : uint8_t { TYPE_DISABLED = 0, @@ -134,232 +55,47 @@ private: }; // OPTIONS parameter values - enum options { + enum class options { LOG = 1<<0, DEBUG_TO_GCS = 1<<1, }; - // initialise serial port and gpio pins (run from background thread) - // returns true on success - bool init_internals(); + // initialise driver + void init(); - // returns true if the driver is enabled + // returns true if at least one backend has been configured (e.g. TYPE param has been set) bool enabled() const; + bool enabled(uint8_t instance) const; - // 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 parse_byte(uint8_t b); + // returns true if all backends are communicating with the motor + bool healthy(); + bool healthy(uint8_t instance); - // process message held in _received_buff - void parse_message(); + // run pre-arm check. returns false on failure and fills in failure_msg + // any failure_msg returned will not include a prefix + bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len); - // returns true if it is safe to send a message - bool safe_to_send() const { return ((_send_delay_us == 0) && (_reply_wait_start_ms == 0)); } + // clear motor errors + void clear_motor_error(); - // set pin to enable sending a message - void send_start(); + // get latest battery status info. returns true on success and populates arguments + // instance is normally 0 or 1, if invalid instances are provided the first instance is used + bool get_batt_info(uint8_t instance, float &voltage, float ¤t_amps, float &temp_C, uint8_t &pct_remaining) const WARN_IF_UNUSED; + bool get_batt_capacity_Ah(uint8_t instance, uint16_t &_hours) const; - // check for timeout after sending a message and unset pin if required - void check_for_send_end(); + // parameter var table + static const struct AP_Param::GroupInfo var_info[]; - // calculate delay required to allow message to be completely sent - uint32_t calc_send_delay_us(uint8_t num_bytes); + // parameters for backends + AP_Torqeedo_Params _params[AP_TORQEEDO_MAX_INSTANCES]; - // record msgid of message to wait for and set timer for reply timeout handling - void set_expected_reply_msgid(uint8_t msg_id); +private: - // check for timeout waiting for reply - void check_for_reply_timeout(); + // return pointer to backend given an instance number + AP_Torqeedo_Backend *get_instance(uint8_t instance) const; - // mark reply received. should be called whenever a message is received regardless of whether we are actually waiting for a reply - void set_reply_received(); - - // 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 send_message(const uint8_t msg_contents[], uint8_t num_bytes); - - // 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 add_byte_to_message(uint8_t byte_to_add, uint8_t msg_buff[], uint8_t msg_buff_size, uint8_t &num_bytes) const; - - // send a motor speed command as a value from -1000 to +1000 - // value is taken directly from SRV_Channel - void send_motor_speed_cmd(); - - // send request to motor to reply with a particular message - // msg_id can be INFO, STATUS or PARAM - void send_motor_msg_request(MotorMsgId 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 calc_motor_speed_limited(int16_t desired_motor_speed); - int16_t get_motor_speed_limited() const { return (int16_t)_motor_speed_limited; } - - // log TRQD message which holds high level status and latest desired motors peed - // force_logging should be true to immediately write log bypassing timing check to avoid spamming - void log_TRQD(bool force_logging); - - // send ESC telemetry - void update_esc_telem(float rpm, float voltage, float current_amps, float esc_tempC, float motor_tempC); - - // parameters - AP_Enum _type; // connector type used (0:disabled, 1:tiller connector, 2: motor connector) - 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 - AP_Int16 _options; // options bitmask - AP_Int8 _motor_power; // motor power (0 ~ 100). only applied when using motor connection - AP_Float _slew_time; // slew rate specified as the minimum number of seconds required to increase the throttle from 0 to 100%. A value of zero disables the limit - AP_Float _dir_delay; // direction change delay. output will remain at zero for this many seconds when transitioning between forward and backwards rotation - - // members - AP_HAL::UARTDriver *_uart; // serial port to communicate with motor - bool _initialised; // true once driver has been initialised - bool _send_motor_speed; // true if motor speed should be sent at next opportunity - int16_t _motor_speed_desired; // desired motor speed (set from within update method) - uint32_t _last_send_motor_ms; // system time (in millis) last motor speed command was sent (used for health reporting) - bool _motor_clear_error; // true if the motor error should be cleared (sent in "Drive" message) - uint32_t _send_start_us; // system time (in micros) when last message started being sent (used for timing to unset DE pin) - uint32_t _send_delay_us; // delay (in micros) to allow bytes to be sent after which pin can be unset. 0 if not delaying - - // motor speed limit variables - float _motor_speed_limited; // limited desired motor speed. this value is actually sent to the motor - uint32_t _motor_speed_limited_ms; // system time that _motor_speed_limited was last updated - int8_t _dir_limit; // acceptable directions for output to motor (+1 = positive OK, -1 = negative OK, 0 = either positive or negative OK) - uint32_t _motor_speed_zero_ms; // system time that _motor_speed_limited reached zero. 0 if currently not zero - - // health reporting - HAL_Semaphore _last_healthy_sem;// semaphore protecting reading and updating of _last_send_motor_ms and _last_received_ms - uint32_t _last_log_TRQD_ms; // system time (in millis) that TRQD was last logged - - // message parsing members - ParseState _parse_state; // current state of parsing - bool _parse_escape_received; // true if the escape character has been received so we must XOR the next byte - 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 - uint32_t _last_received_ms; // system time (in millis) that a message was successfully parsed (for health reporting) - - // reply message handling - uint8_t _reply_msgid; // replies expected msgid (reply often does not specify the msgid so we must record it) - uint32_t _reply_wait_start_ms; // system time that we started waiting for a reply message - - // Display system state flags - typedef union PACKED { - struct { - uint8_t set_throttle_stop : 1; // 0, warning that user must set throttle to stop before motor can run - uint8_t setup_allowed : 1; // 1, remote is allowed to enter setup mode - uint8_t in_charge : 1; // 2, master is in charging state - uint8_t in_setup : 1; // 3, master is in setup state - uint8_t bank_available : 1; // 4 - uint8_t no_menu : 1; // 5 - uint8_t menu_off : 1; // 6 - uint8_t reserved7 : 1; // 7, unused - uint8_t temp_warning : 1; // 8, motor or battery temp warning - uint8_t batt_charge_valid : 1; // 9, battery charge valid - uint8_t batt_nearly_empty : 1; // 10, battery nearly empty - uint8_t batt_charging : 1; // 11, battery charging - uint8_t gps_searching : 1; // 12, gps searching for satellites - uint8_t gps_speed_valid : 1; // 13, gps speed is valid - uint8_t range_miles_valid : 1; // 14, range (in miles) is valid - uint8_t range_minutes_valid : 1; // 15, range (in minutes) is valid - }; - uint16_t value; - } DisplaySystemStateFlags; - - // Display system state - struct DisplaySystemState { - DisplaySystemStateFlags flags; // flags, see above for individual bit definitions - uint8_t master_state; // deprecated - uint8_t master_error_code; // error code (0=no error) - float motor_voltage; // motor voltage in volts - float motor_current; // motor current in amps - uint16_t motor_power; // motor power in watts - int16_t motor_rpm; // motor speed in rpm - uint8_t motor_pcb_temp; // motor pcb temp in C - uint8_t motor_stator_temp; // motor stator temp in C - uint8_t batt_charge_pct; // battery state of charge (0 to 100%) - float batt_voltage; // battery voltage in volts - float batt_current; // battery current in amps - uint16_t gps_speed; // gps speed in knots * 100 - uint16_t range_miles; // range in nautical miles * 10 - uint16_t range_minutes; // range in minutes (at current speed and current draw) - uint8_t temp_sw; // master PCB temp in C (close to motor power switches) - uint8_t temp_rp; // master PCB temp in C (close to reverse voltage protection) - uint32_t last_update_ms; // system time that system state was last updated - } _display_system_state; - - // Display system setup - struct DisplaySystemSetup { - uint8_t flags; // 0 : battery config valid, all other bits unused - uint8_t motor_type; // motor type (0 or 3:Unknown, 1:Ultralight, 2:Cruise2, 4:Cruise4, 5:Travel503, 6:Travel1003, 7:Cruise10kW) - uint16_t motor_sw_version; // motor software version - uint16_t batt_capacity; // battery capacity in amp hours - uint8_t batt_charge_pct; // battery state of charge (0 to 100%) - uint8_t batt_type; // battery type (0:lead acid, 1:Lithium) - uint16_t master_sw_version; // master software version - } _display_system_setup; - - // Motor status - struct MotorStatus { - union PACKED { - struct { - uint8_t temp_limit_motor : 1; // 0, motor speed limited due to motor temp - uint8_t temp_limit_pcb : 1; // 1, motor speed limited tue to PCB temp - uint8_t emergency_stop : 1; // 2, motor in emergency stop (must be cleared by master) - uint8_t running : 1; // 3, motor running - uint8_t power_limit : 1; // 4, motor power limited - uint8_t low_voltage_limit : 1; // 5, motor speed limited because of low voltage - uint8_t tilt : 1; // 6, motor is tilted - uint8_t reserved7 : 1; // 7, unused (always zero) - } status_flags; - uint8_t status_flags_value; - }; - union PACKED { - struct { - uint8_t overcurrent : 1; // 0, motor stopped because of overcurrent - uint8_t blocked : 1; // 1, motor stopped because it is blocked - uint8_t overvoltage_static : 1; // 2, motor stopped because voltage too high - uint8_t undervoltage_static : 1; // 3, motor stopped because voltage too low - uint8_t overvoltage_current : 1; // 4, motor stopped because voltage spiked high - uint8_t undervoltage_current: 1; // 5, motor stopped because voltage spiked low - uint8_t overtemp_motor : 1; // 6, motor stopped because stator temp too high - uint8_t overtemp_pcb : 1; // 7, motor stopped because pcb temp too high - uint8_t timeout_rs485 : 1; // 8, motor stopped because Drive message not received for too long - uint8_t temp_sensor_error : 1; // 9, motor temp sensor is defective (motor will not stop) - uint8_t tilt : 1; // 10, motor stopped because it was tilted - uint8_t unused11to15 : 5; // 11 ~ 15 (always zero) - } error_flags; - uint16_t error_flags_value; - }; - } _motor_status; - uint32_t _last_send_motor_status_request_ms; // system time (in milliseconds) that last motor status request was sent - - // Motor params - struct MotorParam { - int16_t rpm; // motor rpm - uint16_t power; // motor power consumption in Watts - float voltage; // motor voltage in volts - float current; // motor current in amps - float pcb_temp; // pcb temp in C - float stator_temp; // stator temp in C - uint32_t last_update_ms;// system time that above values were updated - } _motor_param; - uint32_t _last_send_motor_param_request_ms; // system time (in milliseconds) that last motor param request was sent - - // error reporting - DisplaySystemStateFlags _display_system_state_flags_prev; // backup of display system state flags - uint8_t _display_system_state_master_error_code_prev; // backup of display system state master_error_code - uint32_t _last_error_report_ms; // system time that flag changes were last reported (used to prevent spamming user) - MotorStatus _motor_status_prev; // backup of motor status static AP_Torqeedo *_singleton; - - // 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 *map_master_error_code_to_string(uint8_t code) const; + AP_Torqeedo_Backend *_backends[AP_TORQEEDO_MAX_INSTANCES]; // pointers to instantiated backends }; namespace AP { diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_Backend.cpp b/libraries/AP_Torqeedo/AP_Torqeedo_Backend.cpp new file mode 100644 index 0000000000..243f7cab23 --- /dev/null +++ b/libraries/AP_Torqeedo/AP_Torqeedo_Backend.cpp @@ -0,0 +1,31 @@ +/* + 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 . + */ + +#include "AP_Torqeedo_Backend.h" + +#if HAL_TORQEEDO_ENABLED + +#include +#include + +extern const AP_HAL::HAL& hal; + +// constructor +AP_Torqeedo_Backend::AP_Torqeedo_Backend(AP_Torqeedo_Params ¶ms, uint8_t instance) : + _params(params), + _instance(instance) +{} + +#endif // HAL_TORQEEDO_ENABLED diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_Backend.h b/libraries/AP_Torqeedo/AP_Torqeedo_Backend.h new file mode 100644 index 0000000000..fd56d0612a --- /dev/null +++ b/libraries/AP_Torqeedo/AP_Torqeedo_Backend.h @@ -0,0 +1,84 @@ +/* + 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 . + */ + +/* + This driver supports communicating with Torqeedo motors that implement the "TQ Bus" protocol + which includes the Ultralight, Cruise 2.0 R, Cruise 4.0 R, Travel 503, Travel 1003 and Cruise 10kW + + The autopilot should be connected either to the battery's tiller connector or directly to the motor + as described on the ArduPilot wiki. https://ardupilot.org/rover/docs/common-torqeedo.html + TQ Bus is a serial protocol over RS-485 meaning that a serial to RS-485 converter is required. + + Tiller connection: Autopilot <-> Battery (master) <-> Motor + Motor connection: Autopilot (master) <-> Motor + + Communication between the components is always initiated by the master with replies sent within 25ms + + 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 + + More details of the TQ Bus protocol are available from Torqeedo after signing an NDA. + */ + +#pragma once + +#include "AP_Torqeedo_config.h" + +#if HAL_TORQEEDO_ENABLED + +#include "AP_Torqeedo.h" +#include + +class AP_Torqeedo_Backend : public AP_ESC_Telem_Backend { +public: + AP_Torqeedo_Backend(AP_Torqeedo_Params ¶ms, uint8_t instance); + + // do not allow copies + CLASS_NO_COPY(AP_Torqeedo_Backend); + + // initialise driver + virtual void init() = 0; + + // returns true if communicating with the motor + virtual bool healthy() = 0; + + // clear motor errors + virtual void clear_motor_error() = 0; + + // get latest battery status info. returns true on success and populates arguments + virtual bool get_batt_info(float &voltage, float ¤t_amps, float &temp_C, uint8_t &pct_remaining) const WARN_IF_UNUSED = 0; + virtual bool get_batt_capacity_Ah(uint16_t &_hours) const = 0; + + protected: + + // parameter helper functions + AP_Torqeedo::ConnectionType get_type() const { return (AP_Torqeedo::ConnectionType)_params.type.get(); } + bool option_enabled(AP_Torqeedo::options opt) const { return ((uint16_t)_params.options.get() & (uint16_t)opt) != 0; } + + AP_Torqeedo_Params &_params; // parameters for this backend + uint8_t _instance; // this instance's number +}; + +#endif // HAL_TORQEEDO_ENABLED diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_Params.cpp b/libraries/AP_Torqeedo/AP_Torqeedo_Params.cpp new file mode 100644 index 0000000000..2ded30ca84 --- /dev/null +++ b/libraries/AP_Torqeedo/AP_Torqeedo_Params.cpp @@ -0,0 +1,79 @@ +#include "AP_Torqeedo_Params.h" +#include + +// table of user settable parameters +const AP_Param::GroupInfo AP_Torqeedo_Params::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_Params, type, 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 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_Params, 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_Params, 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_Params, options, 1), + + // @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_Params, 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_Params, 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_Params, dir_delay, 1.0), + + // @Param: SERVO_FN + // @DisplayName: Torqeedo Servo Output Function + // @Description: Torqeedo Servo Output Function + // @Values: 70:Throttle,73:ThrottleLeft,74:ThrottleRight + // @Increment: 0.1 + // @User: Advanced + AP_GROUPINFO("SERVO_FN", 8, AP_Torqeedo_Params, servo_fn, (int16_t)SRV_Channel::k_throttle), + + AP_GROUPEND +}; + +AP_Torqeedo_Params::AP_Torqeedo_Params(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_Params.h b/libraries/AP_Torqeedo/AP_Torqeedo_Params.h new file mode 100644 index 0000000000..21211de322 --- /dev/null +++ b/libraries/AP_Torqeedo/AP_Torqeedo_Params.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include + +class AP_Torqeedo_Params { +public: + static const struct AP_Param::GroupInfo var_info[]; + + AP_Torqeedo_Params(void); + + /* Do not allow copies */ + CLASS_NO_COPY(AP_Torqeedo_Params); + + // parameters + AP_Int8 type; // connector type used (0:disabled, 1:tiller connector, 2:motor connector) + 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 + AP_Int16 options; // options bitmask + AP_Int8 motor_power; // motor power (0 ~ 100). only applied when using motor connection + AP_Float slew_time; // slew rate specified as the minimum number of seconds required to increase the throttle from 0 to 100%. A value of zero disables the limit + AP_Float dir_delay; // direction change delay. output will remain at zero for this many seconds when transitioning between forward and backwards rotation + AP_Int16 servo_fn; // servo output function number +}; diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.cpp b/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.cpp new file mode 100644 index 0000000000..d90749d4f8 --- /dev/null +++ b/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.cpp @@ -0,0 +1,1077 @@ +/* + 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 . + */ + +#include "AP_Torqeedo_TQBus.h" + +#if HAL_TORQEEDO_ENABLED + +#include +#include +#include +#include +#include +#include +#include + +#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; + +// initialise driver +void AP_Torqeedo_TQBus::init() +{ + // 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 + char thread_name[15]; + hal.util->snprintf(thread_name, sizeof(thread_name), "torqeedo%u", (unsigned)_instance); + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Torqeedo_TQBus::thread_main, void), thread_name, 2048, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) { + return; + } +} + +// returns true if communicating with the motor +bool AP_Torqeedo_TQBus::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)); + } +} + +// initialise serial port and gpio pins (run from background thread) +bool AP_Torqeedo_TQBus::init_internals() +{ + // find serial driver and initialise + const AP_SerialManager &serial_manager = AP::serialmanager(); + _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Torqeedo, _instance); + 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 (get_type() == AP_Torqeedo::ConnectionType::TYPE_TILLER) { + if (_params.pin_onoff > -1) { + hal.gpio->pinMode(_params.pin_onoff, HAL_GPIO_OUTPUT); + hal.gpio->write(_params.pin_onoff, 1); + hal.scheduler->delay(500); + hal.gpio->write(_params.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 (_params.pin_de > -1) { + hal.gpio->pinMode(_params.pin_de, HAL_GPIO_OUTPUT); + hal.gpio->write(_params.pin_de, 0); + } else { + _uart->set_CTS_pin(false); + } + + return true; +} + +// consume incoming messages from motor, reply with latest motor speed +// runs in background thread +void AP_Torqeedo_TQBus::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 (get_type() == AP_Torqeedo::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 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_TQBus::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_TQBus::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:"; + (void)msg_prefix; // sometimes unused when HAL_GCS_ENABLED is false + 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.batt_nearly_empty) { + 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_TQBus::get_batt_info(float &voltage, float ¤t_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_TQBus::get_batt_capacity_Ah(uint16_t &_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_TQBus::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_TQBus::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 ((get_type() == AP_Torqeedo::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 ((get_type() == AP_Torqeedo::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); + +#if HAL_LOGGING_ENABLED + // log data + if (option_enabled(AP_Torqeedo::options::LOG)) { + // @LoggerMessage: TRST + // @Description: Torqeedo System State + // @Field: TimeUS: Time since system startup + // @Field: I: instance + // @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,I,F,Err,MVolt,MCur,Pow,RPM,MTemp,BPct,BVolt,BCur", + "s#--vAWqO%vA", // units + "F---00000000", // multipliers + "QBHBffHhBBff", // formats + AP_HAL::micros64(), + _instance, + _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); + } +#endif + + // send to GCS + if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRST i:%u, F:%u Err:%u MV:%4.1f MC:%4.1f P:%u MT:%d B%%:%d BV:%4.1f BC:%4.1f", + (unsigned)_instance, + (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]); + +#if HAL_LOGGING_ENABLED + // log data + if (option_enabled(AP_Torqeedo::options::LOG)) { + // @LoggerMessage: TRSE + // @Description: Torqeedo System Setup + // @Field: TimeUS: Time since system startup + // @Field: I: instance + // @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,I,Flag,MotType,MotVer,BattCap,BattPct,BattType,SwVer", + "s#---a%--", // units + "F----00--", // multipliers + "QBBBHHBBH", // formats + AP_HAL::micros64(), + _instance, + _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); + } +#endif + + // send to GCS + if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRSE i:%u v:%u F:%u Mot:%u/%u Bat:%u/%u/%u%%", + (unsigned)_instance, + (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 ((get_type() == AP_Torqeedo::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 + +#if HAL_LOGGING_ENABLED + // log data + if (option_enabled(AP_Torqeedo::options::LOG)) { + // @LoggerMessage: TRMP + // @Description: Torqeedo Motor Param + // @Field: TimeUS: Time since system startup + // @Field: I: instance + // @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,I,RPM,Pow,Volt,Cur,ETemp,MTemp", + "s#qWvAOO", // units + "F-000000", // multipliers + "QBhHffff", // formats + AP_HAL::micros64(), + _instance, + _motor_param.rpm, + _motor_param.power, + _motor_param.voltage, + _motor_param.current, + _motor_param.pcb_temp, + _motor_param.stator_temp); + } +#endif + + // send to GCS + if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TRMP i:%u rpm:%d p:%u V:%4.1f C:%4.1f PT:%4.1f MT:%4.1f", + (unsigned)_instance, + (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]); + +#if HAL_LOGGING_ENABLED + // log data + if (option_enabled(AP_Torqeedo::options::LOG)) { + // @LoggerMessage: TRMS + // @Description: Torqeedo Motor Status + // @Field: TimeUS: Time since system startup + // @Field: I: instance + // @Field: State: Motor status flags + // @Field: Err: Motor error flags + AP::logger().Write("TRMS", "TimeUS,I,State,Err", + "s#--", // units + "F---", // multipliers + "QBBH", // formats + AP_HAL::micros64(), + _instance, + _motor_status.status_flags_value, + _motor_status.error_flags_value); + } +#endif + + // send to GCS + if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRMS i:%u S:%d Err:%d", + (unsigned)_instance, + _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_TQBus::send_start() +{ + // set gpio pin or serial port's CTS pin + if (_params.pin_de > -1) { + hal.gpio->write(_params.pin_de, 1); + } else { + _uart->set_CTS_pin(true); + } +} + +// check for timeout after sending and unset pin if required +void AP_Torqeedo_TQBus::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 (_params.pin_de > -1) { + hal.gpio->write(_params.pin_de, 0); + } else { + _uart->set_CTS_pin(false); + } +} + +// calculate delay require to allow bytes to be sent +uint32_t AP_Torqeedo_TQBus::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_TQBus::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_TQBus::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_TQBus::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_TQBus::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= 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_TQBus::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_TQBus::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)_params.servo_fn.get()) * 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 (get_type() == AP_Torqeedo::ConnectionType::TYPE_MOTOR) { + const uint8_t motor_power = (uint8_t)constrain_int16(_params.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_TQBus::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_TQBus::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(_params.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) > (_params.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 (_params.slew_time > 0) { + const float chg_max = 1000.0 * dt / _params.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_TQBus::log_TRQD(bool force_logging) +{ + // exit immediately if logging and debug options are not set + if (!option_enabled(AP_Torqeedo::options::LOG) && option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) { + 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; + +#if HAL_LOGGING_ENABLED || HAL_GCS_ENABLED + const bool health = healthy(); + int16_t actual_motor_speed = get_motor_speed_limited(); + +#if HAL_LOGGING_ENABLED + if (option_enabled(AP_Torqeedo::options::LOG)) { + // @LoggerMessage: TRQD + // @Description: Torqeedo Status + // @Field: TimeUS: Time since system startup + // @Field: I: instance + // @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,I,Health,DesMotSpeed,MotSpeed,SuccCnt,ErrCnt", + "s#-----", // units + "F------", // multipliers + "QBBhhII", // formats + AP_HAL::micros64(), + _instance, + health, + _motor_speed_desired, + actual_motor_speed, + _parse_success_count, + _parse_error_count); + } +#endif + + if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRQD i:%u h:%u dspd:%d spd:%d succ:%ld err:%ld", + (unsigned)_instance, + (unsigned)health, + (int)_motor_speed_desired, + (int)actual_motor_speed, + (unsigned long)_parse_success_count, + (unsigned long)_parse_error_count); + } +#endif // HAL_LOGGING_ENABLED || HAL_GCS_ENABLED +} + +// send ESC telemetry +void AP_Torqeedo_TQBus::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)_params.servo_fn.get(), 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 +} + +#endif // HAL_TORQEEDO_ENABLED diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.h b/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.h new file mode 100644 index 0000000000..1b8dfc560a --- /dev/null +++ b/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.h @@ -0,0 +1,332 @@ +/* + 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 . + */ + +/* + This driver supports communicating with Torqeedo motors that implement the "TQ Bus" protocol + which includes the Ultralight, Cruise 2.0 R, Cruise 4.0 R, Travel 503, Travel 1003 and Cruise 10kW + + The autopilot should be connected either to the battery's tiller connector or directly to the motor + as described on the ArduPilot wiki. https://ardupilot.org/rover/docs/common-torqeedo.html + TQ Bus is a serial protocol over RS-485 meaning that a serial to RS-485 converter is required. + + Tiller connection: Autopilot <-> Battery (master) <-> Motor + Motor connection: Autopilot (master) <-> Motor + + Communication between the components is always initiated by the master with replies sent within 25ms + + 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 + + More details of the TQ Bus protocol are available from Torqeedo after signing an NDA. + */ + +#pragma once + +#include "AP_Torqeedo_config.h" + +#if HAL_TORQEEDO_ENABLED + +#include "AP_Torqeedo_Backend.h" + +#define TORQEEDO_MESSAGE_LEN_MAX 35 // messages are no more than 35 bytes + +class AP_Torqeedo_TQBus : public AP_Torqeedo_Backend { +public: + + // constructor + using AP_Torqeedo_Backend::AP_Torqeedo_Backend; + + CLASS_NO_COPY(AP_Torqeedo_TQBus); + + // initialise driver + void init() override; + + // returns true if communicating with the motor + bool healthy() override; + + // clear motor errors + void clear_motor_error() override { _motor_clear_error = true; } + + // get latest battery status info. returns true on success and populates arguments + bool get_batt_info(float &voltage, float ¤t_amps, float &temp_C, uint8_t &pct_remaining) const override; + bool get_batt_capacity_Ah(uint16_t &_hours) const override; + +private: + + // consume incoming messages from motor, reply with latest motor speed + // runs in background thread + void thread_main(); + + // report changes in error codes to user + void report_error_codes(); + + // message addresses + enum class MsgAddress : uint8_t { + BUS_MASTER = 0x00, + REMOTE1 = 0x14, + DISPLAY = 0x20, + MOTOR = 0x30, + BATTERY = 0x80 + }; + + // Remote specific message ids + enum class RemoteMsgId : uint8_t { + INFO = 0x00, + REMOTE = 0x01, + SETUP = 0x02 + }; + + // Display specific message ids + enum class DisplayMsgId : uint8_t { + INFO = 0x00, + SYSTEM_STATE = 0x41, + SYSTEM_SETUP = 0x42 + }; + + // Motor specific message ids + enum class MotorMsgId : uint8_t { + INFO = 0x00, + STATUS = 0x01, + PARAM = 0x03, + CONFIG = 0x04, + DRIVE = 0x82 + }; + + 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 complete message has been received (the message will be held in _received_buff) + bool parse_byte(uint8_t b); + + // process message held in _received_buff + void parse_message(); + + // returns true if it is safe to send a message + bool safe_to_send() const { return ((_send_delay_us == 0) && (_reply_wait_start_ms == 0)); } + + // set pin to enable sending a message + void send_start(); + + // check for timeout after sending a message and unset pin if required + void check_for_send_end(); + + // calculate delay required to allow message to be completely sent + uint32_t calc_send_delay_us(uint8_t num_bytes); + + // record msgid of message to wait for and set timer for reply timeout handling + void set_expected_reply_msgid(uint8_t msg_id); + + // check for timeout waiting for reply + void check_for_reply_timeout(); + + // mark reply received. should be called whenever a message is received regardless of whether we are actually waiting for a reply + void set_reply_received(); + + // 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 send_message(const uint8_t msg_contents[], uint8_t num_bytes); + + // 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 add_byte_to_message(uint8_t byte_to_add, uint8_t msg_buff[], uint8_t msg_buff_size, uint8_t &num_bytes) const; + + // send a motor speed command as a value from -1000 to +1000 + // value is taken directly from SRV_Channel + void send_motor_speed_cmd(); + + // send request to motor to reply with a particular message + // msg_id can be INFO, STATUS or PARAM + void send_motor_msg_request(MotorMsgId 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 calc_motor_speed_limited(int16_t desired_motor_speed); + int16_t get_motor_speed_limited() const { return (int16_t)_motor_speed_limited; } + + // log TRQD message which holds high level status and latest desired motors peed + // force_logging should be true to immediately write log bypassing timing check to avoid spamming + void log_TRQD(bool force_logging); + + // send ESC telemetry + void update_esc_telem(float rpm, float voltage, float current_amps, float esc_tempC, float motor_tempC); + + // members + AP_HAL::UARTDriver *_uart; // serial port to communicate with motor + bool _initialised; // true once driver has been initialised + bool _send_motor_speed; // true if motor speed should be sent at next opportunity + int16_t _motor_speed_desired; // desired motor speed (set from within update method) + uint32_t _last_send_motor_ms; // system time (in millis) last motor speed command was sent (used for health reporting) + bool _motor_clear_error; // true if the motor error should be cleared (sent in "Drive" message) + uint32_t _send_start_us; // system time (in micros) when last message started being sent (used for timing to unset DE pin) + uint32_t _send_delay_us; // delay (in micros) to allow bytes to be sent after which pin can be unset. 0 if not delaying + + // motor speed limit variables + float _motor_speed_limited; // limited desired motor speed. this value is actually sent to the motor + uint32_t _motor_speed_limited_ms; // system time that _motor_speed_limited was last updated + int8_t _dir_limit; // acceptable directions for output to motor (+1 = positive OK, -1 = negative OK, 0 = either positive or negative OK) + uint32_t _motor_speed_zero_ms; // system time that _motor_speed_limited reached zero. 0 if currently not zero + + // health reporting + HAL_Semaphore _last_healthy_sem;// semaphore protecting reading and updating of _last_send_motor_ms and _last_received_ms + uint32_t _last_log_TRQD_ms; // system time (in millis) that TRQD was last logged + + // message parsing members + ParseState _parse_state; // current state of parsing + bool _parse_escape_received; // true if the escape character has been received so we must XOR the next byte + 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 + uint32_t _last_received_ms; // system time (in millis) that a message was successfully parsed (for health reporting) + + // reply message handling + uint8_t _reply_msgid; // replies expected msgid (reply often does not specify the msgid so we must record it) + uint32_t _reply_wait_start_ms; // system time that we started waiting for a reply message + + // Display system state flags + typedef union PACKED { + struct { + uint8_t set_throttle_stop : 1; // 0, warning that user must set throttle to stop before motor can run + uint8_t setup_allowed : 1; // 1, remote is allowed to enter setup mode + uint8_t in_charge : 1; // 2, master is in charging state + uint8_t in_setup : 1; // 3, master is in setup state + uint8_t bank_available : 1; // 4 + uint8_t no_menu : 1; // 5 + uint8_t menu_off : 1; // 6 + uint8_t reserved7 : 1; // 7, unused + uint8_t temp_warning : 1; // 8, motor or battery temp warning + uint8_t batt_charge_valid : 1; // 9, battery charge valid + uint8_t batt_nearly_empty : 1; // 10, battery nearly empty + uint8_t batt_charging : 1; // 11, battery charging + uint8_t gps_searching : 1; // 12, gps searching for satellites + uint8_t gps_speed_valid : 1; // 13, gps speed is valid + uint8_t range_miles_valid : 1; // 14, range (in miles) is valid + uint8_t range_minutes_valid : 1; // 15, range (in minutes) is valid + }; + uint16_t value; + } DisplaySystemStateFlags; + + // Display system state + struct DisplaySystemState { + DisplaySystemStateFlags flags; // flags, see above for individual bit definitions + uint8_t master_state; // deprecated + uint8_t master_error_code; // error code (0=no error) + float motor_voltage; // motor voltage in volts + float motor_current; // motor current in amps + uint16_t motor_power; // motor power in watts + int16_t motor_rpm; // motor speed in rpm + uint8_t motor_pcb_temp; // motor pcb temp in C + uint8_t motor_stator_temp; // motor stator temp in C + uint8_t batt_charge_pct; // battery state of charge (0 to 100%) + float batt_voltage; // battery voltage in volts + float batt_current; // battery current in amps + uint16_t gps_speed; // gps speed in knots * 100 + uint16_t range_miles; // range in nautical miles * 10 + uint16_t range_minutes; // range in minutes (at current speed and current draw) + uint8_t temp_sw; // master PCB temp in C (close to motor power switches) + uint8_t temp_rp; // master PCB temp in C (close to reverse voltage protection) + uint32_t last_update_ms; // system time that system state was last updated + } _display_system_state; + + // Display system setup + struct DisplaySystemSetup { + uint8_t flags; // 0 : battery config valid, all other bits unused + uint8_t motor_type; // motor type (0 or 3:Unknown, 1:Ultralight, 2:Cruise2, 4:Cruise4, 5:Travel503, 6:Travel1003, 7:Cruise10kW) + uint16_t motor_sw_version; // motor software version + uint16_t batt_capacity; // battery capacity in amp hours + uint8_t batt_charge_pct; // battery state of charge (0 to 100%) + uint8_t batt_type; // battery type (0:lead acid, 1:Lithium) + uint16_t master_sw_version; // master software version + } _display_system_setup; + + // Motor status + struct MotorStatus { + union PACKED { + struct { + uint8_t temp_limit_motor : 1; // 0, motor speed limited due to motor temp + uint8_t temp_limit_pcb : 1; // 1, motor speed limited tue to PCB temp + uint8_t emergency_stop : 1; // 2, motor in emergency stop (must be cleared by master) + uint8_t running : 1; // 3, motor running + uint8_t power_limit : 1; // 4, motor power limited + uint8_t low_voltage_limit : 1; // 5, motor speed limited because of low voltage + uint8_t tilt : 1; // 6, motor is tilted + uint8_t reserved7 : 1; // 7, unused (always zero) + } status_flags; + uint8_t status_flags_value; + }; + union PACKED { + struct { + uint8_t overcurrent : 1; // 0, motor stopped because of overcurrent + uint8_t blocked : 1; // 1, motor stopped because it is blocked + uint8_t overvoltage_static : 1; // 2, motor stopped because voltage too high + uint8_t undervoltage_static : 1; // 3, motor stopped because voltage too low + uint8_t overvoltage_current : 1; // 4, motor stopped because voltage spiked high + uint8_t undervoltage_current: 1; // 5, motor stopped because voltage spiked low + uint8_t overtemp_motor : 1; // 6, motor stopped because stator temp too high + uint8_t overtemp_pcb : 1; // 7, motor stopped because pcb temp too high + uint8_t timeout_rs485 : 1; // 8, motor stopped because Drive message not received for too long + uint8_t temp_sensor_error : 1; // 9, motor temp sensor is defective (motor will not stop) + uint8_t tilt : 1; // 10, motor stopped because it was tilted + uint8_t unused11to15 : 5; // 11 ~ 15 (always zero) + } error_flags; + uint16_t error_flags_value; + }; + } _motor_status; + uint32_t _last_send_motor_status_request_ms; // system time (in milliseconds) that last motor status request was sent + + // Motor params + struct MotorParam { + int16_t rpm; // motor rpm + uint16_t power; // motor power consumption in Watts + float voltage; // motor voltage in volts + float current; // motor current in amps + float pcb_temp; // pcb temp in C + float stator_temp; // stator temp in C + uint32_t last_update_ms;// system time that above values were updated + } _motor_param; + uint32_t _last_send_motor_param_request_ms; // system time (in milliseconds) that last motor param request was sent + + // error reporting + DisplaySystemStateFlags _display_system_state_flags_prev; // backup of display system state flags + uint8_t _display_system_state_master_error_code_prev; // backup of display system state master_error_code + uint32_t _last_error_report_ms; // system time that flag changes were last reported (used to prevent spamming user) + MotorStatus _motor_status_prev; // backup of motor status + + // 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 *map_master_error_code_to_string(uint8_t code) const; +}; + +#endif // HAL_TORQEEDO_ENABLED