PX4 Firmware
PX4 Autopilot Software http://px4.io
DShotOutput Class Reference
Inheritance diagram for DShotOutput:
Collaboration diagram for DShotOutput:

Classes

struct  Command
 
struct  Telemetry
 

Public Types

enum  Mode {
  MODE_NONE, MODE_1PWM, MODE_2PWM, MODE_2PWM2CAP,
  MODE_3PWM, MODE_3PWM1CAP, MODE_4PWM, MODE_4PWM1CAP,
  MODE_4PWM2CAP, MODE_5PWM, MODE_5PWM1CAP, MODE_6PWM,
  MODE_8PWM, MODE_14PWM, MODE_4CAP, MODE_5CAP,
  MODE_6CAP
}
 

Public Member Functions

 DShotOutput ()
 
virtual ~DShotOutput ()
 
void Run () override
 
int print_status () override
 
virtual int ioctl (file *filp, int cmd, unsigned long arg)
 
virtual int init ()
 
int set_mode (Mode mode)
 
Mode get_mode ()
 
bool updateOutputs (bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override
 Callback to update the (physical) actuator outputs in the driver. More...
 
void mixerChanged () override
 called whenever the mixer gets updated/reset More...
 
int sendCommandThreadSafe (dshot_command_t command, int num_repetitions, int motor_index)
 Send a dshot command to one or all motors This is expected to be called from another thread. More...
 
void retrieveAndPrintESCInfoThreadSafe (int motor_index)
 
bool telemetryEnabled () const
 
- Public Member Functions inherited from cdev::CDev
 CDev (const char *devname)
 Constructor. More...
 
 CDev (const CDev &)=delete
 
CDevoperator= (const CDev &)=delete
 
 CDev (CDev &&)=delete
 
CDevoperator= (CDev &&)=delete
 
virtual ~CDev ()
 
virtual int open (file_t *filep)
 Handle an open of the device. More...
 
virtual int close (file_t *filep)
 Handle a close of the device. More...
 
virtual ssize_t read (file_t *filep, char *buffer, size_t buflen)
 Perform a read from the device. More...
 
virtual ssize_t write (file_t *filep, const char *buffer, size_t buflen)
 Perform a write to the device. More...
 
virtual off_t seek (file_t *filep, off_t offset, int whence)
 Perform a logical seek operation on the device. More...
 
virtual int ioctl (file_t *filep, int cmd, unsigned long arg)
 Perform an ioctl operation on the device. More...
 
virtual int poll (file_t *filep, px4_pollfd_struct_t *fds, bool setup)
 Perform a poll setup/teardown operation. More...
 
const char * get_devname () const
 Get the device name. More...
 
- Public Member Functions inherited from OutputModuleInterface
 OutputModuleInterface (const char *name, const px4::wq_config_t &config)
 

Static Public Member Functions

static int task_spawn (int argc, char *argv[])
 
static int custom_command (int argc, char *argv[])
 
static int print_usage (const char *reason=nullptr)
 
static int module_new_mode (PortMode new_mode)
 change the mode of the running module More...
 
static void capture_trampoline (void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
 

Private Types

enum  DShotConfig {
  DShotConfig::Disabled = 0, DShotConfig::DShot150 = 150, DShotConfig::DShot300 = 300, DShotConfig::DShot600 = 600,
  DShotConfig::DShot1200 = 1200
}
 

Private Member Functions

void updateTelemetryNumMotors ()
 
void initTelemetry (const char *device)
 
void handleNewTelemetryData (int motor_index, const DShotTelemetry::EscData &data)
 
int requestESCInfo ()
 
void capture_callback (uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
 
int pwm_ioctl (file *filp, int cmd, unsigned long arg)
 
void update_dshot_out_state (bool on)
 
void update_params ()
 
int capture_ioctl (file *filp, int cmd, unsigned long arg)
 
 DShotOutput (const DShotOutput &)=delete
 
DShotOutput operator= (const DShotOutput &)=delete
 

Private Attributes

MixingOutput _mixing_output {DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}
 
Telemetry_telemetry {nullptr}
 
px4::atomic< DShotTelemetry::OutputBuffer * > _request_esc_info {nullptr}
 
bool _waiting_for_esc_info {false}
 
Mode _mode {MODE_NONE}
 
uORB::Subscription _param_sub {ORB_ID(parameter_update)}
 
Command _current_command
 
px4::atomic< Command * > _new_command {nullptr}
 
unsigned _num_outputs {0}
 
int _class_instance {-1}
 
bool _outputs_on {false}
 
uint32_t _output_mask {0}
 
bool _outputs_initialized {false}
 
perf_counter_t _cycle_perf
 

Static Private Attributes

static constexpr uint16_t DISARMED_VALUE = 0
 
static char _telemetry_device [20] {}
 
static px4::atomic_bool _request_telemetry_init {false}
 

Additional Inherited Members

- Static Public Attributes inherited from OutputModuleInterface
static constexpr int MAX_ACTUATORS = PWM_OUTPUT_MAX_CHANNELS
 
- Protected Member Functions inherited from cdev::CDev
virtual pollevent_t poll_state (file_t *filep)
 Check the current state of the device for poll events from the perspective of the file. More...
 
virtual void poll_notify (pollevent_t events)
 Report new poll events. More...
 
virtual void poll_notify_one (px4_pollfd_struct_t *fds, pollevent_t events)
 Internal implementation of poll_notify. More...
 
virtual int open_first (file_t *filep)
 Notification of the first open. More...
 
virtual int close_last (file_t *filep)
 Notification of the last close. More...
 
virtual int register_class_devname (const char *class_devname)
 Register a class device name, automatically adding device class instance suffix if need be. More...
 
virtual int unregister_class_devname (const char *class_devname, unsigned class_instance)
 Register a class device name, automatically adding device class instance suffix if need be. More...
 
void lock ()
 Take the driver lock. More...
 
void unlock ()
 Release the driver lock. More...
 
int unregister_driver_and_memory ()
 First, unregisters the driver. More...
 
- Protected Attributes inherited from cdev::CDev
px4_sem_t _lock
 lock to protect access to all class members (also for derived classes) More...
 
- Static Protected Attributes inherited from cdev::CDev
static const px4_file_operations_t fops = {}
 Pointer to the default cdev file operations table; useful for registering clone devices etc. More...
 

Detailed Description

Definition at line 96 of file dshot.cpp.

Member Enumeration Documentation

◆ DShotConfig

enum DShotOutput::DShotConfig
strongprivate
Enumerator
Disabled 
DShot150 
DShot300 
DShot600 
DShot1200 

Definition at line 170 of file dshot.cpp.

◆ Mode

Enumerator
MODE_NONE 
MODE_1PWM 
MODE_2PWM 
MODE_2PWM2CAP 
MODE_3PWM 
MODE_3PWM1CAP 
MODE_4PWM 
MODE_4PWM1CAP 
MODE_4PWM2CAP 
MODE_5PWM 
MODE_5PWM1CAP 
MODE_6PWM 
MODE_8PWM 
MODE_14PWM 
MODE_4CAP 
MODE_5CAP 
MODE_6CAP 

Definition at line 99 of file dshot.cpp.

Constructor & Destructor Documentation

◆ DShotOutput() [1/2]

DShotOutput::DShotOutput ( )

Definition at line 246 of file dshot.cpp.

References _mixing_output, DSHOT_MAX_THROTTLE, MixingOutput::setAllDisarmedValues(), MixingOutput::setAllMaxValues(), and MixingOutput::setAllMinValues().

Referenced by task_spawn().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ~DShotOutput()

DShotOutput::~DShotOutput ( )
virtual

Definition at line 257 of file dshot.cpp.

References _class_instance, _telemetry, perf_free(), PWM_OUTPUT_BASE_DEVICE_PATH, cdev::CDev::unregister_class_devname(), and up_dshot_arm().

Here is the call graph for this function:

◆ DShotOutput() [2/2]

DShotOutput::DShotOutput ( const DShotOutput )
privatedelete

Member Function Documentation

◆ capture_callback()

void DShotOutput::capture_callback ( uint32_t  chan_index,
hrt_abstime  edge_time,
uint32_t  edge_state,
uint32_t  overflow 
)
private

Definition at line 500 of file dshot.cpp.

Referenced by capture_trampoline().

Here is the caller graph for this function:

◆ capture_ioctl()

◆ capture_trampoline()

void DShotOutput::capture_trampoline ( void *  context,
uint32_t  chan_index,
hrt_abstime  edge_time,
uint32_t  edge_state,
uint32_t  overflow 
)
static

Definition at line 492 of file dshot.cpp.

References capture_callback().

Here is the call graph for this function:

◆ custom_command()

◆ get_mode()

Mode DShotOutput::get_mode ( )
inline

Definition at line 143 of file dshot.cpp.

References _mode, command, and hrt_abstime.

Referenced by module_new_mode().

Here is the caller graph for this function:

◆ handleNewTelemetryData()

void DShotOutput::handleNewTelemetryData ( int  motor_index,
const DShotTelemetry::EscData data 
)
private

◆ init()

int DShotOutput::init ( )
virtual

Reimplemented from cdev::CDev.

Definition at line 270 of file dshot.cpp.

References _class_instance, _mixing_output, CLASS_DEVICE_PRIMARY, ToneAlarmInterface::init(), OK, PWM_OUTPUT_BASE_DEVICE_PATH, cdev::CDev::register_class_devname(), MixingOutput::setDriverInstance(), and update_params().

Referenced by task_spawn().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ initTelemetry()

void DShotOutput::initTelemetry ( const char *  device)
private

Definition at line 563 of file dshot.cpp.

References _telemetry, DShotOutput::Telemetry::handler, DShotTelemetry::init(), and updateTelemetryNumMotors().

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ioctl()

int DShotOutput::ioctl ( file filp,
int  cmd,
unsigned long  arg 
)
virtual

Definition at line 821 of file dshot.cpp.

References _mode, capture_ioctl(), MODE_14PWM, MODE_1PWM, MODE_2PWM, MODE_2PWM2CAP, MODE_3PWM, MODE_3PWM1CAP, MODE_4PWM, MODE_4PWM1CAP, MODE_4PWM2CAP, MODE_5PWM, MODE_5PWM1CAP, MODE_6PWM, MODE_8PWM, and pwm_ioctl().

Here is the call graph for this function:

◆ mixerChanged()

void DShotOutput::mixerChanged ( )
overridevirtual

called whenever the mixer gets updated/reset

Reimplemented from OutputModuleInterface.

Definition at line 681 of file dshot.cpp.

References updateTelemetryNumMotors().

Here is the call graph for this function:

◆ module_new_mode()

int DShotOutput::module_new_mode ( PortMode  new_mode)
static

change the mode of the running module

Definition at line 1237 of file dshot.cpp.

References get_mode(), is_running(), MODE_14PWM, MODE_1PWM, MODE_2PWM, MODE_2PWM2CAP, MODE_3PWM, MODE_3PWM1CAP, MODE_4PWM, MODE_4PWM1CAP, MODE_4PWM2CAP, MODE_5PWM, MODE_5PWM1CAP, MODE_6PWM, MODE_8PWM, MODE_NONE, OK, PORT_FULL_GPIO, PORT_FULL_PWM, PORT_MODE_UNSET, PORT_PWM1, PORT_PWM2, PORT_PWM2CAP2, PORT_PWM3, PORT_PWM3CAP1, PORT_PWM4, PORT_PWM4CAP1, PORT_PWM4CAP2, PORT_PWM5, PORT_PWM5CAP1, PORT_PWM6, and PORT_PWM8.

Referenced by custom_command().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ operator=()

DShotOutput DShotOutput::operator= ( const DShotOutput )
privatedelete

◆ print_status()

int DShotOutput::print_status ( )
override
See also
ModuleBase::print_status()

Definition at line 1533 of file dshot.cpp.

References _mixing_output, _mode, _outputs_initialized, _outputs_on, _telemetry, DShotOutput::Telemetry::handler, MODE_1PWM, MODE_2PWM, MODE_2PWM2CAP, MODE_3PWM, MODE_3PWM1CAP, MODE_4CAP, MODE_4PWM, MODE_4PWM1CAP, MODE_4PWM2CAP, MODE_5CAP, MODE_5PWM, MODE_5PWM1CAP, MODE_6CAP, MODE_6PWM, MODE_8PWM, MODE_NONE, perf_print_counter(), DShotTelemetry::printStatus(), and MixingOutput::printStatus().

Here is the call graph for this function:

◆ print_usage()

int DShotOutput::print_usage ( const char *  reason = nullptr)
static
See also
ModuleBase

Definition at line 1592 of file dshot.cpp.

Referenced by custom_command().

Here is the caller graph for this function:

◆ pwm_ioctl()

int DShotOutput::pwm_ioctl ( file filp,
int  cmd,
unsigned long  arg 
)
private

◆ requestESCInfo()

int DShotOutput::requestESCInfo ( )
private

Definition at line 669 of file dshot.cpp.

References _mixing_output, _request_esc_info, _telemetry, _waiting_for_esc_info, DShotOutput::Command::command, DShot_cmd_esc_info, DShotOutput::Telemetry::handler, DShotOutput::Command::motor_mask, DShotOutput::Command::num_repetitions, DShotTelemetry::redirectOutput(), and MixingOutput::reorderedMotorIndex().

Referenced by updateOutputs().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ retrieveAndPrintESCInfoThreadSafe()

void DShotOutput::retrieveAndPrintESCInfoThreadSafe ( int  motor_index)

Definition at line 640 of file dshot.cpp.

References _request_esc_info, DShotTelemetry::OutputBuffer::buf_pos, DShotTelemetry::decodeAndPrintEscInfoPacket(), and DShotTelemetry::OutputBuffer::motor_index.

Here is the call graph for this function:

◆ Run()

◆ sendCommandThreadSafe()

int DShotOutput::sendCommandThreadSafe ( dshot_command_t  command,
int  num_repetitions,
int  motor_index 
)

Send a dshot command to one or all motors This is expected to be called from another thread.

Parameters
num_repetitionsnumber of times to repeat, set at least to 1
motor_indexindex or -1 for all
Returns
0 on success, <0 error otherwise

Definition at line 617 of file dshot.cpp.

References _mixing_output, _new_command, command, DShotOutput::Command::command, DShotOutput::Command::motor_mask, DShotOutput::Command::num_repetitions, and MixingOutput::reorderedMotorIndex().

Here is the call graph for this function:

◆ set_mode()

int DShotOutput::set_mode ( Mode  mode)

Definition at line 299 of file dshot.cpp.

References _mode, _num_outputs, _output_mask, _outputs_initialized, MODE_14PWM, MODE_1PWM, MODE_2PWM, MODE_2PWM2CAP, MODE_3PWM, MODE_3PWM1CAP, MODE_4PWM, MODE_4PWM1CAP, MODE_4PWM2CAP, MODE_5PWM, MODE_5PWM1CAP, MODE_6PWM, MODE_8PWM, MODE_NONE, OK, Rising, up_input_capture_set(), and update_dshot_out_state().

Referenced by capture_ioctl(), and pwm_ioctl().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ task_spawn()

int DShotOutput::task_spawn ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 468 of file dshot.cpp.

References DShotOutput(), init(), and ll40ls::instance.

Referenced by custom_command().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ telemetryEnabled()

bool DShotOutput::telemetryEnabled ( ) const
inline

Definition at line 165 of file dshot.cpp.

Referenced by custom_command().

Here is the caller graph for this function:

◆ update_dshot_out_state()

void DShotOutput::update_dshot_out_state ( bool  on)
private

Definition at line 507 of file dshot.cpp.

References _output_mask, _outputs_initialized, _outputs_on, DShot1200, DSHOT1200, DShot150, DSHOT150, DShot300, DSHOT300, DShot600, DSHOT600, up_dshot_arm(), and up_dshot_init().

Referenced by Run(), and set_mode().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ update_params()

void DShotOutput::update_params ( )
private

Definition at line 807 of file dshot.cpp.

References _mixing_output, _param_sub, math::constrain(), DSHOT_MAX_THROTTLE, MixingOutput::setAllMinValues(), and uORB::Subscription::update().

Referenced by init(), and Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ updateOutputs()

bool DShotOutput::updateOutputs ( bool  stop_motors,
uint16_t  outputs[MAX_ACTUATORS],
unsigned  num_outputs,
unsigned  num_control_groups_updated 
)
overridevirtual

Callback to update the (physical) actuator outputs in the driver.

Parameters
stop_motorsif true, all motors must be stopped (if false, individual motors might still be stopped via outputs[i] == disarmed_value)
outputsindividual actuator outputs in range [min, max] or failsafe/disarmed value
num_outputsnumber of outputs (<= max_num_outputs)
num_control_groups_updatednumber of actuator_control groups updated
Returns
if true, the update got handled, and actuator_outputs can be published

Implements OutputModuleInterface.

Definition at line 686 of file dshot.cpp.

References _mixing_output, _outputs_on, _request_esc_info, _telemetry, _waiting_for_esc_info, DShotOutput::Command::clear(), DShotOutput::Command::command, DShot_cmd_motor_stop, DSHOT_MAX_THROTTLE, DShotTelemetry::expectingData(), DShotTelemetry::getRequestMotorIndex(), DShotOutput::Telemetry::handler, math::min(), DShotOutput::Command::motor_mask, DShotOutput::Command::num_repetitions, MixingOutput::reorderedMotorIndex(), requestESCInfo(), up_dshot_motor_command(), up_dshot_motor_data_set(), up_dshot_trigger(), and DShotOutput::Command::valid().

Here is the call graph for this function:

◆ updateTelemetryNumMotors()

void DShotOutput::updateTelemetryNumMotors ( )
private

Definition at line 548 of file dshot.cpp.

References _mixing_output, _telemetry, MixerGroup::get_multirotor_count(), DShotOutput::Telemetry::handler, MixingOutput::mixers(), and DShotTelemetry::setNumMotors().

Referenced by initTelemetry(), and mixerChanged().

Here is the call graph for this function:
Here is the caller graph for this function:

Member Data Documentation

◆ _class_instance

int DShotOutput::_class_instance {-1}
private

Definition at line 216 of file dshot.cpp.

Referenced by init(), and ~DShotOutput().

◆ _current_command

Command DShotOutput::_current_command
private

Definition at line 212 of file dshot.cpp.

◆ _cycle_perf

perf_counter_t DShotOutput::_cycle_perf
private

Definition at line 222 of file dshot.cpp.

◆ _mixing_output

MixingOutput DShotOutput::_mixing_output {DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}
private

◆ _mode

Mode DShotOutput::_mode {MODE_NONE}
private

Definition at line 208 of file dshot.cpp.

Referenced by capture_ioctl(), ioctl(), print_status(), pwm_ioctl(), and set_mode().

◆ _new_command

px4::atomic<Command *> DShotOutput::_new_command {nullptr}
private

Definition at line 213 of file dshot.cpp.

Referenced by Run(), and sendCommandThreadSafe().

◆ _num_outputs

unsigned DShotOutput::_num_outputs {0}
private

Definition at line 215 of file dshot.cpp.

Referenced by set_mode().

◆ _output_mask

uint32_t DShotOutput::_output_mask {0}
private

Definition at line 219 of file dshot.cpp.

Referenced by set_mode(), and update_dshot_out_state().

◆ _outputs_initialized

bool DShotOutput::_outputs_initialized {false}
private

Definition at line 220 of file dshot.cpp.

Referenced by print_status(), set_mode(), and update_dshot_out_state().

◆ _outputs_on

bool DShotOutput::_outputs_on {false}
private

Definition at line 218 of file dshot.cpp.

Referenced by print_status(), Run(), update_dshot_out_state(), and updateOutputs().

◆ _param_sub

uORB::Subscription DShotOutput::_param_sub {ORB_ID(parameter_update)}
private

Definition at line 210 of file dshot.cpp.

Referenced by Run(), and update_params().

◆ _request_esc_info

px4::atomic<DShotTelemetry::OutputBuffer *> DShotOutput::_request_esc_info {nullptr}
private

Definition at line 205 of file dshot.cpp.

Referenced by requestESCInfo(), retrieveAndPrintESCInfoThreadSafe(), Run(), and updateOutputs().

◆ _request_telemetry_init

px4::atomic_bool DShotOutput::_request_telemetry_init {false}
staticprivate

Definition at line 203 of file dshot.cpp.

◆ _telemetry

Telemetry* DShotOutput::_telemetry {nullptr}
private

◆ _telemetry_device

char DShotOutput::_telemetry_device {}
staticprivate

Definition at line 202 of file dshot.cpp.

Referenced by custom_command().

◆ _waiting_for_esc_info

bool DShotOutput::_waiting_for_esc_info {false}
private

Definition at line 206 of file dshot.cpp.

Referenced by requestESCInfo(), Run(), and updateOutputs().

◆ DISARMED_VALUE

constexpr uint16_t DShotOutput::DISARMED_VALUE = 0
staticprivate

Definition at line 168 of file dshot.cpp.


The documentation for this class was generated from the following file: