PX4模块设计之二十五：DShot模块

2023-05-16

PX4模块设计之二十五：DShot模块

1. DShot模块简介
2. DShot类继承关系
3. 模块入口函数
- 3.1 主入口dshot_main
- 3.2 自定义子命令custom_command
4. DShot模块重要函数
- 4.1 task_spawn
- 4.2 instantiate
- 4.3 init
- 4.4 Run
5. DShot命令发送和解析相关函数
- 5.1 send_command_thread_safe
- 5.2 retrieve_and_print_esc_info_thread_safe
- 5.3 handle_new_telemetry_data
- 5.4 handle_vehicle_commands
6. 总结
7. 参考资料

DShot是一种数字ESC协议，由Felix (KISS)设计开发，在与Boris 和 betaflight 团队的其他成员不断完善下，最终成为一种成熟稳定的协议。Steffen (BLHeli)将该协议移植到了BLHeli_S。目前已经成为穿越机的主流电调协议。

目前DSHOT协议有4种：

DSHOT150：150 kbps ~ 6.67us for 1bit, 106.72us for 1frame
DSHOT300：300 kbps ~ 3.33us for 1bit, 39.96 for 1frame
DSHOT600：600 kbps ~ 1.67us for 1bit, 26.72 for 1frame
DSHOT1200：1200 kbps ~ 0.83us for 1bit, 13.28 for 1frame

主要优势：

去除了PWM的抖动影响
主动感知checksum错误
高速传递ESC控制数据（快速响应）

注：ESC对checksum异常报文统计分析有助于了解PWM信号线干扰情况；若出现大量连续受干扰影响报文，将会影响到飞控的控制和响应。

1. DShot模块简介

模块子命令给出了主要模块功能：

start/stop/status
telemetry: 设置串口设备名称
支持正反方向转动控制
支持3D功能
支持ESC状态查询
支持ESC蜂鸣控制

### Description
This is the DShot output driver. It is similar to the fmu driver, and can be used as drop-in replacement
to use DShot as ESC communication protocol instead of PWM.

On startup, the module tries to occupy all available pins for DShot output.
It skips all pins already in use (e.g. by a camera trigger module).

It supports:
- DShot150, DShot300, DShot600, DShot1200
- telemetry via separate UART and publishing as esc_status message
- sending DShot commands via CLI

### Examples
Permanently reverse motor 1:
$ dshot reverse -m 1
$ dshot save -m 1
After saving, the reversed direction will be regarded as the normal one. So to reverse again repeat the same commands.

dshot <command> [arguments...]
 Commands:
   start

   telemetry     Enable Telemetry on a UART
     <device>    UART device

   reverse       Reverse motor direction
     [-m <val>]  Motor index (1-based, default=all)

   normal        Normal motor direction
     [-m <val>]  Motor index (1-based, default=all)

   save          Save current settings
     [-m <val>]  Motor index (1-based, default=all)

   3d_on         Enable 3D mode
     [-m <val>]  Motor index (1-based, default=all)

   3d_off        Disable 3D mode
     [-m <val>]  Motor index (1-based, default=all)

   beep1         Send Beep pattern 1
     [-m <val>]  Motor index (1-based, default=all)

   beep2         Send Beep pattern 2
     [-m <val>]  Motor index (1-based, default=all)

   beep3         Send Beep pattern 3
     [-m <val>]  Motor index (1-based, default=all)

   beep4         Send Beep pattern 4
     [-m <val>]  Motor index (1-based, default=all)

   beep5         Send Beep pattern 5
     [-m <val>]  Motor index (1-based, default=all)

   esc_info      Request ESC information
     -m <val>    Motor index (1-based)

   stop

   status        print status info

注：print_usage函数是具体对应实现。

class DShot : public cdev::CDev, public ModuleBase<DShot>, public OutputModuleInterface

class OutputModuleInterface : public px4::ScheduledWorkItem, public ModuleParams

注：DShot模块采用了ModuleBase和WorkQueue设计。

2. DShot类继承关系

这里主要是为了了解WorkItem是如何每次被调度到的。因为PX4模块在实际完成功能时市场类之间前后（继承和被继承）方法互相调用，相互之间的耦合是比较紧密的。

注：给我的感觉就是大量C语言钩子函数+对象强制cast+业务抽象，所以个人的感觉是既不像内核纯C设计，又不像C++对象UML建模设计，有点兼而有之。Anyway，慢慢适应是了解开源代码最好办法。俗语说：“能工作的代码就是好代码”，哈哈哈哈，对开源来说我觉得确实真的就是这样。

DShot
 ├──> CDev
 └──> OutputModuleInterface
     ├──> ModuleParams
     │   └──> ListNode
     └──> px4::ScheduledWorkItem // schedule_trampoline-->ScheduleNow
         └──> WorkItem
             ├──> IntrusiveSortedListNode
             └──> IntrusiveQueueNode

3. 模块入口函数

3.1 主入口dshot_main

这里比较简单，因为主要继承了ModuleBase，由ModlueBase来完成模块入口。

dshot_main
 └──> return DShot::main(argc, argv)

3.2 自定义子命令custom_command

模块除支持start/stop/status命令，自定义命令支持以下子命令：

telemetry: 设置串口设备名称
正反方向转动控制
3D功能
ESC状态查询
ESC蜂鸣控制

DShot::custom_command
 ├──> const char *verb = argv[0]
 ├──> <telemetry>  // telemetry can be requested before the module is started
 │   ├──> <argc > 1>
 │   │   ├──> strncpy(_telemetry_device, argv[1], sizeof(_telemetry_device) - 1)
 │   │   ├──> _telemetry_device[sizeof(_telemetry_device) - 1] = '\0'
 │   │   └──> _request_telemetry_init.store(true)
 │   └──> return 0
 ├──> [解析m参量，第几个电机]  //select motor index, default: -1=all
 ├──> <reverse><is_running>
 │   └──> return get_instance()->send_command_thread_safe(command, num_repetitions, motor_index)   //发送反向转动命令
 ├──> <normal><is_running>
 │   └──> return get_instance()->send_command_thread_safe(command, num_repetitions, motor_index)   //发送正常转动命令
 ├──> <save><is_running>
 │   └──> return get_instance()->send_command_thread_safe(command, num_repetitions, motor_index)   //保存设置
 ├──> <3d_on><is_running>
 │   └──> return get_instance()->send_command_thread_safe(command, num_repetitions, motor_index)   //3d设置打开
 ├──> <3d_off><is_running>
 │   └──> return get_instance()->send_command_thread_safe(command, num_repetitions, motor_index)   //3d设置关闭
 ├──> <beep1><is_running>
 │   └──> return get_instance()->send_command_thread_safe(command, num_repetitions, motor_index)   //1= low freq. 5 = high freq
 ├──> <beep2><is_running>
 │   └──> return get_instance()->send_command_thread_safe(command, num_repetitions, motor_index)   //1= low freq. 5 = high freq
 ├──> <beep3><is_running>
 │   └──> return get_instance()->send_command_thread_safe(command, num_repetitions, motor_index)   //1= low freq. 5 = high freq
 ├──> <beep4><is_running>
 │   └──> return get_instance()->send_command_thread_safe(command, num_repetitions, motor_index)   //1= low freq. 5 = high freq
 ├──> <beep5><is_running>
 │   └──> return get_instance()->send_command_thread_safe(command, num_repetitions, motor_index)   //1= low freq. 5 = high freq
 ├──> <esc_info><is_running>
 │   ├──> get_instance()->retrieve_and_print_esc_info_thread_safe(motor_index)                 //获取ESC信息
 │   └──> return 0
 ├──> <!is_running()>
 │   └──> int ret = DShot::task_spawn(argc, argv)                      
 └──> return print_usage("unknown command");

注1：根据drv_pwm_output.h给出的完整命令集，上述custom_command仅支持了部分通用命令码。

typedef enum {
	DShot_cmd_motor_stop = 0,
	DShot_cmd_beacon1,
	DShot_cmd_beacon2,
	DShot_cmd_beacon3,
	DShot_cmd_beacon4,
	DShot_cmd_beacon5,
	DShot_cmd_esc_info, // V2 includes settings
	DShot_cmd_spin_direction_1,
	DShot_cmd_spin_direction_2,
	DShot_cmd_3d_mode_off,
	DShot_cmd_3d_mode_on,
	DShot_cmd_settings_request, // Currently not implemented
	DShot_cmd_save_settings,
	DShot_cmd_spin_direction_normal   = 20,
	DShot_cmd_spin_direction_reversed = 21,
	DShot_cmd_led0_on,      // BLHeli32 only
	DShot_cmd_led1_on,      // BLHeli32 only
	DShot_cmd_led2_on,      // BLHeli32 only
	DShot_cmd_led3_on,      // BLHeli32 only
	DShot_cmd_led0_off,     // BLHeli32 only
	DShot_cmd_led1_off,     // BLHeli32 only
	DShot_cmd_led2_off,     // BLHeli32 only
	DShot_cmd_led4_off,     // BLHeli32 only
	DShot_cmd_audio_stream_mode_on_off              = 30, // KISS audio Stream mode on/off
	DShot_cmd_silent_mode_on_off                    = 31, // KISS silent Mode on/off
	DShot_cmd_signal_line_telemetry_disable         = 32,
	DShot_cmd_signal_line_continuous_erpm_telemetry = 33,
	DShot_cmd_MAX          = 47,     // >47 are throttle values
	DShot_cmd_MIN_throttle = 48,
	DShot_cmd_MAX_throttle = 2047
} dshot_command_t;

注2：目前代码上看似乎比BetaFlight的dshot_command.h定义更全面

typedef enum {
    DSHOT_CMD_MOTOR_STOP = 0,
    DSHOT_CMD_BEACON1,
    DSHOT_CMD_BEACON2,
    DSHOT_CMD_BEACON3,
    DSHOT_CMD_BEACON4,
    DSHOT_CMD_BEACON5,
    DSHOT_CMD_ESC_INFO, // V2 includes settings
    DSHOT_CMD_SPIN_DIRECTION_1,
    DSHOT_CMD_SPIN_DIRECTION_2,
    DSHOT_CMD_3D_MODE_OFF,
    DSHOT_CMD_3D_MODE_ON,
    DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented
    DSHOT_CMD_SAVE_SETTINGS,
    DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
    DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
    DSHOT_CMD_LED0_ON, // BLHeli32 only
    DSHOT_CMD_LED1_ON, // BLHeli32 only
    DSHOT_CMD_LED2_ON, // BLHeli32 only
    DSHOT_CMD_LED3_ON, // BLHeli32 only
    DSHOT_CMD_LED0_OFF, // BLHeli32 only
    DSHOT_CMD_LED1_OFF, // BLHeli32 only
    DSHOT_CMD_LED2_OFF, // BLHeli32 only
    DSHOT_CMD_LED3_OFF, // BLHeli32 only
    DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF = 30, // KISS audio Stream mode on/Off
    DSHOT_CMD_SILENT_MODE_ON_OFF = 31, // KISS silent Mode on/Off
    DSHOT_CMD_MAX = 47
} dshotCommands_e;

注3：也许将来某个时间点，开发者能有机会将Dshot Protocol能够抽象到一个组件，规范和统一起来（当前ESC控制器大量应用汇编语言，不容易理解啊！！！）。

4. DShot模块重要函数

4.1 task_spawn

这里主要初始化了DShot对象，后续通过WorkQueue来完成后续每个命令。

DShot::task_spawn
 ├──> DShot *instance = new DShot()
 ├──> <instance>
 │   ├──> _object.store(instance)
 │   ├──> _task_id = task_id_is_work_queue   // 使用了WorkQueue设计
 │   └──> <instance->init() == PX4_OK>       // 初始化，并执行第一次ScheduleNow(后续触发Run)
 │       └──> return PX4_OK;
 ├──> <else>
 │   └──> PX4_ERR("alloc failed")
 ├──> delete instance
 ├──> _object.store(nullptr)
 ├──> _task_id = -1
 └──> return PX4_ERROR

4.2 instantiate

注：鉴于该模块不采用任务(线程)，所以ModuleBase::run_trampoline无需执行，所以可以不实现。

4.3 init

在task_spawn中使用，对cdev设备初始化，并第一次调用ScheduleNow，触发Run过程。

DShot::init
 ├──> int ret = CDev::init()  // do regular cdev init
 ├──> <ret != OK)>
 │   └──> return ret
 ├──> _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH)  // try to claim the generic PWM output device node as well - it's OK if we fail at this
 ├──> <_class_instance == CLASS_DEVICE_PRIMARY>
 │   └──> // lets not be too verbose
 ├──> <else>
 │   └──> PX4_ERR("FAILED registering class device")
 ├──> _mixing_output.setDriverInstance(_class_instance)
 ├──> _output_mask = (1u << _num_outputs) - 1
 ├──> update_params()
 ├──> ScheduleNow()
 └──> return OK

4.4 Run

每次WorkQueue执行，会自动调用Run函数，详见WorkQueue::Run。

DShot::Run
 ├──> <should_exit()>
 │   ├──> ScheduleClear()
 │   ├──> _mixing_output.unregister()
 │   ├──> exit_and_cleanup()
 │   └──> return
 ├──> SmartLock lock_guard(_lock)
 ├──> perf_begin(_cycle_perf)
 ├──> _mixing_output.update()    //内部调用到DShot::updateOutputs，DShot协议实现细节。
 ├──> bool outputs_on = _mixing_output.armed().armed || _mixing_output.initialized() // update output status if armed or if mixer is loaded
 ├──> <_outputs_on != outputs_on>
 │   └──> enable_dshot_outputs(outputs_on)
 ├──> <_telemetry>
 │   ├──> int telem_update = _telemetry->handler.update()
 │   └──> <_waiting_for_esc_info>
 │       ├──> <telem_update != -1>
 │       │   ├──> _request_esc_info.store(nullptr)
 │       │   └──> _waiting_for_esc_info = false
 │       └──> <else if telem_update >= 0>
 │           └──> _handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData())
 ├──> <_parameter_update_sub.updated()>
 │   └──> update_params()
 ├──> <_request_telemetry_init.load()> // telemetry device update request?
 │   ├──> init_telemetry(_telemetry_device)
 │   └──> _request_telemetry_init.store(false)
 ├──> <!_current_command.valid()> // new command?
 │   ├──> Command *new_command = _new_command.load()
 │   └──> <new_command>
 │       ├──> _current_command = *new_command
 │       └──> _new_command.store(nullptr)
 ├──> handle_vehicle_commands()
 ├──> <!_mixing_output.armed().armed>
 │   └──> <_reversible_outputs != _mixing_output.reversibleOutputs()>
 │       ├──> _reversible_outputs = _mixing_output.reversibleOutputs()
 │       └──> update_params()
 ├──> _mixing_output.updateSubscriptions(true)   // 再次更新ScheduleNow时间
 └──> perf_end(_cycle_perf)

注：对DShot协议感兴趣的朋友，可以进一步深入分析DShot::updateOutputs。

5. DShot命令发送和解析相关函数

5.1 send_command_thread_safe

向ESC发送一条命令，具体发送指令的拼装会在DShot::updateOutputs中实现。

DShot::send_command_thread_safe
 ├──> cmd.command = command
 ├──> <motor_index == -1>
 │   └──> cmd.motor_mask = 0xff
 ├──> <else>
 │   └──> cmd.motor_mask = 1 << _mixing_output.reorderedMotorIndex(motor_index)
 ├──> cmd.num_repetitions = num_repetitions
 ├──> _new_command.store(&cmd)
 ├──> hrt_abstime timestamp_for_timeout = hrt_absolute_time()
 ├──> <while (_new_command.load()) >  // wait until main thread processed it
 │   ├──> <hrt_elapsed_time(&timestamp_for_timeout) < 2_s>
 │   │   └──> px4_usleep(1000)
 │   └──> <else>
 │       ├──> _new_command.store(nullptr)
 │       └──> PX4_WARN("DShot command timeout!")
 └──> return 0

5.2 retrieve_and_print_esc_info_thread_safe

向ESC获取状态数据，具体发送指令的拼装会在DShot::updateOutputs中实现。

DShot::retrieve_and_print_esc_info_thread_safe
 ├──> <_request_esc_info.load() != nullptr>
 │   └──> return // already in progress (not expected to ever happen)
 ├──> DShotTelemetry::OutputBuffer output_buffer{}
 ├──> output_buffer.motor_index = motor_index
 ├──> _request_esc_info.store(&output_buffer)  // start the request
 ├──> int max_time = 1000 // wait until processed
 ├──> <while (_request_esc_info.load() != nullptr && max_time-- > 0)>
 │   └──> px4_usleep(1000)
 ├──> _request_esc_info.store(nullptr); // just in case we time out...
 ├──> <output_buffer.buf_pos == 0>
 │   ├──> PX4_ERR("No data received. If telemetry is setup correctly, try again")
 │   └──> return
 └──> DShotTelemetry::decodeAndPrintEscInfoPacket(output_buffer)

5.3 handle_new_telemetry_data

解析ESC状态报文。

DShot::handle_new_telemetry_data
 ├──> esc_status_s &esc_status = _telemetry->esc_status_pub.get()   // fill in new motor data
 ├──> <telemetry_index < esc_status_s::CONNECTED_ESC_MAX>
 │   ├──> esc_status.esc_online_flags |= 1 << telemetry_index;
 │   ├──> esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index];
 │   ├──> esc_status.esc[telemetry_index].timestamp       = data.time;
 │   ├──> esc_status.esc[telemetry_index].esc_rpm         = (static_cast<int>(data.erpm) * 100) / (_param_mot_pole_count.get() / 2);
 │   ├──> esc_status.esc[telemetry_index].esc_voltage     = static_cast<float>(data.voltage) * 0.01f;
 │   ├──> esc_status.esc[telemetry_index].esc_current     = static_cast<float>(data.current) * 0.01f;
 │   ├──> esc_status.esc[telemetry_index].esc_temperature = static_cast<float>(data.temperature);
 │   └──> // TODO: accumulate consumption and use for battery estimation
 ├──> <telemetry_index <= _telemetry->last_telemetry_index>
 │   ├──> esc_status.timestamp = hrt_absolute_time();
 │   ├──> esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
 │   ├──> esc_status.esc_count = _telemetry->handler.numMotors();
 │   ├──> ++esc_status.counter;
 │   │
 │   ├──> [ // FIXME: mark all ESC's as online, otherwise commander complains even for a single dropout]
 │   ├──> esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
 │   ├──> esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1;
 │   │
 │   ├──> _telemetry->esc_status_pub.update();
 │   │
 │   ├──> [ // reset esc data (in case a motor times out, so we won't send stale data)]
 │   ├──> memset(&esc_status.esc, 0, sizeof(_telemetry->esc_status_pub.get().esc));
 │   └──> esc_status.esc_online_flags = 0;
 └──> _telemetry->last_telemetry_index = telemetry_index

5.4 handle_vehicle_commands

主要为了支持MAVLink命令：VEHICLE_CMD_CONFIGURE_ACTUATOR

DShot::handle_vehicle_commands
 ├──> vehicle_command_s vehicle_command
 └──> <while (!_current_command.valid() && _vehicle_command_sub.update(&vehicle_command))>
     └──> <vehicle_command.command == vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR>
         ├──> int function = (int)(vehicle_command.param5 + 0.5)
         ├──> <function < 1000>
         │   ├──> const int first_motor_function = 1 // from MAVLink ACTUATOR_OUTPUT_FUNCTION
         │   ├──> const int first_servo_function = 33
         │   ├──> <function >= first_motor_function && function < first_motor_function + actuator_test_s::MAX_NUM_MOTORS>
         │   │   └──> function = function - first_motor_function + actuator_test_s::FUNCTION_MOTOR1
         │   ├──> <function >= first_servo_function && function < first_servo_function + actuator_test_s::MAX_NUM_SERVOS>
         │   │   └──> function = function - first_servo_function + actuator_test_s::FUNCTION_SERVO1
         │   └──> <else>
         │       └──> function = INT32_MAX
         ├──> <else>
         │   └──> function -= 1000
         ├──> int type = (int)(vehicle_command.param1 + 0.5f)
         ├──> int index = -1
         ├──> <for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i)>
         │   ├──> <(int)_mixing_output.outputFunction(i) == function>
         │   └──> index = i
         ├──> vehicle_command_ack_s command_ack{};
         ├──> command_ack.command = vehicle_command.command;
         ├──> command_ack.target_system = vehicle_command.source_system;
         ├──> command_ack.target_component = vehicle_command.source_component;
         ├──> command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
         ├──> <index != -1>
         │   ├──> PX4_DEBUG("setting command: index: %i type: %i", index, type)
         │   ├──> _current_command.command = dshot_command_t::DShot_cmd_motor_stop
         │   ├──> <switch (type)>
         │   │   ├──> case 1: _current_command.command = dshot_command_t::DShot_cmd_beacon1; break;
         │   │   ├──> ccase 2: _current_command.command = dshot_command_t::DShot_cmd_3d_mode_on; break;
         │   │   ├──> ccase 3: _current_command.command = dshot_command_t::DShot_cmd_3d_mode_off; break;
         │   │   ├──> ccase 4: _current_command.command = dshot_command_t::DShot_cmd_spin_direction_1; break;
         │   │   └──> ccase 5: _current_command.command = dshot_command_t::DShot_cmd_spin_direction_2; break;
         │   ├──> <_current_command.command == dshot_command_t::DShot_cmd_motor_stop>
         │   │   └──> PX4_WARN("unknown command: %i", type)
         │   └──> <else>
         │       ├──> command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
         │       ├──> _current_command.motor_mask = 1 << index;
         │       ├──> _current_command.num_repetitions = 10;
         │       └──> _current_command.save = true;
         ├──> command_ack.timestamp = hrt_absolute_time()
         └──> _command_ack_pub.publish(command_ack)

6. 总结

关于DShot协议代码细节实现方面这里不再过多描述，感兴趣的朋友可以去详细看下DShot::updateOutputs，以及官方文档DSHOT ESC Protocol。

这里给出了PX4系统关于DShot模块的总体业务框架：功能点，命令字，以及基于High Resolution Timer/WorkQueue/ModuleBase的设计逻辑关系。

7. 参考资料

【1】PX4开源软件框架简明简介
【2】PX4 modules_main
【3】PX4模块设计之十一：Built-In框架
【4】PX4模块设计之十二：High Resolution Timer设计
【5】PX4模块设计之十三：WorkQueue设计
【6】PX4模块设计之十七：ModuleBase模块
【7】What is DSHOT?
【8】DSHOT ESC Protocol
【9】Dshot bidir blhelis support
【10】Run-length limited or RLL coding
【11】最详细的DSHOT协议介绍

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)