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

AP_Torqeedo: major refactor and enhancement 

SLEW_TIME specifies the rate of change in output to the motor
DIR_DELAY specifies the length of the pause at zero during forward-reverse transitions
POWER param allows configurable power output
Consume Display system state and setup messages
Consume Motor messages to retrieve volt and current
TRQD logs on each motor speed update
AP_Torqeedo: request and consume motor status messages
ESC_Telemetry support
Battery info accessor
Add real-time error reporting of failures to user
Support user manually clearing motor errors via RC
Improve message reply handling
Tiller message consumption improved
Messages with unexpected lengths increment error count
Enums added
Comments improved
Remove unused include
Disable for AP_Periph and minimise features
This commit is contained in:
Randy Mackay 2021-09-04 13:41:30 +09:00
parent e61624d43b
commit 866ea184c8
2 changed files with 869 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

693
libraries/AP_Torqeedo/AP_Torqeedo.cpp
View File

@ -26,8 +26,14 @@
#define TORQEEDO_SERIAL_BAUD 19200 // communication is always at 19200 #define TORQEEDO_SERIAL_BAUD 19200 // communication is always at 19200
#define TORQEEDO_PACKET_HEADER 0xAC // communication packet header #define TORQEEDO_PACKET_HEADER 0xAC // communication packet header
#define TORQEEDO_PACKET_FOOTER 0xAD // communication packer footer #define TORQEEDO_PACKET_FOOTER 0xAD // communication packer footer
#define TORQEEDO_LOG_INTERVAL_MS 5000 // log debug info at this interval in milliseconds #define TORQEEDO_LOG_TRQD_INTERVAL_MS 5000// log TRQD message at this interval in milliseconds
#define TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_US 100000 // motor speed sent at 10hz if connected to motor #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_MIN_MS 1000 // errors reported to user at no more than once per second
#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; extern const AP_HAL::HAL& hal;
@ -65,6 +71,33 @@ const AP_Param::GroupInfo AP_Torqeedo::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("OPTIONS", 4, AP_Torqeedo, _options, (int8_t)options::LOG), 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 Slew Time
// @Description: Torqeedo 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
// @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_GROUPEND
}; };
@ -164,37 +197,54 @@ void AP_Torqeedo::thread_main()
// check if transmit pin should be unset // check if transmit pin should be unset
check_for_send_end(); check_for_send_end();
// check for timeout waiting for reply
check_for_reply_timeout();
// parse incoming characters // parse incoming characters
uint32_t nbytes = MIN(_uart->available(), 1024U); uint32_t nbytes = MIN(_uart->available(), 1024U);
while (nbytes-- > 0) { while (nbytes-- > 0) {
int16_t b = _uart->read(); int16_t b = _uart->read();
if (b >= 0 ) { if (b >= 0 ) {
if (parse_byte((uint8_t)b)) { if (parse_byte((uint8_t)b)) {
// request received to send updated motor speed // complete message received, parse it!
if (_type == ConnectionType::TYPE_TILLER) { parse_message();
_send_motor_speed = true; // clear wait-for-reply because if we are waiting for a reply, this message must be it
} set_reply_received();
} }
} }
} }
// send motor speed // send motor speed
bool log_update = false;
if (safe_to_send()) { if (safe_to_send()) {
// if connected to motor send motor speed every 0.5sec uint32_t now_ms = AP_HAL::millis();
if (_type == ConnectionType::TYPE_MOTOR &&
(AP_HAL::micros() - _last_send_motor_us > TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_US)) { // if connected to motor
_send_motor_speed = true; 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 // send motor speed
if (_send_motor_speed) { if (_send_motor_speed) {
send_motor_speed_cmd(); send_motor_speed_cmd();
_send_motor_speed = false; _send_motor_speed = false;
log_update = true;
} }
} }
// logging and debug output // log high level status and motor speed
log_and_debug(); log_TRQD(log_update);
} }
} }
@ -232,11 +282,126 @@ bool AP_Torqeedo::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
return true; return true;
} }
// 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();
const uint32_t diff_ms = now_ms - _last_error_report_ms;
if (diff_ms < TORQEEDO_ERROR_REPORT_INTERVAL_MIN_MS) {
return;
}
// 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 && (diff_ms < TORQEEDO_ERROR_REPORT_INTERVAL_MAX_MS)) {
return;
}
// report display system errors
const char* msg_prefix = "Torqeedo:";
if (_display_system_state.flags.set_throttle_stop) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s zero throttle required", msg_prefix);
}
if (_display_system_state.flags.temp_warning) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s high temp", msg_prefix);
}
if (_display_system_state.flags.temp_warning) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s batt nearly empty", msg_prefix);
}
if (_display_system_state.master_error_code > 0) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s err:%d", msg_prefix, _display_system_state.master_error_code);
}
// report motor status errors
if (_motor_status.error_flags.overcurrent) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s overcurrent", msg_prefix);
}
if (_motor_status.error_flags.blocked) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s prop blocked", msg_prefix);
}
if (_motor_status.error_flags.overvoltage_static || _motor_status.error_flags.overvoltage_current) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s high voltage", msg_prefix);
}
if (_motor_status.error_flags.undervoltage_static || _motor_status.error_flags.undervoltage_current) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s low voltage", msg_prefix);
}
if (_motor_status.error_flags.overtemp_motor || _motor_status.error_flags.overtemp_pcb) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s high temp", msg_prefix);
}
if (_motor_status.error_flags.timeout_rs485) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s comm timeout", msg_prefix);
}
if (_motor_status.error_flags.temp_sensor_error) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s temp sensor err", msg_prefix);
}
if (_motor_status.error_flags.tilt) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s tilted", msg_prefix);
}
// display OK if all errors cleared
const bool prev_errored = (_display_system_state_flags_prev.value != 0) ||
(_display_system_state_master_error_code_prev != 0) ||
(_motor_status_prev.error_flags_value != 0);
const bool now_errored = (_display_system_state.flags.value != 0) ||
(_display_system_state.master_error_code != 0) ||
(_motor_status.error_flags_value != 0);
if (!now_errored && prev_errored) {
gcs().send_text(MAV_SEVERITY_INFO, "%s OK", msg_prefix);
}
// record change in state and reporting time
_display_system_state_flags_prev.value = _display_system_state.flags.value;
_display_system_state_master_error_code_prev = _display_system_state.master_error_code;
_motor_status_prev = _motor_status;
_last_error_report_ms = now_ms;
}
// get latest battery status info. returns true on success and populates arguments
bool AP_Torqeedo::get_batt_info(float &voltage, float &current_amps, float &temp_C, uint8_t &pct_remaining) const
{
// use battery info from display_system_state if available (tiller connection)
if ((AP_HAL::millis() - _display_system_state.last_update_ms) <= TORQEEDO_BATT_TIMEOUT_MS) {
voltage = _display_system_state.batt_voltage;
current_amps = _display_system_state.batt_current;
temp_C = MAX(_display_system_state.temp_sw, _display_system_state.temp_rp);
pct_remaining = _display_system_state.batt_charge_pct;
return true;
}
// use battery info from motor_param if available (motor connection)
if ((AP_HAL::millis() - _motor_param.last_update_ms) <= TORQEEDO_BATT_TIMEOUT_MS) {
voltage = _motor_param.voltage;
current_amps = _motor_param.current;
temp_C = MAX(_motor_param.pcb_temp, _motor_param.stator_temp);
pct_remaining = 0; // motor does not know percent remaining
return true;
}
return false;
}
// get battery capacity. returns true on success and populates argument
bool AP_Torqeedo::get_batt_capacity_Ah(uint16_t &amp_hours) const
{
if (_display_system_setup.batt_capacity == 0) {
return false;
}
amp_hours = _display_system_setup.batt_capacity;
return true;
}
// process a single byte received on serial port // process a single byte received on serial port
// return true if a this driver should send a set-motor-speed message // 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 AP_Torqeedo::parse_byte(uint8_t b)
{ {
bool motor_speed_requested = false; bool complete_msg_received = false;
switch (_parse_state) { switch (_parse_state) {
case ParseState::WAITING_FOR_HEADER: case ParseState::WAITING_FOR_HEADER:
@ -267,13 +432,7 @@ bool AP_Torqeedo::parse_byte(uint8_t b)
WITH_SEMAPHORE(_last_healthy_sem); WITH_SEMAPHORE(_last_healthy_sem);
_last_received_ms = AP_HAL::millis(); _last_received_ms = AP_HAL::millis();
} }
complete_msg_received = true;
// check message id
MsgId msg_id = (MsgId)_received_buff[0];
if (msg_id == MsgId::REQUEST_MOTOR_SPEED) {
motor_speed_requested = true;
}
} else { } else {
// add to buffer // add to buffer
_received_buff[_received_buff_len] = b; _received_buff[_received_buff_len] = b;
@ -287,7 +446,269 @@ bool AP_Torqeedo::parse_byte(uint8_t b)
break; break;
} }
return motor_speed_requested; return complete_msg_received;
}
// process message held in _received_buff
void AP_Torqeedo::parse_message()
{
// message address (e.g. 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
update_esc_telem(_display_system_state.motor_rpm,
_display_system_state.motor_voltage,
_display_system_state.motor_current,
MAX(_display_system_state.temp_sw, _display_system_state.temp_rp), // esc temp
MAX(_display_system_state.motor_pcb_temp, _display_system_state.motor_stator_temp)); // motor temp
// log data
if ((_options & options::LOG) != 0) {
// @LoggerMessage: TRST
// @Description: Torqeedo System State
// @Field: TimeUS: Time since system startup
// @Field: F: Flags bitmask
// @Field: Err: Master error code
// @Field: MVolt: Motor voltage
// @Field: MCur: Motor current
// @Field: Pow: Motor power
// @Field: RPM: Motor RPM
// @Field: MTemp: Motor Temp (higher of pcb or stator)
// @Field: BPct: Battery charge percentage
// @Field: BVolt: Battery voltage
// @Field: BCur: Battery current
AP::logger().Write("TRST", "TimeUS,F,Err,MVolt,MCur,Pow,RPM,MTemp,BPct,BVolt,BCur", "QHBfffhBBff",
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,
MAX(_display_system_state.motor_pcb_temp, _display_system_state.motor_stator_temp),
_display_system_state.batt_charge_pct,
_display_system_state.batt_voltage,
_display_system_state.batt_current);
}
// send to GCS
if ((_options & options::DEBUG_TO_GCS) != 0) {
gcs().send_text(MAV_SEVERITY_INFO,"TRST F:%u Err:%u MV:%4.1f MC:%4.1f P:%4.1f 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,
(double)_display_system_state.motor_power,
MAX(_display_system_state.motor_pcb_temp, _display_system_state.motor_stator_temp),
(unsigned)_display_system_state.batt_charge_pct,
(double)_display_system_state.batt_voltage,
(double)_display_system_state.batt_current);
}
// report any errors
report_error_codes();
} else {
// unexpected length
_parse_error_count++;
}
break;
case DisplayMsgId::SYSTEM_SETUP:
if (_received_buff_len == 13) {
// fill in display system setup
_display_system_setup.flags = _received_buff[2];
_display_system_setup.motor_type = _received_buff[3];
_display_system_setup.motor_sw_version = UINT16_VALUE(_received_buff[4], _received_buff[5]);
_display_system_setup.batt_capacity = UINT16_VALUE(_received_buff[6], _received_buff[7]);
_display_system_setup.batt_charge_pct = _received_buff[8];
_display_system_setup.batt_type = _received_buff[9];
_display_system_setup.master_sw_version = UINT16_VALUE(_received_buff[10], _received_buff[11]);
// log data
if ((_options & options::LOG) != 0) {
// @LoggerMessage: TRSE
// @Description: Torqeedo System Setup
// @Field: TimeUS: Time since system startup
// @Field: Flag: Flags
// @Field: MotType: Motor type
// @Field: MotVer: Motor software version
// @Field: BattCap: Battery capacity
// @Field: BattPct: Battery charge percentage
// @Field: BattType: Battery type
// @Field: SwVer: Master software version
AP::logger().Write("TRSE", "TimeUS,Flag,MotType,MotVer,BattCap,BattPct,BattType,SwVer", "QBBHHBBH",
AP_HAL::micros64(),
_display_system_setup.flags,
_display_system_setup.motor_type,
_display_system_setup.motor_sw_version,
_display_system_setup.batt_capacity,
_display_system_setup.batt_charge_pct,
_display_system_setup.batt_type,
_display_system_setup.master_sw_version);
}
// send to GCS
if ((_options & options::DEBUG_TO_GCS) != 0) {
gcs().send_text(MAV_SEVERITY_INFO,"TRSE:%u F:%u Mot:%u/%u Bat:%u/%u/%u%%",
(unsigned)_display_system_setup.master_sw_version,
(unsigned)_display_system_setup.flags,
(unsigned)_display_system_setup.motor_type,
(unsigned)_display_system_setup.motor_sw_version,
(unsigned)_display_system_setup.batt_type,
(unsigned)_display_system_setup.batt_capacity,
(unsigned)_display_system_setup.batt_charge_pct);
}
} else {
// unexpected length
_parse_error_count++;
}
break;
default:
// ignore message
break;
}
return;
}
// handle reply from motor
if ((_type == ConnectionType::TYPE_MOTOR) && (msg_addr == MsgAddress::BUS_MASTER)) {
// replies strangely do not return the msgid so we must have stored it
MotorMsgId msg_id = (MotorMsgId)_reply_msgid;
switch (msg_id) {
case MotorMsgId::PARAM:
if (_received_buff_len == 15) {
_motor_param.rpm = (int16_t)UINT16_VALUE(_received_buff[2], _received_buff[3]);
_motor_param.power = UINT16_VALUE(_received_buff[4], _received_buff[5]);
_motor_param.voltage = UINT16_VALUE(_received_buff[6], _received_buff[7]) * 0.01;
_motor_param.current = UINT16_VALUE(_received_buff[8], _received_buff[9]) * 0.1;
_motor_param.pcb_temp = (int16_t)UINT16_VALUE(_received_buff[10], _received_buff[11]) * 0.1;
_motor_param.stator_temp = (int16_t)UINT16_VALUE(_received_buff[12], _received_buff[13]) * 0.1;
_motor_param.last_update_ms = AP_HAL::millis();
// update esc telem sent to ground station
update_esc_telem(_motor_param.rpm,
_motor_param.voltage,
_motor_param.current,
_motor_param.pcb_temp, // esc temp
_motor_param.stator_temp); // motor temp
// log data
if ((_options & options::LOG) != 0) {
// @LoggerMessage: TRMP
// @Description: Torqeedo Motor Param
// @Field: TimeUS: Time since system startup
// @Field: RPM: Motor RPM
// @Field: Pow: Motor power
// @Field: Volt: Motor voltage
// @Field: Cur: Motor current
// @Field: ETemp: ESC Temp
// @Field: MTemp: Motor Temp
AP::logger().Write("TRMP", "TimeUS,RPM,Pow,Volt,Cur,ETemp,MTemp", "QhHffff",
AP_HAL::micros64(),
_motor_param.rpm,
_motor_param.power,
_motor_param.voltage,
_motor_param.current,
_motor_param.pcb_temp,
_motor_param.stator_temp);
}
// send to GCS
if ((_options & options::DEBUG_TO_GCS) != 0) {
gcs().send_text(MAV_SEVERITY_INFO, "TRMP: rpm:%d p:%u V:%4.1f C:%4.1f PT:%4.1f MT:%4.1f",
(int)_motor_param.rpm,
(unsigned)_motor_param.power,
(double)_motor_param.voltage,
(double)_motor_param.current,
(double)_motor_param.pcb_temp,
(double)_motor_param.stator_temp);
}
} else {
// unexpected length
_parse_error_count++;
}
break;
case MotorMsgId::STATUS:
if (_received_buff_len == 6) {
_motor_status.status_flags_value = _received_buff[2];
_motor_status.error_flags_value = UINT16_VALUE(_received_buff[3], _received_buff[4]);
// log data
if ((_options & options::LOG) != 0) {
// @LoggerMessage: TRMS
// @Description: Torqeedo Motor Status
// @Field: TimeUS: Time since system startup
// @Field: State: Motor status flags
// @Field: Err: Motor error flags
AP::logger().Write("TRMS", "TimeUS,State,Err", "QBHH",
AP_HAL::micros64(),
_motor_status.status_flags_value,
_motor_status.error_flags_value);
}
// send to GCS
if ((_options & options::DEBUG_TO_GCS) != 0) {
gcs().send_text(MAV_SEVERITY_INFO,"TRMS S:%d Err:%d",
_motor_status.status_flags_value,
_motor_status.error_flags_value);
}
// report any errors
report_error_codes();
} else {
// unexpected length
_parse_error_count++;
}
break;
case MotorMsgId::INFO:
case MotorMsgId::DRIVE:
case MotorMsgId::CONFIG:
// we do not process these replies
break;
default:
// ignore unknown messages
break;
}
}
} }
// set DE Serial CTS pin to enable sending commands to motor // set DE Serial CTS pin to enable sending commands to motor
@ -309,7 +730,7 @@ void AP_Torqeedo::check_for_send_end()
return; return;
} }
if (AP_HAL::micros() - _last_send_motor_us < _send_delay_us) { if (AP_HAL::micros() - _send_start_us < _send_delay_us) {
// return if delay has not yet elapsed // return if delay has not yet elapsed
return; return;
} }
@ -334,50 +755,86 @@ uint32_t AP_Torqeedo::calc_send_delay_us(uint8_t num_bytes)
return delay_us; return delay_us;
} }
// format of motor speed command packet // record msgid of message to wait for and set timer for timeout handling
// void AP_Torqeedo::set_expected_reply_msgid(uint8_t msg_id)
// Data Byte Field Definition Example Value Comments {
// --------------------------------------------------------------------------------- _reply_msgid = msg_id;
// byte 0 Header 0xAC _reply_wait_start_ms = AP_HAL::millis();
// byte 1 SourceId? 0x00 0 = tiller? }
// byte 2 Destination ID? 0x00 0 = all?
// byte 3 Command Id? 0x05 0=Stop? 4=Don'tTurn? 5=Turn?
// byte 4 Command Id? 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 forwards: "AC 00 00 05 00 00 ED 95 AD" (+237)
// example message when rotating backwards: "AC 00 00 05 00 FF AE 2C 0C AD" (-82)
// 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;
}
// 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 // send a motor speed command as a value from -1000 to +1000
// value is taken directly from SRV_Channel // 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() void AP_Torqeedo::send_motor_speed_cmd()
{ {
// calculate desired motor speed // calculate desired motor speed
if (!hal.util->get_soft_armed()) { if (!hal.util->get_soft_armed()) {
_motor_speed = 0; _motor_speed_desired = 0;
} else { } else {
// convert throttle output to motor output in range -1000 to +1000 // 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 // ToDo: convert PWM output to motor output so that SERVOx_MIN, MAX and TRIM take effect
_motor_speed = constrain_int16(SRV_Channels::get_output_norm(SRV_Channel::Aux_servo_function_t::k_throttle) * 1000.0, -1000, 1000); _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);
// set send pin // set send pin
send_start(); send_start();
// by default use tiller connection command // by default use tiller connection command
uint8_t mot_speed_cmd_buff[] = {TORQEEDO_PACKET_HEADER, 0x0, 0x0, 0x5, 0x0, HIGHBYTE(_motor_speed), LOWBYTE(_motor_speed), 0x0, TORQEEDO_PACKET_FOOTER}; uint8_t mot_speed_cmd_buff[] = {TORQEEDO_PACKET_HEADER, (uint8_t)MsgAddress::BUS_MASTER, 0x0, 0x5, 0x0, HIGHBYTE(mot_speed_limited), LOWBYTE(mot_speed_limited), 0x0, TORQEEDO_PACKET_FOOTER};
uint8_t buff_size = ARRAY_SIZE(mot_speed_cmd_buff); uint8_t buff_size = ARRAY_SIZE(mot_speed_cmd_buff);
// update message if using motor connection // update message if using motor connection
if (_type == ConnectionType::TYPE_MOTOR) { if (_type == ConnectionType::TYPE_MOTOR) {
mot_speed_cmd_buff[1] = 0x30; const uint8_t motor_power = (uint8_t)constrain_int16(_motor_power, 0, 100);
mot_speed_cmd_buff[2] = 0x82; mot_speed_cmd_buff[1] = (uint8_t)MsgAddress::MOTOR;
mot_speed_cmd_buff[3] = _motor_speed == 0 ? 0 : 0x1; // enable motor mot_speed_cmd_buff[2] = (uint8_t)MotorMsgId::DRIVE;
mot_speed_cmd_buff[4] = _motor_speed == 0 ? 0 : 0x64; // motor power from 0 to 100 mot_speed_cmd_buff[3] = (mot_speed_limited == 0 ? 0 : 0x01) | (_motor_clear_error ? 0x04 : 0); // 1:enable motor, 2:fast off, 4:clear error
mot_speed_cmd_buff[4] = 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;
} }
// calculate crc and add to buffer // calculate crc and add to buffer
@ -387,7 +844,8 @@ void AP_Torqeedo::send_motor_speed_cmd()
// write message // write message
_uart->write(mot_speed_cmd_buff, buff_size); _uart->write(mot_speed_cmd_buff, buff_size);
_last_send_motor_us = AP_HAL::micros(); // record start and expected delay to send message
_send_start_us = AP_HAL::micros();
_send_delay_us = calc_send_delay_us(buff_size); _send_delay_us = calc_send_delay_us(buff_size);
{ {
@ -395,11 +853,108 @@ void AP_Torqeedo::send_motor_speed_cmd()
WITH_SEMAPHORE(_last_healthy_sem); WITH_SEMAPHORE(_last_healthy_sem);
_last_send_motor_ms = AP_HAL::millis(); _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)
{
// set send pin
send_start();
// prepare message
uint8_t mot_status_request_buff[] = {TORQEEDO_PACKET_HEADER, (uint8_t)MsgAddress::MOTOR, (uint8_t)msg_id, 0x0, TORQEEDO_PACKET_FOOTER};
uint8_t buff_size = ARRAY_SIZE(mot_status_request_buff);
// calculate crc and add to buffer
const uint8_t crc = crc8_maxim(&mot_status_request_buff[1], buff_size-3);
mot_status_request_buff[buff_size-2] = crc;
// write message
_uart->write(mot_status_request_buff, buff_size);
// record start and expected delay to send message
_send_start_us = AP_HAL::micros();
_send_delay_us = calc_send_delay_us(buff_size);
// 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 = (_dir_delay > 0);
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) / 1000.0f;
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) // output logging and debug messages (if required)
void AP_Torqeedo::log_and_debug() // 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 // exit immediately if options are all unset
if (_options == 0) { if (_options == 0) {
@ -408,39 +963,67 @@ void AP_Torqeedo::log_and_debug()
// return if not enough time has passed since last output // return if not enough time has passed since last output
const uint32_t now_ms = AP_HAL::millis(); const uint32_t now_ms = AP_HAL::millis();
if (now_ms - _last_debug_ms < TORQEEDO_LOG_INTERVAL_MS) { if (!force_logging && (now_ms - _last_log_TRQD_ms < TORQEEDO_LOG_TRQD_INTERVAL_MS)) {
return; return;
} }
_last_debug_ms = now_ms; _last_log_TRQD_ms = now_ms;
const bool health = healthy(); const bool health = healthy();
const int16_t mot_speed = health ? _motor_speed : 0; int16_t actual_motor_speed = get_motor_speed_limited();
if ((_options & options::LOG) != 0) { if ((_options & options::LOG) != 0) {
// @LoggerMessage: TRQD // @LoggerMessage: TRQD
// @Description: Torqeedo Status // @Description: Torqeedo Status
// @Field: TimeUS: Time since system startup // @Field: TimeUS: Time since system startup
// @Field: Health: Health // @Field: Health: Health
// @Field: DesMotSpeed: Desired Motor Speed (-1000 to 1000)
// @Field: MotSpeed: Motor Speed (-1000 to 1000) // @Field: MotSpeed: Motor Speed (-1000 to 1000)
// @Field: SuccCnt: Success Count // @Field: SuccCnt: Success Count
// @Field: ErrCnt: Error Count // @Field: ErrCnt: Error Count
AP::logger().Write("TRQD", "TimeUS,Health,MotSpeed,SuccCnt,ErrCnt", "QBHII", AP::logger().Write("TRQD", "TimeUS,Health,DesMotSpeed,MotSpeed,SuccCnt,ErrCnt", "QBhhII",
AP_HAL::micros64(), AP_HAL::micros64(),
health, health,
mot_speed, _motor_speed_desired,
actual_motor_speed,
_parse_success_count, _parse_success_count,
_parse_error_count); _parse_error_count);
} }
if ((_options & options::DEBUG_TO_GCS) != 0) { if ((_options & options::DEBUG_TO_GCS) != 0) {
gcs().send_text(MAV_SEVERITY_INFO,"Trqd h:%u spd:%d succ:%ld err:%ld", gcs().send_text(MAV_SEVERITY_INFO,"TRQD h:%u dspd:%d spd:%d succ:%ld err:%ld",
(unsigned)health, (unsigned)health,
(int)mot_speed, (int)_motor_speed_desired,
(int)actual_motor_speed,
(unsigned long)_parse_success_count, (unsigned long)_parse_success_count,
(unsigned long)_parse_error_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 // get the AP_Torqeedo singleton
AP_Torqeedo *AP_Torqeedo::get_singleton() AP_Torqeedo *AP_Torqeedo::get_singleton()
{ {

253
libraries/AP_Torqeedo/AP_Torqeedo.h
View File

@ -13,22 +13,51 @@
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
/*
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 #pragma once
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#ifndef HAL_TORQEEDO_ENABLED #ifndef HAL_TORQEEDO_ENABLED
#define HAL_TORQEEDO_ENABLED (BOARD_FLASH_SIZE > 1024) #define HAL_TORQEEDO_ENABLED !HAL_MINIMIZE_FEATURES && (BOARD_FLASH_SIZE > 1024) && !defined(HAL_BUILD_AP_PERIPH)
#endif #endif
#if HAL_TORQEEDO_ENABLED #if HAL_TORQEEDO_ENABLED
#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <AP_HAL/Semaphores.h>
#define TORQEEDO_MESSAGE_LEN_MAX 30 // messages are no more than 30 bytes #define TORQEEDO_MESSAGE_LEN_MAX 35 // messages are no more than 35 bytes
class AP_Torqeedo { class AP_Torqeedo : public AP_ESC_Telem_Backend {
public: public:
AP_Torqeedo(); AP_Torqeedo();
@ -50,17 +79,50 @@ public:
// any failure_msg returned will not include a prefix // any failure_msg returned will not include a prefix
bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len); 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 &current_amps, float &temp_C, uint8_t &pct_remaining) const WARN_IF_UNUSED;
bool get_batt_capacity_Ah(uint16_t &amp_hours) const;
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private:
// message ids // message addresses
enum class MsgId : uint8_t { enum class MsgAddress : uint8_t {
SET_MOTOR_SPEED = 0x0, BUS_MASTER = 0x00,
UNKNOWN_0x01 = 0x1, REMOTE1 = 0x14,
REQUEST_MOTOR_SPEED = 0x14, DISPLAY = 0x20,
UNKNOWN_0x20 = 0x20, MOTOR = 0x30,
UNKNOWN_0x30 = 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 { enum class ParseState {
@ -89,46 +151,81 @@ private:
bool enabled() const; bool enabled() const;
// process a single byte received on serial port // process a single byte received on serial port
// return true if a this driver should send a set-motor-speed message // return true if a complete message has been received (the message will be held in _received_buff)
bool parse_byte(uint8_t b); bool parse_byte(uint8_t b);
// returns true if it is safe to send a message // process message held in _received_buff
bool safe_to_send() const { return (_send_delay_us == 0); } void parse_message();
// set pin to enable sending commands to motor // 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(); void send_start();
// check for timeout after sending and unset pin if required // check for timeout after sending a message and unset pin if required
void check_for_send_end(); void check_for_send_end();
// calculate delay require to allow bytes to be sent // calculate delay required to allow message to be completely sent
uint32_t calc_send_delay_us(uint8_t num_bytes); 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 motor speed command as a value from -1000 to +1000 // send a motor speed command as a value from -1000 to +1000
// value is taken directly from SRV_Channel // value is taken directly from SRV_Channel
void send_motor_speed_cmd(); void send_motor_speed_cmd();
// output logging and debug messages (if required) // send request to motor to reply with a particular message
void log_and_debug(); // 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 // parameters
AP_Enum<ConnectionType> _type; // connector type used (0:disabled, 1:tiller connector, 2: motor connector) AP_Enum<ConnectionType> _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_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_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_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 // members
AP_HAL::UARTDriver *_uart; // serial port to communicate with motor AP_HAL::UARTDriver *_uart; // serial port to communicate with motor
bool _initialised; // true once driver has been initialised bool _initialised; // true once driver has been initialised
bool _send_motor_speed; // true if motor speed should be sent at next opportunity bool _send_motor_speed; // true if motor speed should be sent at next opportunity
int16_t _motor_speed; // desired motor speed (set from within update method) 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) uint32_t _last_send_motor_ms; // system time (in millis) last motor speed command was sent (used for health reporting)
uint32_t _last_send_motor_us; // system time (in micros) last motor speed command was sent (used for timing to unset DE pin) 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 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 // health reporting
HAL_Semaphore _last_healthy_sem;// semaphore protecting reading and updating of _last_send_motor_ms and _last_received_ms HAL_Semaphore _last_healthy_sem;// semaphore protecting reading and updating of _last_send_motor_ms and _last_received_ms
uint32_t _last_debug_ms; // system time (in millis) that debug was last output uint32_t _last_log_TRQD_ms; // system time (in millis) that TRQD was last logged
// message parsing members // message parsing members
ParseState _parse_state; // current state of parsing ParseState _parse_state; // current state of parsing
@ -138,6 +235,118 @@ private:
uint8_t _received_buff_len; // number of 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) 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
float 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; static AP_Torqeedo *_singleton;
}; };