PX4 Firmware
PX4 Autopilot Software http://px4.io
|
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 | |
CDev & | operator= (const CDev &)=delete |
CDev (CDev &&)=delete | |
CDev & | operator= (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... | |
|
strongprivate |
enum DShotOutput::Mode |
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().
|
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().
|
privatedelete |
|
private |
Definition at line 500 of file dshot.cpp.
Referenced by capture_trampoline().
|
private |
Definition at line 1085 of file dshot.cpp.
References _mode, input_capture_config_t::callback, input_capture_stats_t::chan_in_edges_out, input_capture_config_t::channel, input_capture_config_t::context, input_capture_config_t::edge, input_capture_config_t::filter, INPUT_CAP_GET_CALLBACK, INPUT_CAP_GET_CLR_STATS, INPUT_CAP_GET_COUNT, INPUT_CAP_GET_EDGE, INPUT_CAP_GET_FILTER, INPUT_CAP_GET_STATS, INPUT_CAP_SET, INPUT_CAP_SET_CALLBACK, INPUT_CAP_SET_COUNT, INPUT_CAP_SET_EDGE, INPUT_CAP_SET_FILTER, cdev::CDev::lock(), MODE_2PWM2CAP, MODE_3PWM1CAP, MODE_4PWM1CAP, MODE_4PWM2CAP, MODE_5PWM1CAP, OK, set_mode(), cdev::CDev::unlock(), up_input_capture_get_callback(), up_input_capture_get_filter(), up_input_capture_get_stats(), up_input_capture_get_trigger(), up_input_capture_set(), up_input_capture_set_callback(), up_input_capture_set_filter(), and up_input_capture_set_trigger().
Referenced by ioctl().
|
static |
Definition at line 492 of file dshot.cpp.
References capture_callback().
|
static |
Definition at line 1365 of file dshot.cpp.
References _telemetry_device, DShotOutput::Command::command, DShot_cmd_3d_mode_off, DShot_cmd_3d_mode_on, DShot_cmd_beacon1, DShot_cmd_beacon2, DShot_cmd_beacon3, DShot_cmd_beacon4, DShot_cmd_beacon5, DShot_cmd_save_settings, DShot_cmd_spin_direction_normal, DShot_cmd_spin_direction_reversed, is_running(), module_new_mode(), name, DShotOutput::Command::num_repetitions, 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, PORT_PWM8, print_usage(), task_spawn(), and telemetryEnabled().
|
inline |
Definition at line 143 of file dshot.cpp.
References _mode, command, and hrt_abstime.
Referenced by module_new_mode().
|
private |
Definition at line 583 of file dshot.cpp.
References _telemetry, esc_status_s::counter, DShotTelemetry::EscData::current, DShotTelemetry::EscData::erpm, esc_status_s::esc, esc_status_s::esc_connectiontype, esc_status_s::esc_count, esc_report_s::esc_current, esc_status_s::esc_online_flags, esc_report_s::esc_rpm, DShotOutput::Telemetry::esc_status_pub, esc_report_s::esc_temperature, esc_report_s::esc_voltage, uORB::PublicationData< T >::get(), DShotOutput::Telemetry::handler, hrt_absolute_time(), DShotOutput::Telemetry::last_motor_index, DShotTelemetry::numMotors(), DShotTelemetry::EscData::temperature, DShotTelemetry::EscData::time, esc_report_s::timestamp, esc_status_s::timestamp, uORB::PublicationData< T >::update(), and DShotTelemetry::EscData::voltage.
Referenced by Run().
|
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().
|
private |
Definition at line 563 of file dshot.cpp.
References _telemetry, DShotOutput::Telemetry::handler, DShotTelemetry::init(), and updateTelemetryNumMotors().
Referenced by Run().
|
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().
|
overridevirtual |
called whenever the mixer gets updated/reset
Reimplemented from OutputModuleInterface.
Definition at line 681 of file dshot.cpp.
References updateTelemetryNumMotors().
|
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().
|
privatedelete |
|
override |
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().
|
static |
Definition at line 1592 of file dshot.cpp.
Referenced by custom_command().
|
private |
Definition at line 870 of file dshot.cpp.
References _mixing_output, _mode, MixingOutput::loadMixerThreadSafe(), cdev::CDev::lock(), MIXERIOCLOADBUF, MIXERIOCRESET, MODE_14PWM, 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, OK, PWM_SERVO_GET_COUNT, PWM_SERVO_MODE_1PWM, PWM_SERVO_MODE_2PWM, PWM_SERVO_MODE_2PWM2CAP, PWM_SERVO_MODE_3PWM, PWM_SERVO_MODE_3PWM1CAP, PWM_SERVO_MODE_4CAP, PWM_SERVO_MODE_4PWM, PWM_SERVO_MODE_4PWM1CAP, PWM_SERVO_MODE_4PWM2CAP, PWM_SERVO_MODE_5CAP, PWM_SERVO_MODE_5PWM, PWM_SERVO_MODE_5PWM1CAP, PWM_SERVO_MODE_6CAP, PWM_SERVO_MODE_6PWM, PWM_SERVO_MODE_8PWM, PWM_SERVO_MODE_NONE, PWM_SERVO_SET_COUNT, PWM_SERVO_SET_MODE, MixingOutput::resetMixerThreadSafe(), set_mode(), and cdev::CDev::unlock().
Referenced by ioctl().
|
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().
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.
|
override |
Definition at line 745 of file dshot.cpp.
References _mixing_output, _new_command, _outputs_on, _param_sub, _request_esc_info, _telemetry, _waiting_for_esc_info, actuator_armed_s::armed, MixingOutput::armed(), handleNewTelemetryData(), DShotOutput::Telemetry::handler, initTelemetry(), DShotTelemetry::latestESCData(), MixingOutput::mixers(), perf_begin(), perf_end(), MixingOutput::unregister(), DShotTelemetry::update(), MixingOutput::update(), update_dshot_out_state(), update_params(), uORB::Subscription::updated(), MixingOutput::updateSubscriptions(), and DShotOutput::Command::valid().
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.
num_repetitions | number of times to repeat, set at least to 1 |
motor_index | index or -1 for all |
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().
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().
|
static |
Definition at line 468 of file dshot.cpp.
References DShotOutput(), init(), and ll40ls::instance.
Referenced by custom_command().
|
inline |
Definition at line 165 of file dshot.cpp.
Referenced by custom_command().
|
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().
|
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().
|
overridevirtual |
Callback to update the (physical) actuator outputs in the driver.
stop_motors | if true, all motors must be stopped (if false, individual motors might still be stopped via outputs[i] == disarmed_value) |
outputs | individual actuator outputs in range [min, max] or failsafe/disarmed value |
num_outputs | number of outputs (<= max_num_outputs) |
num_control_groups_updated | number of actuator_control groups updated |
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().
|
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().
|
private |
Definition at line 216 of file dshot.cpp.
Referenced by init(), and ~DShotOutput().
|
private |
|
private |
Definition at line 199 of file dshot.cpp.
Referenced by DShotOutput(), init(), print_status(), pwm_ioctl(), requestESCInfo(), Run(), sendCommandThreadSafe(), update_params(), updateOutputs(), and updateTelemetryNumMotors().
Definition at line 208 of file dshot.cpp.
Referenced by capture_ioctl(), ioctl(), print_status(), pwm_ioctl(), and set_mode().
|
private |
Definition at line 213 of file dshot.cpp.
Referenced by Run(), and sendCommandThreadSafe().
|
private |
Definition at line 215 of file dshot.cpp.
Referenced by set_mode().
|
private |
Definition at line 219 of file dshot.cpp.
Referenced by set_mode(), and update_dshot_out_state().
|
private |
Definition at line 220 of file dshot.cpp.
Referenced by print_status(), set_mode(), and update_dshot_out_state().
|
private |
Definition at line 218 of file dshot.cpp.
Referenced by print_status(), Run(), update_dshot_out_state(), and updateOutputs().
|
private |
Definition at line 210 of file dshot.cpp.
Referenced by Run(), and update_params().
|
private |
Definition at line 205 of file dshot.cpp.
Referenced by requestESCInfo(), retrieveAndPrintESCInfoThreadSafe(), Run(), and updateOutputs().
|
staticprivate |
|
private |
Definition at line 201 of file dshot.cpp.
Referenced by handleNewTelemetryData(), initTelemetry(), print_status(), requestESCInfo(), Run(), updateOutputs(), updateTelemetryNumMotors(), and ~DShotOutput().
|
staticprivate |
Definition at line 202 of file dshot.cpp.
Referenced by custom_command().
|
private |
Definition at line 206 of file dshot.cpp.
Referenced by requestESCInfo(), Run(), and updateOutputs().
|
staticprivate |