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

Public Member Functions

 TAP_ESC (char const *const device, uint8_t channels_count)
 
virtual ~TAP_ESC ()
 
void run () override
 
virtual int init ()
 
virtual int ioctl (cdev::file_t *filp, int cmd, unsigned long arg)
 Perform an ioctl operation on the device. More...
 
void cycle ()
 
- 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 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...
 

Static Public Member Functions

static int task_spawn (int argc, char *argv[])
 
static TAP_ESCinstantiate (int argc, char *argv[])
 
static int custom_command (int argc, char *argv[])
 
static int print_usage (const char *reason=nullptr)
 

Private Member Functions

 DEFINE_PARAMETERS ((ParamInt< px4::params::MC_AIRMODE >) _param_mc_airmode) void subscribe()
 
void send_esc_outputs (const uint16_t *pwm, const uint8_t motor_cnt)
 
int control_callback (uint8_t control_group, uint8_t control_index, float &input)
 

Static Private Member Functions

static int control_callback_trampoline (uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
 

Private Attributes

char _device [DEVICE_ARGUMENT_MAX_LENGTH]
 
int _uart_fd = -1
 
bool _is_armed = false
 
int _armed_sub = -1
 
int _test_motor_sub = -1
 
uORB::Subscription _parameter_update_sub {ORB_ID(parameter_update)}
 
orb_advert_t _outputs_pub = nullptr
 
actuator_outputs_s _outputs = {}
 
actuator_armed_s _armed = {}
 
perf_counter_t _perf_control_latency
 
int _control_subs [actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
 
actuator_controls_s _controls [actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
 
orb_id_t _control_topics [actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
 
px4_pollfd_struct_t _poll_fds [actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
 
unsigned _poll_fds_num = 0
 
orb_advert_t _esc_feedback_pub = nullptr
 
orb_advert_t _to_mixer_status = nullptr
 mixer status flags More...
 
esc_status_s _esc_feedback = {}
 
uint8_t _channels_count = 0
 nnumber of ESC channels More...
 
uint8_t _responding_esc = 0
 
MixerGroup_mixers = nullptr
 
uint32_t _groups_required = 0
 
uint32_t _groups_subscribed = 0
 
ESC_UART_BUF _uartbuf = {}
 
EscPacket _packet = {}
 

Static Private Attributes

static const uint8_t _device_mux_map [TAP_ESC_MAX_MOTOR_NUM] = ESC_POS
 
static const uint8_t _device_dir_map [TAP_ESC_MAX_MOTOR_NUM] = ESC_DIR
 

Additional Inherited Members

- 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 84 of file tap_esc.cpp.

Constructor & Destructor Documentation

◆ TAP_ESC()

TAP_ESC::TAP_ESC ( char const *const  device,
uint8_t  channels_count 
)

Definition at line 158 of file tap_esc.cpp.

References _control_subs, _control_topics, _controls, _device, _outputs, _poll_fds, actuator_outputs_s::noutputs, ORB_ID, and actuator_outputs_s::output.

Referenced by instantiate().

Here is the caller graph for this function:

◆ ~TAP_ESC()

TAP_ESC::~TAP_ESC ( )
virtual

Definition at line 185 of file tap_esc.cpp.

References _armed_sub, _control_subs, _esc_feedback_pub, _outputs_pub, _perf_control_latency, _test_motor_sub, _to_mixer_status, _uart_fd, tap_esc_common::deinitialise_uart(), orb_unadvertise(), orb_unsubscribe(), and perf_free().

Here is the call graph for this function:

Member Function Documentation

◆ control_callback()

int TAP_ESC::control_callback ( uint8_t  control_group,
uint8_t  control_index,
float &  input 
)
inlineprivate

Definition at line 635 of file tap_esc.cpp.

References _armed, _controls, actuator_armed_s::armed, actuator_controls_s::control, f(), and actuator_armed_s::prearmed.

Referenced by control_callback_trampoline().

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

◆ control_callback_trampoline()

int TAP_ESC::control_callback_trampoline ( uintptr_t  handle,
uint8_t  control_group,
uint8_t  control_index,
float &  input 
)
staticprivate

Definition at line 629 of file tap_esc.cpp.

References control_callback().

Referenced by ioctl().

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

◆ custom_command()

int TAP_ESC::custom_command ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 264 of file tap_esc.cpp.

References print_usage().

Here is the call graph for this function:

◆ cycle()

void TAP_ESC::cycle ( )

Definition at line 406 of file tap_esc.cpp.

References _armed, _armed_sub, _channels_count, _control_subs, _control_topics, _controls, _esc_feedback, _esc_feedback_pub, _groups_required, _groups_subscribed, _is_armed, _mixers, _outputs, _outputs_pub, _packet, _parameter_update_sub, _perf_control_latency, _poll_fds, _poll_fds_num, _test_motor_sub, _to_mixer_status, _uart_fd, _uartbuf, test_motor_s::action, actuator_armed_s::armed, RunInfoRepsonse::channelID, math::constrain(), uORB::Subscription::copy(), esc_status_s::counter, EscPacket::d, esc_status_s::esc, esc_status_s::esc_connectiontype, esc_status_s::esc_count, esc_report_s::esc_rpm, esc_report_s::esc_state, ESCBUS_MSG_ID_RUN_INFO, RunInfoRepsonse::ESCStatus, MixerGroup::get_saturation_status(), hrt_absolute_time(), hrt_abstime, actuator_armed_s::lockdown, actuator_armed_s::manual_lockdown, MixerGroup::mix(), test_motor_s::motor_number, EscPacket::msg_id, actuator_outputs_s::noutputs, orb_check(), orb_copy(), ORB_ID, orb_publish(), orb_set_interval(), actuator_outputs_s::output, tap_esc_common::parse_tap_esc_feedback(), perf_set_elapsed(), px4_poll(), tap_esc_common::read_data_from_uart(), RPMMAX, RPMMIN, RPMSTOPPED, EscPacket::rspRunInfo, multirotor_motor_limits_s::saturation_status, send_esc_outputs(), MixerGroup::set_airmode(), RunInfoRepsonse::speed, linux_pwm_out::subscribe(), TAP_ESC_CTRL_UORB_UPDATE_INTERVAL, TAP_ESC_MAX_MOTOR_NUM, actuator_outputs_s::timestamp, esc_status_s::timestamp, actuator_controls_s::timestamp_sample, uORB::Subscription::updated(), and test_motor_s::value.

Referenced by run().

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

◆ DEFINE_PARAMETERS()

TAP_ESC::DEFINE_PARAMETERS ( (ParamInt< px4::params::MC_AIRMODE >)  _param_mc_airmode)
private
Parameters
_param_mc_airmodemulticopter air-mode

◆ init()

int TAP_ESC::init ( )
virtual

◆ instantiate()

TAP_ESC * TAP_ESC::instantiate ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 209 of file tap_esc.cpp.

References init(), print_usage(), and TAP_ESC().

Here is the call graph for this function:

◆ ioctl()

int TAP_ESC::ioctl ( cdev::file_t filep,
int  cmd,
unsigned long  arg 
)
virtual

Perform an ioctl operation on the device.

The default implementation handles DIOC_GETPRIV, and otherwise returns -ENOTTY. Subclasses should call the default implementation for any command they do not handle themselves.

Parameters
filepPointer to the NuttX file structure.
cmdThe ioctl command value.
argThe ioctl argument value.
Returns
OK on success, or -errno otherwise.

Reimplemented from cdev::CDev.

Definition at line 660 of file tap_esc.cpp.

References _groups_required, _mixers, control_callback_trampoline(), MixerGroup::groups_required(), MixerGroup::load_from_buf(), MIXERIOCLOADBUF, MIXERIOCRESET, and OK.

Here is the call graph for this function:

◆ print_usage()

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

Definition at line 750 of file tap_esc.cpp.

Referenced by custom_command(), and instantiate().

Here is the caller graph for this function:

◆ run()

void TAP_ESC::run ( )
override
See also
ModuleBase::run()
ModuleBase

Definition at line 715 of file tap_esc.cpp.

References cycle().

Here is the call graph for this function:

◆ send_esc_outputs()

void TAP_ESC::send_esc_outputs ( const uint16_t *  pwm,
const uint8_t  motor_cnt 
)
private

Definition at line 371 of file tap_esc.cpp.

References _channels_count, _responding_esc, _uart_fd, EscPacket::d, ESCBUS_MSG_ID_RUN, EscPacket::len, PACKET_HEAD, EscPacket::reqRun, RunReq::rpm_flags, RPMMAX, RPMSTOPPED, RUN_BLUE_LED_ON_MASK, RUN_FEEDBACK_ENABLE_MASK, tap_esc_common::send_packet(), and TAP_ESC_MAX_MOTOR_NUM.

Referenced by cycle().

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

◆ task_spawn()

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

Definition at line 724 of file tap_esc.cpp.

Member Data Documentation

◆ _armed

actuator_armed_s TAP_ESC::_armed = {}
private

Definition at line 122 of file tap_esc.cpp.

Referenced by control_callback(), and cycle().

◆ _armed_sub

int TAP_ESC::_armed_sub = -1
private

Definition at line 115 of file tap_esc.cpp.

Referenced by cycle(), init(), and ~TAP_ESC().

◆ _channels_count

uint8_t TAP_ESC::_channels_count = 0
private

nnumber of ESC channels

Definition at line 135 of file tap_esc.cpp.

Referenced by cycle(), init(), and send_esc_outputs().

◆ _control_subs

int TAP_ESC::_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
private

Definition at line 126 of file tap_esc.cpp.

Referenced by cycle(), init(), TAP_ESC(), and ~TAP_ESC().

◆ _control_topics

orb_id_t TAP_ESC::_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
private

Definition at line 128 of file tap_esc.cpp.

Referenced by cycle(), init(), and TAP_ESC().

◆ _controls

actuator_controls_s TAP_ESC::_controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
private

Definition at line 127 of file tap_esc.cpp.

Referenced by control_callback(), cycle(), and TAP_ESC().

◆ _device

char TAP_ESC::_device[DEVICE_ARGUMENT_MAX_LENGTH]
private

Definition at line 110 of file tap_esc.cpp.

Referenced by init(), and TAP_ESC().

◆ _device_dir_map

const uint8_t TAP_ESC::_device_dir_map = ESC_DIR
staticprivate

Definition at line 113 of file tap_esc.cpp.

Referenced by init().

◆ _device_mux_map

const uint8_t TAP_ESC::_device_mux_map = ESC_POS
staticprivate

Definition at line 112 of file tap_esc.cpp.

Referenced by init().

◆ _esc_feedback

esc_status_s TAP_ESC::_esc_feedback = {}
private

Definition at line 134 of file tap_esc.cpp.

Referenced by cycle(), and init().

◆ _esc_feedback_pub

orb_advert_t TAP_ESC::_esc_feedback_pub = nullptr
private

Definition at line 132 of file tap_esc.cpp.

Referenced by cycle(), init(), and ~TAP_ESC().

◆ _groups_required

uint32_t TAP_ESC::_groups_required = 0
private

Definition at line 139 of file tap_esc.cpp.

Referenced by cycle(), init(), and ioctl().

◆ _groups_subscribed

uint32_t TAP_ESC::_groups_subscribed = 0
private

Definition at line 140 of file tap_esc.cpp.

Referenced by cycle(), and init().

◆ _is_armed

bool TAP_ESC::_is_armed = false
private

Definition at line 114 of file tap_esc.cpp.

Referenced by cycle().

◆ _mixers

MixerGroup* TAP_ESC::_mixers = nullptr
private

Definition at line 138 of file tap_esc.cpp.

Referenced by cycle(), and ioctl().

◆ _outputs

actuator_outputs_s TAP_ESC::_outputs = {}
private

Definition at line 121 of file tap_esc.cpp.

Referenced by cycle(), init(), and TAP_ESC().

◆ _outputs_pub

orb_advert_t TAP_ESC::_outputs_pub = nullptr
private

Definition at line 120 of file tap_esc.cpp.

Referenced by cycle(), init(), and ~TAP_ESC().

◆ _packet

EscPacket TAP_ESC::_packet = {}
private

Definition at line 142 of file tap_esc.cpp.

Referenced by cycle().

◆ _parameter_update_sub

uORB::Subscription TAP_ESC::_parameter_update_sub {ORB_ID(parameter_update)}
private

Definition at line 118 of file tap_esc.cpp.

Referenced by cycle().

◆ _perf_control_latency

perf_counter_t TAP_ESC::_perf_control_latency
private

Definition at line 124 of file tap_esc.cpp.

Referenced by cycle(), and ~TAP_ESC().

◆ _poll_fds

px4_pollfd_struct_t TAP_ESC::_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
private

Definition at line 129 of file tap_esc.cpp.

Referenced by cycle(), init(), and TAP_ESC().

◆ _poll_fds_num

unsigned TAP_ESC::_poll_fds_num = 0
private

Definition at line 130 of file tap_esc.cpp.

Referenced by cycle(), and init().

◆ _responding_esc

uint8_t TAP_ESC::_responding_esc = 0
private

Definition at line 136 of file tap_esc.cpp.

Referenced by send_esc_outputs().

◆ _test_motor_sub

int TAP_ESC::_test_motor_sub = -1
private

Definition at line 116 of file tap_esc.cpp.

Referenced by cycle(), init(), and ~TAP_ESC().

◆ _to_mixer_status

orb_advert_t TAP_ESC::_to_mixer_status = nullptr
private

mixer status flags

Definition at line 133 of file tap_esc.cpp.

Referenced by cycle(), init(), and ~TAP_ESC().

◆ _uart_fd

int TAP_ESC::_uart_fd = -1
private

Definition at line 111 of file tap_esc.cpp.

Referenced by cycle(), init(), send_esc_outputs(), and ~TAP_ESC().

◆ _uartbuf

ESC_UART_BUF TAP_ESC::_uartbuf = {}
private

Definition at line 141 of file tap_esc.cpp.

Referenced by cycle().


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