PX4 Firmware
PX4 Autopilot Software http://px4.io
|
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 | |
PX4FMU () | |
virtual | ~PX4FMU () |
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 () |
void | update_pwm_trims () |
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... | |
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) | |
virtual void | mixerChanged () |
called whenever the mixer gets updated/reset More... | |
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 | fmu_new_mode (PortMode new_mode) |
change the FMU mode of the running module More... | |
static int | test () |
static int | fake (int argc, char *argv[]) |
static int | set_i2c_bus_clock (unsigned bus, unsigned clock_hz) |
static void | capture_trampoline (void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) |
Private Member Functions | |
void | capture_callback (uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) |
void | update_current_rate () |
int | set_pwm_rate (unsigned rate_map, unsigned default_rate, unsigned alt_rate) |
int | pwm_ioctl (file *filp, int cmd, unsigned long arg) |
void | update_pwm_rev_mask () |
void | update_pwm_out_state (bool on) |
void | update_params () |
int | capture_ioctl (file *filp, int cmd, unsigned long arg) |
PX4FMU (const PX4FMU &)=delete | |
PX4FMU | operator= (const PX4FMU &)=delete |
Static Private Member Functions | |
static void | sensor_reset (int ms) |
static void | peripheral_reset (int ms) |
Private Attributes | |
MixingOutput | _mixing_output {FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true} |
Mode | _mode {MODE_NONE} |
unsigned | _pwm_default_rate {50} |
unsigned | _pwm_alt_rate {50} |
uint32_t | _pwm_alt_rate_channels {0} |
unsigned | _current_update_rate {0} |
uORB::Subscription | _parameter_update_sub {ORB_ID(parameter_update)} |
unsigned | _num_outputs {0} |
int | _class_instance {-1} |
bool | _pwm_on {false} |
uint32_t | _pwm_mask {0} |
bool | _pwm_initialized {false} |
bool | _test_mode {false} |
unsigned | _num_disarmed_set {0} |
perf_counter_t | _cycle_perf |
Static Private Attributes | |
static constexpr int | FMU_MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS |
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... | |
enum PX4FMU::Mode |
PX4FMU::PX4FMU | ( | ) |
Definition at line 211 of file fmu.cpp.
References _mixing_output, PWM_DEFAULT_MAX, PWM_DEFAULT_MIN, MixingOutput::setAllMaxValues(), and MixingOutput::setAllMinValues().
Referenced by task_spawn().
|
virtual |
Definition at line 221 of file fmu.cpp.
References _class_instance, _cycle_perf, perf_free(), PWM_OUTPUT_BASE_DEVICE_PATH, cdev::CDev::unregister_class_devname(), and up_pwm_servo_deinit().
|
privatedelete |
|
private |
Definition at line 700 of file fmu.cpp.
Referenced by capture_trampoline().
|
private |
Definition at line 1514 of file fmu.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 692 of file fmu.cpp.
References capture_callback().
Referenced by test().
|
static |
Definition at line 2043 of file fmu.cpp.
References fake(), fmu_new_mode(), is_running(), peripheral_reset(), 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(), sensor_reset(), task_spawn(), and test().
|
static |
Definition at line 2001 of file fmu.cpp.
References actuator_armed_s::armed, actuator_controls_s::control, actuator_armed_s::lockdown, orb_advert_t, orb_advertise(), ORB_ID, ORB_ID_VEHICLE_ATTITUDE_CONTROLS, orb_unadvertise(), and print_usage().
Referenced by custom_command().
|
static |
change the FMU mode of the running module
Definition at line 1666 of file fmu.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, PORT_PWM8, and set_i2c_bus_clock().
Referenced by custom_command().
|
inline |
Definition at line 149 of file fmu.cpp.
References _mode, and hrt_abstime.
Referenced by fmu_new_mode().
|
virtual |
Reimplemented from cdev::CDev.
Definition at line 233 of file fmu.cpp.
References _class_instance, _current_update_rate, _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().
|
virtual |
Definition at line 795 of file fmu.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().
Referenced by test().
|
staticprivate |
Definition at line 1504 of file fmu.cpp.
Referenced by custom_command().
|
override |
Definition at line 2189 of file fmu.cpp.
References _current_update_rate, _cycle_perf, _mixing_output, _mode, 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(), and MixingOutput::printStatus().
|
static |
Definition at line 2242 of file fmu.cpp.
Referenced by custom_command(), and fake().
|
private |
Definition at line 844 of file fmu.cpp.
References _mixing_output, _mode, _num_disarmed_set, _pwm_alt_rate, _pwm_alt_rate_channels, _pwm_default_rate, _test_mode, pwm_output_values::channel_count, MixingOutput::disarmedValue(), MixingOutput::failsafeValue(), FMU_MAX_ACTUATORS, MixerGroup::get_trims(), MixingOutput::loadMixerThreadSafe(), cdev::CDev::lock(), MixingOutput::maxValue(), MixingOutput::minValue(), MIXERIOCLOADBUF, MIXERIOCRESET, MixingOutput::mixers(), 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_HIGHEST_MAX, PWM_HIGHEST_MIN, PWM_LOWEST_MAX, PWM_LOWEST_MIN, PWM_SERVO_ARM, PWM_SERVO_CLEAR_ARM_OK, PWM_SERVO_DISARM, PWM_SERVO_ENTER_TEST_MODE, PWM_SERVO_EXIT_TEST_MODE, PWM_SERVO_GET, PWM_SERVO_GET_COUNT, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, PWM_SERVO_GET_DISARMED_PWM, PWM_SERVO_GET_FAILSAFE_PWM, PWM_SERVO_GET_MAX_PWM, PWM_SERVO_GET_MIN_PWM, PWM_SERVO_GET_RATEGROUP, PWM_SERVO_GET_SELECT_UPDATE_RATE, PWM_SERVO_GET_TRIM_PWM, PWM_SERVO_GET_UPDATE_RATE, 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, PWM_SERVO_SET_ARM_OK, PWM_SERVO_SET_COUNT, PWM_SERVO_SET_DISARMED_PWM, PWM_SERVO_SET_FAILSAFE_PWM, PWM_SERVO_SET_FORCE_SAFETY_OFF, PWM_SERVO_SET_FORCE_SAFETY_ON, PWM_SERVO_SET_MAX_PWM, PWM_SERVO_SET_MIN_PWM, PWM_SERVO_SET_MODE, PWM_SERVO_SET_SELECT_UPDATE_RATE, PWM_SERVO_SET_TRIM_PWM, PWM_SERVO_SET_UPDATE_RATE, MixingOutput::resetMixerThreadSafe(), set_mode(), set_pwm_rate(), MixerGroup::set_trims(), cdev::CDev::unlock(), up_pwm_servo_get(), up_pwm_servo_get_rate_group(), up_pwm_servo_set(), update_pwm_out_state(), update_pwm_trims(), and pwm_output_values::values.
Referenced by ioctl().
|
override |
Definition at line 744 of file fmu.cpp.
References _current_update_rate, _cycle_perf, _mixing_output, _num_disarmed_set, _parameter_update_sub, _pwm_on, actuator_armed_s::armed, MixingOutput::armed(), uORB::Subscription::copy(), actuator_armed_s::in_esc_calibration_mode, perf_begin(), perf_end(), MixingOutput::unregister(), MixingOutput::update(), update_current_rate(), update_params(), update_pwm_out_state(), uORB::Subscription::updated(), and MixingOutput::updateSubscriptions().
|
staticprivate |
Definition at line 1494 of file fmu.cpp.
Referenced by custom_command().
|
static |
Definition at line 561 of file fmu.cpp.
Referenced by fmu_new_mode().
int PX4FMU::set_mode | ( | Mode | mode | ) |
Definition at line 267 of file fmu.cpp.
References _mode, _num_outputs, _pwm_alt_rate, _pwm_alt_rate_channels, _pwm_default_rate, _pwm_initialized, _pwm_mask, 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 up_pwm_servo_deinit().
Referenced by capture_ioctl(), and pwm_ioctl().
|
private |
Definition at line 493 of file fmu.cpp.
References _current_update_rate, _pwm_alt_rate, _pwm_alt_rate_channels, _pwm_default_rate, FMU_MAX_ACTUATORS, OK, up_pwm_servo_get_rate_group(), and up_pwm_servo_set_rate_group_update().
Referenced by pwm_ioctl(), and update_pwm_out_state().
|
static |
Definition at line 668 of file fmu.cpp.
References init(), ll40ls::instance, and PX4FMU().
Referenced by custom_command().
|
static |
Definition at line 1809 of file fmu.cpp.
References input_capture_config_t::callback, capture_trampoline(), input_capture_stats_t::chan_in_edges_out, cdev::CDev::close(), input_capture_config_t::context, fd, INPUT_CAP_GET_CALLBACK, INPUT_CAP_GET_COUNT, INPUT_CAP_GET_STATS, INPUT_CAP_SET_CALLBACK, INPUT_CAPTURE_MAX_CHANNELS, ioctl(), cdev::CDev::open(), cdev::CDev::poll(), PWM_SERVO_ARM, PWM_SERVO_ENTER_TEST_MODE, PWM_SERVO_EXIT_TEST_MODE, PWM_SERVO_GET, PWM_SERVO_GET_COUNT, PWM_SERVO_SET, PWM_SERVO_SET_MODE, PX4FMU_DEVICE_PATH, and cdev::CDev::read().
Referenced by custom_command().
|
private |
Definition at line 567 of file fmu.cpp.
References _current_update_rate, _mixing_output, _pwm_alt_rate, _pwm_default_rate, math::constrain(), and MixingOutput::setMaxTopicUpdateRate().
Referenced by Run().
|
private |
Definition at line 786 of file fmu.cpp.
References update_pwm_rev_mask(), and update_pwm_trims().
Referenced by init(), and Run().
|
private |
Definition at line 707 of file fmu.cpp.
References _pwm_alt_rate, _pwm_alt_rate_channels, _pwm_default_rate, _pwm_initialized, _pwm_mask, set_pwm_rate(), up_pwm_servo_arm(), and up_pwm_servo_init().
Referenced by pwm_ioctl(), and Run().
|
private |
Definition at line 590 of file fmu.cpp.
References _class_instance, _mixing_output, CLASS_DEVICE_PRIMARY, CLASS_DEVICE_SECONDARY, FMU_MAX_ACTUATORS, param_find(), param_get(), PARAM_INVALID, and MixingOutput::reverseOutputMask().
Referenced by update_params().
void PX4FMU::update_pwm_trims | ( | ) |
Definition at line 624 of file fmu.cpp.
References _class_instance, _mixing_output, CLASS_DEVICE_PRIMARY, CLASS_DEVICE_SECONDARY, FMU_MAX_ACTUATORS, MixingOutput::mixers(), param_find(), param_get(), PARAM_INVALID, and MixerGroup::set_trims().
Referenced by pwm_ioctl(), and update_params().
|
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 719 of file fmu.cpp.
References _pwm_initialized, _test_mode, up_pwm_servo_set(), and up_pwm_update().
|
private |
Definition at line 180 of file fmu.cpp.
Referenced by init(), update_pwm_rev_mask(), update_pwm_trims(), and ~PX4FMU().
|
private |
Definition at line 174 of file fmu.cpp.
Referenced by init(), print_status(), Run(), set_pwm_rate(), and update_current_rate().
|
private |
Definition at line 189 of file fmu.cpp.
Referenced by print_status(), Run(), and ~PX4FMU().
|
private |
Definition at line 166 of file fmu.cpp.
Referenced by init(), print_status(), pwm_ioctl(), PX4FMU(), Run(), update_current_rate(), update_pwm_rev_mask(), and update_pwm_trims().
Definition at line 168 of file fmu.cpp.
Referenced by capture_ioctl(), ioctl(), print_status(), pwm_ioctl(), and set_mode().
|
private |
Definition at line 187 of file fmu.cpp.
Referenced by pwm_ioctl(), and Run().
|
private |
Definition at line 179 of file fmu.cpp.
Referenced by set_mode().
|
private |
|
private |
Definition at line 171 of file fmu.cpp.
Referenced by pwm_ioctl(), set_mode(), set_pwm_rate(), update_current_rate(), and update_pwm_out_state().
|
private |
Definition at line 172 of file fmu.cpp.
Referenced by pwm_ioctl(), set_mode(), set_pwm_rate(), and update_pwm_out_state().
|
private |
Definition at line 170 of file fmu.cpp.
Referenced by pwm_ioctl(), set_mode(), set_pwm_rate(), update_current_rate(), and update_pwm_out_state().
|
private |
Definition at line 184 of file fmu.cpp.
Referenced by set_mode(), update_pwm_out_state(), and updateOutputs().
|
private |
Definition at line 183 of file fmu.cpp.
Referenced by set_mode(), and update_pwm_out_state().
|
private |
Definition at line 185 of file fmu.cpp.
Referenced by pwm_ioctl(), and updateOutputs().
|
staticprivate |
Definition at line 163 of file fmu.cpp.
Referenced by pwm_ioctl(), set_pwm_rate(), update_pwm_rev_mask(), and update_pwm_trims().