/* 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.h" #if HAL_TORQEEDO_ENABLED #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; // parameters const AP_Param::GroupInfo AP_Torqeedo::var_info[] = { // @Param: TYPE // @DisplayName: Torqeedo connection type // @Description: Torqeedo connection type // @Values: 0:Disabled, 1:Tiller, 2:Motor // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("TYPE", 1, AP_Torqeedo, _type, (int8_t)ConnectionType::TYPE_DISABLED, AP_PARAM_FLAG_ENABLE), // @Param: ONOFF_PIN // @DisplayName: Torqeedo ON/Off pin // @Description: Pin number connected to Torqeedo's on/off pin. -1 to use serial port's RTS pin if available // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6 // @User: Standard // @RebootRequired: True AP_GROUPINFO("ONOFF_PIN", 2, AP_Torqeedo, _pin_onoff, -1), // @Param: DE_PIN // @DisplayName: Torqeedo DE pin // @Description: Pin number connected to RS485 to Serial converter's DE pin. -1 to use serial port's CTS pin if available // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6 // @User: Standard // @RebootRequired: True AP_GROUPINFO("DE_PIN", 3, AP_Torqeedo, _pin_de, -1), // @Param: OPTIONS // @DisplayName: Torqeedo Options // @Description: Torqeedo Options Bitmask // @Bitmask: 0:Log,1:Send debug to GCS // @User: Advanced AP_GROUPINFO("OPTIONS", 4, AP_Torqeedo, _options, (int8_t)options::LOG), // @Param: POWER // @DisplayName: Torqeedo Motor Power // @Description: Torqeedo motor power. Only applied when using motor connection type (e.g. TRQD_TYPE=2) // @Units: % // @Range: 0 100 // @Increment: 1 // @User: Advanced AP_GROUPINFO("POWER", 5, AP_Torqeedo, _motor_power, 100), // @Param: SLEW_TIME // @DisplayName: Torqeedo Throttle Slew Time // @Description: Torqeedo slew rate specified as the minimum number of seconds required to increase the throttle from 0 to 100%. Higher values cause a slower response, lower values cause a faster response. A value of zero disables the limit // @Units: s // @Range: 0 5 // @Increment: 0.1 // @User: Advanced AP_GROUPINFO("SLEW_TIME", 6, AP_Torqeedo, _slew_time, 2.0), // @Param: DIR_DELAY // @DisplayName: Torqeedo Direction Change Delay // @Description: Torqeedo direction change delay. Output will remain at zero for this many seconds when transitioning between forward and backwards rotation // @Units: s // @Range: 0 5 // @Increment: 0.1 // @User: Advanced AP_GROUPINFO("DIR_DELAY", 7, AP_Torqeedo, _dir_delay, 1.0), AP_GROUPEND }; AP_Torqeedo::AP_Torqeedo() { _singleton = this; AP_Param::setup_object_defaults(this, var_info); } // initialise driver void AP_Torqeedo::init() { // exit immediately if not enabled if (!enabled()) { return; } // only init once // Note: a race condition exists here if init is called multiple times quickly before thread_main has a chance to set _initialise if (_initialised) { return; } // create background thread to process serial input and output if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Torqeedo::thread_main, void), "torqeedo", 2048, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) { return; } } // initialise serial port and gpio pins (run from background thread) bool AP_Torqeedo::init_internals() { // find serial driver and initialise const AP_SerialManager &serial_manager = AP::serialmanager(); _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Torqeedo, 0); if (_uart == nullptr) { return false; } _uart->begin(TORQEEDO_SERIAL_BAUD); _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); _uart->set_unbuffered_writes(true); // if using tiller connection set on/off pin for 0.5 sec to turn on battery if (_type == ConnectionType::TYPE_TILLER) { if (_pin_onoff > -1) { hal.gpio->pinMode(_pin_onoff, HAL_GPIO_OUTPUT); hal.gpio->write(_pin_onoff, 1); hal.scheduler->delay(500); hal.gpio->write(_pin_onoff, 0); } else { // use serial port's RTS pin to turn on battery _uart->set_RTS_pin(true); hal.scheduler->delay(500); _uart->set_RTS_pin(false); } } // initialise RS485 DE pin (when high, allows send to motor) if (_pin_de > -1) { hal.gpio->pinMode(_pin_de, HAL_GPIO_OUTPUT); hal.gpio->write(_pin_de, 0); } else { _uart->set_CTS_pin(false); } return true; } // returns true if the driver is enabled bool AP_Torqeedo::enabled() const { switch ((ConnectionType)_type) { case ConnectionType::TYPE_DISABLED: return false; case ConnectionType::TYPE_TILLER: case ConnectionType::TYPE_MOTOR: return true; } return false; } // consume incoming messages from motor, reply with latest motor speed // runs in background thread void AP_Torqeedo::thread_main() { // initialisation if (!init_internals()) { return; } _initialised = true; while (true) { // 1ms loop delay hal.scheduler->delay_microseconds(1000); // check if transmit pin should be unset check_for_send_end(); // check for timeout waiting for reply check_for_reply_timeout(); // parse incoming characters uint32_t nbytes = MIN(_uart->available(), 1024U); while (nbytes-- > 0) { int16_t b = _uart->read(); if (b >= 0 ) { if (parse_byte((uint8_t)b)) { // complete message received, parse it! parse_message(); // clear wait-for-reply because if we are waiting for a reply, this message must be it set_reply_received(); } } } // send motor speed bool log_update = false; if (safe_to_send()) { uint32_t now_ms = AP_HAL::millis(); // if connected to motor if (_type == ConnectionType::TYPE_MOTOR) { if (now_ms - _last_send_motor_ms > TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_MS) { // send motor speed every 0.1sec _send_motor_speed = true; } else if (now_ms - _last_send_motor_status_request_ms > TORQEEDO_SEND_MOTOR_STATUS_REQUEST_INTERVAL_MS) { // send request for motor status send_motor_msg_request(MotorMsgId::STATUS); _last_send_motor_status_request_ms = now_ms; } else if (now_ms - _last_send_motor_param_request_ms > TORQEEDO_SEND_MOTOR_PARAM_REQUEST_INTERVAL_MS) { // send request for motor params send_motor_msg_request(MotorMsgId::PARAM); _last_send_motor_param_request_ms = now_ms; } } // send motor speed if (_send_motor_speed) { send_motor_speed_cmd(); _send_motor_speed = false; log_update = true; } } // log high level status and motor speed log_TRQD(log_update); } } // returns true if communicating with the motor bool AP_Torqeedo::healthy() { if (!_initialised) { return false; } { // healthy if both receive and send have occurred in the last 3 seconds WITH_SEMAPHORE(_last_healthy_sem); const uint32_t now_ms = AP_HAL::millis(); return ((now_ms - _last_received_ms < 3000) && (now_ms - _last_send_motor_ms < 3000)); } } // run pre-arm check. returns false on failure and fills in failure_msg // any failure_msg returned will not include a prefix bool AP_Torqeedo::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) { // exit immediately if not enabled if (!enabled()) { return true; } if (!_initialised) { strncpy(failure_msg, "not initialised", failure_msg_len); return false; } if (!healthy()) { strncpy(failure_msg, "not healthy", failure_msg_len); return false; } return true; } // returns a human-readable string corresponding the passed-in // master error code (see page 93 of https://media.torqeedo.com/downloads/manuals/torqeedo-Travel-manual-DE-EN.pdf) // If no conversion is available then nullptr is returned const char * AP_Torqeedo::map_master_error_code_to_string(uint8_t code) const { switch (code) { case 2: return "stator high temp"; case 5: return "propeller blocked"; case 6: return "motor low voltage"; case 7: return "motor high current"; case 8: return "pcb temp high"; case 21: return "tiller cal bad"; case 22: return "mag bad"; case 23: return "range incorrect"; case 30: return "motor comm error"; case 32: return "tiller comm error"; case 33: return "general comm error"; case 41: case 42: return "charge voltage bad"; case 43: return "battery flat"; case 45: return "battery high current"; case 46: return "battery temp error"; case 48: return "charging temp error"; } return nullptr; } // report changes in error codes to user void AP_Torqeedo::report_error_codes() { // skip reporting if we have already reported status very recently const uint32_t now_ms = AP_HAL::millis(); // skip reporting if no changes in flags and already reported within 10 seconds const bool flags_changed = (_display_system_state_flags_prev.value != _display_system_state.flags.value) || (_display_system_state_master_error_code_prev != _display_system_state.master_error_code) || (_motor_status_prev.status_flags_value != _motor_status.status_flags_value) || (_motor_status_prev.error_flags_value != _motor_status.error_flags_value); if (!flags_changed && ((now_ms - _last_error_report_ms) < TORQEEDO_ERROR_REPORT_INTERVAL_MAX_MS)) { return; } // report display system errors const char* msg_prefix = "Torqeedo:"; if (_display_system_state.flags.set_throttle_stop) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s zero throttle required", msg_prefix); } if (_display_system_state.flags.temp_warning) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high temp", msg_prefix); } if (_display_system_state.flags.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::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::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::parse_byte(uint8_t b) { bool complete_msg_received = false; switch (_parse_state) { case ParseState::WAITING_FOR_HEADER: if (b == TORQEEDO_PACKET_HEADER) { _parse_state = ParseState::WAITING_FOR_FOOTER; } _received_buff_len = 0; _parse_escape_received = false; break; case ParseState::WAITING_FOR_FOOTER: if (b == TORQEEDO_PACKET_FOOTER) { _parse_state = ParseState::WAITING_FOR_HEADER; // check message length if (_received_buff_len == 0) { _parse_error_count++; break; } // check crc const uint8_t crc_expected = crc8_maxim(_received_buff, _received_buff_len-1); if (_received_buff[_received_buff_len-1] != crc_expected) { _parse_error_count++; break; } _parse_success_count++; { // record time of successful receive for health reporting WITH_SEMAPHORE(_last_healthy_sem); _last_received_ms = AP_HAL::millis(); } complete_msg_received = true; } else { // escape character handling if (_parse_escape_received) { b ^= TORQEEDO_PACKET_ESCAPE_MASK; _parse_escape_received = false; } else if (b == TORQEEDO_PACKET_ESCAPE) { // escape character received, record this and ignore this byte _parse_escape_received = true; break; } // add to buffer _received_buff[_received_buff_len] = b; _received_buff_len++; if (_received_buff_len > TORQEEDO_MESSAGE_LEN_MAX) { // message too long _parse_state = ParseState::WAITING_FOR_HEADER; _parse_error_count++; } } break; } return complete_msg_received; } // process message held in _received_buff void AP_Torqeedo::parse_message() { // message address (i.e. target of message) const MsgAddress msg_addr = (MsgAddress)_received_buff[0]; // handle messages sent to "remote" (aka tiller) if ((_type == ConnectionType::TYPE_TILLER) && (msg_addr == MsgAddress::REMOTE1)) { RemoteMsgId msg_id = (RemoteMsgId)_received_buff[1]; if (msg_id == RemoteMsgId::REMOTE) { // request received to send updated motor speed _send_motor_speed = true; } return; } // handle messages sent to Display if ((_type == ConnectionType::TYPE_TILLER) && (msg_addr == MsgAddress::DISPLAY)) { DisplayMsgId msg_id = (DisplayMsgId)_received_buff[1]; switch (msg_id) { case DisplayMsgId::SYSTEM_STATE : if (_received_buff_len == 30) { // fill in _display_system_state _display_system_state.flags.value = UINT16_VALUE(_received_buff[2], _received_buff[3]); _display_system_state.master_state = _received_buff[4]; // deprecated _display_system_state.master_error_code = _received_buff[5]; _display_system_state.motor_voltage = UINT16_VALUE(_received_buff[6], _received_buff[7]) * 0.01; _display_system_state.motor_current = UINT16_VALUE(_received_buff[8], _received_buff[9]) * 0.1; _display_system_state.motor_power = UINT16_VALUE(_received_buff[10], _received_buff[11]); _display_system_state.motor_rpm = (int16_t)UINT16_VALUE(_received_buff[12], _received_buff[13]); _display_system_state.motor_pcb_temp = _received_buff[14]; _display_system_state.motor_stator_temp = _received_buff[15]; _display_system_state.batt_charge_pct = _received_buff[16]; _display_system_state.batt_voltage = UINT16_VALUE(_received_buff[17], _received_buff[18]) * 0.01; _display_system_state.batt_current = UINT16_VALUE(_received_buff[19], _received_buff[20]) * 0.1; _display_system_state.gps_speed = UINT16_VALUE(_received_buff[21], _received_buff[22]); _display_system_state.range_miles = UINT16_VALUE(_received_buff[23], _received_buff[24]); _display_system_state.range_minutes = UINT16_VALUE(_received_buff[25], _received_buff[26]); _display_system_state.temp_sw = _received_buff[27]; _display_system_state.temp_rp = _received_buff[28]; _display_system_state.last_update_ms = AP_HAL::millis(); // update esc telem sent to ground station const uint8_t esc_temp = MAX(_display_system_state.temp_sw, _display_system_state.temp_rp); const uint8_t motor_temp = MAX(_display_system_state.motor_pcb_temp, _display_system_state.motor_stator_temp); update_esc_telem(_display_system_state.motor_rpm, _display_system_state.motor_voltage, _display_system_state.motor_current, esc_temp, motor_temp); #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", "QBHH", 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; 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); } } // send ESC telemetry void AP_Torqeedo::update_esc_telem(float rpm, float voltage, float current_amps, float esc_tempC, float motor_tempC) { #if HAL_WITH_ESC_TELEM // find servo output channel uint8_t telem_esc_index = 0; IGNORE_RETURN(SRV_Channels::find_channel(SRV_Channel::Aux_servo_function_t::k_throttle, telem_esc_index)); // fill in telemetry data structure AP_ESC_Telem_Backend::TelemetryData telem_dat {}; telem_dat.temperature_cdeg = esc_tempC * 100; // temperature in centi-degrees telem_dat.voltage = voltage; // voltage in volts telem_dat.current = current_amps; // current in amps telem_dat.motor_temp_cdeg = motor_tempC * 100; // motor temperature in centi-degrees // send telem and rpm data update_telem_data(telem_esc_index, telem_dat, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::CURRENT | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE); update_rpm(telem_esc_index, rpm); #endif } // get the AP_Torqeedo singleton AP_Torqeedo *AP_Torqeedo::get_singleton() { return _singleton; } AP_Torqeedo *AP_Torqeedo::_singleton = nullptr; namespace AP { AP_Torqeedo *torqeedo() { return AP_Torqeedo::get_singleton(); } }; #endif // HAL_TORQEEDO_ENABLED