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

AP_Torqeedo: rename TRQD_DEBUG to OPTIONS and make bitmask

This commit is contained in:
Randy Mackay 2021-08-09 13:45:05 +09:00
parent 2890c1c651
commit b147d59ab7
2 changed files with 32 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
libraries/AP_Torqeedo/AP_Torqeedo.cpp
View File

@ -26,7 +26,7 @@
#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 5 // log debug info at this interval in seconds #define TORQEEDO_LOG_INTERVAL_MS 5000 // log debug info at this interval in milliseconds
#define TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_US 300000 // motor speed sent every 0.3sec if connected to motor #define TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_US 300000 // motor speed sent every 0.3sec if connected to motor
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -58,12 +58,12 @@ const AP_Param::GroupInfo AP_Torqeedo::var_info[] = {
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO("DE_PIN", 3, AP_Torqeedo, _pin_de, -1), AP_GROUPINFO("DE_PIN", 3, AP_Torqeedo, _pin_de, -1),
// @Param: DEBUG // @Param: OPTIONS
// @DisplayName: Torqeedo Debug Level // @DisplayName: Torqeedo Options
// @Description: Debug level // @Description: Torqeedo Options Bitmask
// @Values: 0:None, 1:Logging only, 2:Logging and GCS // @Bitmask: 0:Log,1:Send debug to GCS
// @User: Advanced // @User: Advanced
AP_GROUPINFO("DEBUG", 4, AP_Torqeedo, _debug_level, (int8_t)DebugLevel::LOGGING_ONLY), AP_GROUPINFO("OPTIONS", 4, AP_Torqeedo, _options, (int8_t)options::LOG),
AP_GROUPEND AP_GROUPEND
}; };
@ -83,6 +83,7 @@ void AP_Torqeedo::init()
} }
// only init once // 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) { if (_initialised) {
return; return;
} }
@ -395,43 +396,44 @@ void AP_Torqeedo::send_motor_speed_cmd()
// output logging and debug messages (if required) // output logging and debug messages (if required)
void AP_Torqeedo::log_and_debug() void AP_Torqeedo::log_and_debug()
{ {
// exit immediately if debugging is disabled // exit immediately if options are all unset
if (_debug_level == DebugLevel::NONE) { if (_options == 0) {
return; return;
} }
// 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 < (1000 * TORQEEDO_LOG_INTERVAL)) { if (now_ms - _last_debug_ms < TORQEEDO_LOG_INTERVAL_MS) {
return; return;
} }
_last_debug_ms = now_ms;
const bool health = healthy(); const bool health = healthy();
const int16_t mot_speed = health ? _motor_speed : 0; const int16_t mot_speed = health ? _motor_speed : 0;
// @LoggerMessage: TRQD if ((_options & options::LOG) != 0) {
// @Description: Torqeedo Status // @LoggerMessage: TRQD
// @Field: TimeUS: Time since system startup // @Description: Torqeedo Status
// @Field: Health: Health // @Field: TimeUS: Time since system startup
// @Field: MotSpeed: Motor Speed (-1000 to 1000) // @Field: Health: Health
// @Field: SuccCnt: Success Count // @Field: MotSpeed: Motor Speed (-1000 to 1000)
// @Field: ErrCnt: Error Count // @Field: SuccCnt: Success Count
AP::logger().Write("TRQD", "TimeUS,Health,MotSpeed,SuccCnt,ErrCnt", "QbhII", // @Field: ErrCnt: Error Count
AP_HAL::micros64(), AP::logger().Write("TRQD", "TimeUS,Health,MotSpeed,SuccCnt,ErrCnt", "QBHII",
(int)health, AP_HAL::micros64(),
(int)mot_speed, health,
(unsigned long)_parse_success_count, mot_speed,
(unsigned long)_parse_error_count); _parse_success_count,
_parse_error_count);
}
if (_debug_level == DebugLevel::LOGGING_AND_GCS) { 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 spd:%d succ:%ld err:%ld",
(unsigned)health, (unsigned)health,
(int)mot_speed, (int)mot_speed,
(unsigned long)_parse_success_count, (unsigned long)_parse_success_count,
(unsigned long)_parse_error_count); (unsigned long)_parse_error_count);
} }
_last_debug_ms = now_ms;
} }
// get the AP_Torqeedo singleton // get the AP_Torqeedo singleton

10
libraries/AP_Torqeedo/AP_Torqeedo.h
View File

@ -74,10 +74,10 @@ private:
TYPE_MOTOR = 2 TYPE_MOTOR = 2
}; };
enum class DebugLevel { // OPTIONS parameter values
NONE = 0, enum options {
LOGGING_ONLY = 1, LOG = 1<<0,
LOGGING_AND_GCS = 2 DEBUG_TO_GCS = 1<<1,
}; };
// initialise serial port and gpio pins (run from background thread) // initialise serial port and gpio pins (run from background thread)
@ -114,7 +114,7 @@ private:
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_Enum<DebugLevel> _debug_level; // debug level AP_Int16 _options; // options bitmask
// members // members
AP_HAL::UARTDriver *_uart; // serial port to communicate with motor AP_HAL::UARTDriver *_uart; // serial port to communicate with motor