PX4 Firmware
PX4 Autopilot Software http://px4.io
|
Classes | |
struct | MotorTest |
Public Member Functions | |
PX4IO (device::Device *interface) | |
Constructor. More... | |
virtual | ~PX4IO () |
Destructor. More... | |
virtual int | init () |
Initialize the PX4IO class. More... | |
int | init (bool disable_rc_handling, bool hitl_mode) |
Initialize the PX4IO class. More... | |
virtual int | detect () |
Detect if a PX4IO is connected. More... | |
virtual int | ioctl (file *filp, int cmd, unsigned long arg) |
IO Control handler. More... | |
virtual ssize_t | write (file *filp, const char *buffer, size_t len) |
write handler. More... | |
int | set_update_rate (int rate) |
Set the update rate for actuator outputs from FMU to IO. More... | |
int | disable_rc_handling () |
Disable RC input handling. More... | |
void | print_status (bool extended_status) |
Print IO status. More... | |
int | print_debug () |
Fetch and print debug console output. More... | |
void | test_fmu_fail (bool is_fail) |
uint16_t | system_status () 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... | |
Private Member Functions | |
void | task_main () |
worker task More... | |
int | io_set_control_state (unsigned group) |
Send controls for one group to IO. More... | |
int | io_set_control_groups () |
Send all controls to IO. More... | |
int | io_set_arming_state () |
Update IO's arming-related state. More... | |
int | io_set_rc_config () |
Push RC channel configuration to IO. More... | |
int | io_get_status () |
Fetch status and alarms from IO. More... | |
int | io_disable_rc_handling () |
Disable RC input handling. More... | |
int | io_get_raw_rc_input (input_rc_s &input_rc) |
Fetch RC inputs from IO. More... | |
int | io_publish_raw_rc () |
Fetch and publish raw RC input data. More... | |
int | io_publish_pwm_outputs () |
Fetch and publish the PWM servo outputs. More... | |
int | io_reg_set (uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) |
write register(s) More... | |
int | io_reg_set (uint8_t page, uint8_t offset, const uint16_t value) |
write a register More... | |
int | io_reg_get (uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) |
read register(s) More... | |
uint32_t | io_reg_get (uint8_t page, uint8_t offset) |
read a register More... | |
int | io_reg_modify (uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits) |
modify a register More... | |
int | mixer_send (const char *buf, unsigned buflen, unsigned retries=3) |
Send mixer definition text to IO. More... | |
int | io_handle_status (uint16_t status) |
Handle a status update from IO. More... | |
int | io_handle_alarms (uint16_t alarms) |
Handle an alarm update from IO. More... | |
void | dsm_bind_ioctl (int dsmMode) |
Handle issuing dsm bind ioctl to px4io. More... | |
void | io_handle_vservo (uint16_t vservo, uint16_t vrssi) |
Handle a servorail update from IO. More... | |
void | handle_motor_test () |
check and handle test_motor topic updates More... | |
PX4IO (const PX4IO &) | |
PX4IO | operator= (const PX4IO &) |
Static Private Member Functions | |
static void | task_main_trampoline (int argc, char *argv[]) |
Trampoline to the worker task. More... | |
Private Attributes | |
device::Device * | _interface |
unsigned | _hardware |
Hardware revision. More... | |
unsigned | _max_actuators |
Maximum # of actuators supported by PX4IO. More... | |
unsigned | _max_controls |
Maximum # of controls supported by PX4IO. More... | |
unsigned | _max_rc_input |
Maximum receiver channels supported by PX4IO. More... | |
unsigned | _max_relays |
Maximum relays supported by PX4IO. More... | |
unsigned | _max_transfer |
Maximum number of I2C transfers supported by PX4IO. More... | |
unsigned | _update_interval |
Subscription interval limiting send rate. More... | |
bool | _rc_handling_disabled |
If set, IO does not evaluate, but only forward the RC values. More... | |
unsigned | _rc_chan_count |
Internal copy of the last seen number of RC channels. More... | |
uint64_t | _rc_last_valid |
last valid timestamp More... | |
volatile int | _task |
worker task id More... | |
volatile bool | _task_should_exit |
worker terminate flag More... | |
orb_advert_t | _mavlink_log_pub |
mavlink log pub More... | |
perf_counter_t | _perf_update |
local performance counter for status updates More... | |
perf_counter_t | _perf_write |
local performance counter for PWM control writes More... | |
perf_counter_t | _perf_sample_latency |
total system latency (based on passed-through timestamp) More... | |
uint16_t | _status |
Various IO status flags. More... | |
uint16_t | _alarms |
Various IO alarms. More... | |
uint16_t | _last_written_arming_s |
the last written arming state reg More... | |
uint16_t | _last_written_arming_c |
the last written arming state reg More... | |
int | _t_actuator_controls_0 |
actuator controls group 0 topic More... | |
uORB::Subscription | _t_actuator_controls_1 {ORB_ID(actuator_controls_1)} |
actuator controls group 1 topic More... | |
uORB::Subscription | _t_actuator_controls_2 {ORB_ID(actuator_controls_2)} |
uORB::Subscription | _t_actuator_controls_3 {ORB_ID(actuator_controls_3)} |
actuator controls group 2 topic More... | |
uORB::Subscription | _t_actuator_armed {ORB_ID(actuator_armed)} |
actuator controls group 3 topic More... | |
uORB::Subscription | _t_vehicle_control_mode {ORB_ID(vehicle_control_mode)} |
vehicle control mode topic More... | |
uORB::Subscription | _parameter_update_sub {ORB_ID(parameter_update)} |
parameter update topic More... | |
uORB::Subscription | _t_vehicle_command {ORB_ID(vehicle_command)} |
vehicle command topic More... | |
bool | _param_update_force |
force a parameter update More... | |
uORB::PublicationMulti< input_rc_s > | _to_input_rc {ORB_ID(input_rc)} |
uORB::PublicationMulti< actuator_outputs_s > | _to_outputs {ORB_ID(actuator_outputs)} |
uORB::PublicationMulti< multirotor_motor_limits_s > | _to_mixer_status {ORB_ID(multirotor_motor_limits)} |
uORB::Publication< servorail_status_s > | _to_servorail {ORB_ID(servorail_status)} |
uORB::Publication< safety_s > | _to_safety {ORB_ID(safety)} |
bool | _primary_pwm_device |
true if we are the default PWM output More... | |
bool | _lockdown_override |
allow to override the safety lockdown More... | |
bool | _armed |
wether the system is armed More... | |
bool | _override_available |
true if manual reversion mode is enabled More... | |
bool | _cb_flighttermination |
true if the flight termination circuit breaker is enabled More... | |
bool | _in_esc_calibration_mode |
do not send control outputs to IO (used for esc calibration) More... | |
int32_t | _rssi_pwm_chan |
RSSI PWM input channel. More... | |
int32_t | _rssi_pwm_max |
max RSSI input on PWM channel More... | |
int32_t | _rssi_pwm_min |
min RSSI input on PWM channel More... | |
int32_t | _thermal_control |
thermal control state More... | |
bool | _analog_rc_rssi_stable |
true when analog RSSI input is stable More... | |
float | _analog_rc_rssi_volt |
analog RSSI voltage More... | |
bool | _test_fmu_fail |
To test what happens if IO loses FMU. More... | |
MotorTest | _motor_test |
bool | _hitl_mode |
Hardware-in-the-loop simulation mode - don't publish actuator_outputs. More... | |
Static Private Attributes | |
static const uint32_t | _io_reg_get_error = 0x80000000 |
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... | |
PX4IO::PX4IO | ( | device::Device * | interface | ) |
Constructor.
Initialize all class variables.
Definition at line 472 of file px4io.cpp.
References ets_airspeed::g_dev.
Referenced by px4io_main(), and set_update_rate().
|
virtual |
Destructor.
Wait for worker thread to terminate.
Definition at line 516 of file px4io.cpp.
References _interface, _perf_sample_latency, _perf_update, _perf_write, _task, _task_should_exit, ets_airspeed::g_dev, and perf_free().
|
private |
|
virtual |
Detect if a PX4IO is connected.
Only validate if there is a PX4IO to talk to.
Definition at line 545 of file px4io.cpp.
References _io_reg_get_error, _mavlink_log_pub, _task, ToneAlarmInterface::init(), io_reg_get(), mavlink_log_emergency, OK, PX4IO_P_CONFIG_PROTOCOL_VERSION, PX4IO_PAGE_CONFIG, and PX4IO_PROTOCOL_VERSION.
Referenced by px4io_main(), and set_update_rate().
int PX4IO::disable_rc_handling | ( | ) |
Disable RC input handling.
Definition at line 1484 of file px4io.cpp.
References _rc_handling_disabled, and io_disable_rc_handling().
|
private |
Handle issuing dsm bind ioctl to px4io.
dsmMode | 0:dsm2, 1:dsmx |
Definition at line 1705 of file px4io.cpp.
References _mavlink_log_pub, _status, DSM2_BIND_PULSES, DSM_BIND_START, DSMX8_BIND_PULSES, DSMX_BIND_PULSES, ioctl(), mavlink_log_critical, mavlink_log_info, and PX4IO_P_STATUS_FLAGS_SAFETY_OFF.
Referenced by task_main().
|
private |
check and handle test_motor topic updates
Definition at line 1335 of file px4io.cpp.
References _max_actuators, _motor_test, test_motor_s::action, test_motor_s::driver_instance, hrt_absolute_time(), hrt_elapsed_time(), PX4IO::MotorTest::in_test_mode, io_reg_get(), io_reg_set(), test_motor_s::motor_number, PX4IO_PAGE_CONTROL_MAX_PWM, PX4IO_PAGE_CONTROL_MIN_PWM, PX4IO_PAGE_DIRECT_PWM, PX4IO_PAGE_DISARMED_PWM, PX4IO::MotorTest::test_motor_sub, PX4IO::MotorTest::timeout, test_motor_s::timeout_ms, test_motor_s::timestamp, uORB::Subscription::update(), test_motor_s::value, and pwm_output_values::values.
Referenced by task_main().
|
virtual |
Initialize the PX4IO class.
Retrieve relevant initial system parameters. Initialize PX4IO registers.
Reimplemented from cdev::CDev.
Definition at line 588 of file px4io.cpp.
References _hardware, _io_reg_get_error, _mavlink_log_pub, _max_actuators, _max_controls, _max_rc_input, _max_relays, _max_transfer, _primary_pwm_device, _rc_handling_disabled, _rssi_pwm_chan, _rssi_pwm_max, _rssi_pwm_min, _task, CBRK_IO_SAFETY_KEY, circuit_breaker_enabled(), vehicle_command_s::command, vehicle_command_s::confirmation, DM_INIT_REASON_IN_FLIGHT, DM_INIT_REASON_POWER_ON, DM_INIT_REASON_VOLATILE, errx, cdev::CDev::fops, hrt_absolute_time(), hrt_abstime, hrt_elapsed_time(), ToneAlarmInterface::init(), io_disable_rc_handling(), io_get_status(), io_reg_get(), io_reg_modify(), io_reg_set(), io_set_rc_config(), mavlink_log_critical, mavlink_log_emergency, OK, ORB_ID, vehicle_command_s::param1, param_find(), param_get(), PARAM_INVALID, param_set(), param_set_no_notification(), PWM_OUTPUT0_DEVICE_PATH, PX4IO_FORCE_SAFETY_MAGIC, PX4IO_P_CONFIG_ACTUATOR_COUNT, PX4IO_P_CONFIG_CONTROL_COUNT, PX4IO_P_CONFIG_HARDWARE_VERSION, PX4IO_P_CONFIG_MAX_TRANSFER, PX4IO_P_CONFIG_PROTOCOL_VERSION, PX4IO_P_CONFIG_RC_INPUT_COUNT, PX4IO_P_CONFIG_RELAY_COUNT, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, PX4IO_P_SETUP_ARMING_FMU_ARMED, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, PX4IO_P_SETUP_ARMING_LOCKDOWN, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_P_SETUP_REBOOT_BL, PX4IO_PAGE_CONFIG, PX4IO_PAGE_SETUP, PX4IO_PROTOCOL_VERSION, PX4IO_REBOOT_BL_MAGIC, register_driver(), vehicle_command_s::source_component, vehicle_command_s::source_system, vehicle_command_s::target_component, vehicle_command_s::target_system, task_main_trampoline(), and vehicle_command_s::timestamp.
Referenced by init().
int PX4IO::init | ( | bool | disable_rc_handling, |
bool | hitl_mode | ||
) |
Initialize the PX4IO class.
Retrieve relevant initial system parameters. Initialize PX4IO registers.
disable_rc_handling | set to true to forbid override / RC handling on IO |
hitl_mode | set to suppress publication of actuator_outputs - instead defer to pwm_out_sim |
Definition at line 580 of file px4io.cpp.
References _hitl_mode, _rc_handling_disabled, and init().
|
private |
Disable RC input handling.
Definition at line 1491 of file px4io.cpp.
References io_reg_modify(), PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED, and PX4IO_PAGE_SETUP.
Referenced by disable_rc_handling(), and init().
|
private |
Fetch RC inputs from IO.
input_rc | Input structure to populate. |
Definition at line 1785 of file px4io.cpp.
References _analog_rc_rssi_stable, _analog_rc_rssi_volt, _rc_chan_count, _rc_last_valid, _rssi_pwm_chan, _rssi_pwm_max, _rssi_pwm_min, pwm_output_values::channel_count, input_rc_s::channel_count, f(), hrt_absolute_time(), input_rc_s::input_source, io_reg_get(), OK, PX4IO_P_RAW_FRAME_COUNT, PX4IO_P_RAW_LOST_FRAME_COUNT, PX4IO_P_RAW_RC_BASE, PX4IO_P_RAW_RC_COUNT, PX4IO_P_RAW_RC_DATA, PX4IO_P_RAW_RC_FLAGS, PX4IO_P_RAW_RC_FLAGS_FAILSAFE, PX4IO_P_RAW_RC_FLAGS_RC_OK, PX4IO_P_RAW_RC_NRSSI, PX4IO_PAGE_RAW_RC_INPUT, input_rc_s::rc_failsafe, input_rc_s::rc_lost, input_rc_s::rc_lost_frame_count, input_rc_s::rc_ppm_frame_length, input_rc_s::rc_total_frame_count, input_rc_s::rssi, input_rc_s::timestamp, input_rc_s::timestamp_last_signal, and input_rc_s::values.
Referenced by io_publish_raw_rc().
|
private |
Fetch status and alarms from IO.
Also publishes battery voltage/current.
Definition at line 1761 of file px4io.cpp.
References io_handle_alarms(), io_handle_status(), io_handle_vservo(), io_reg_get(), OK, PX4IO_P_STATUS_FLAGS, and PX4IO_PAGE_STATUS.
Referenced by init(), and task_main().
|
private |
Handle an alarm update from IO.
Publish IO alarm information if necessary.
alarm | The status register |
Definition at line 1723 of file px4io.cpp.
References _alarms.
Referenced by io_get_status().
|
private |
Handle a status update from IO.
Publish IO status information if necessary.
status | The status register |
WARNING: This section handles in-air resets.
Get and handle the safety status
Definition at line 1657 of file px4io.cpp.
References _override_available, _status, _to_safety, hrt_absolute_time(), io_reg_modify(), safety_s::override_available, safety_s::override_enabled, uORB::Publication< T >::publish(), PX4IO_P_STATUS_FLAGS, PX4IO_P_STATUS_FLAGS_ARM_SYNC, PX4IO_P_STATUS_FLAGS_OVERRIDE, PX4IO_P_STATUS_FLAGS_SAFETY_OFF, PX4IO_PAGE_STATUS, safety, safety_s::safety_off, safety_s::safety_switch_available, status, and safety_s::timestamp.
Referenced by io_get_status().
|
private |
Handle a servorail update from IO.
Publish servo rail information if necessary.
vservo | vservo register |
vrssi | vrssi register |
Definition at line 1736 of file px4io.cpp.
References _analog_rc_rssi_stable, _analog_rc_rssi_volt, _to_servorail, f(), hrt_absolute_time(), uORB::Publication< T >::publish(), and servorail_status_s::timestamp.
Referenced by io_get_status().
|
private |
Fetch and publish the PWM servo outputs.
Definition at line 1932 of file px4io.cpp.
References _hitl_mode, _max_actuators, _to_mixer_status, _to_outputs, MultirotorMixer::saturation_status::flags, hrt_absolute_time(), io_reg_get(), actuator_outputs_s::noutputs, OK, actuator_outputs_s::output, uORB::PublicationMulti< T >::publish(), PX4IO_P_STATUS_MIXER, PX4IO_PAGE_SERVOS, PX4IO_PAGE_STATUS, multirotor_motor_limits_s::timestamp, actuator_outputs_s::timestamp, MultirotorMixer::saturation_status::valid, and MultirotorMixer::saturation_status::value.
Referenced by task_main().
|
private |
Fetch and publish raw RC input data.
Definition at line 1888 of file px4io.cpp.
References _rc_last_valid, _status, _to_input_rc, input_rc_s::input_source, io_get_raw_rc_input(), OK, uORB::PublicationMulti< T >::publish(), PX4IO_P_STATUS_FLAGS_RC_DSM, PX4IO_P_STATUS_FLAGS_RC_OK, PX4IO_P_STATUS_FLAGS_RC_PPM, PX4IO_P_STATUS_FLAGS_RC_SBUS, PX4IO_P_STATUS_FLAGS_RC_ST24, and input_rc_s::rc_lost.
Referenced by task_main().
|
private |
read register(s)
page | Register page to read from. |
offset | Register offset to start reading from. |
values | Pointer to array where values should be stored. |
num_values | The number of values to read. |
Definition at line 2003 of file px4io.cpp.
References _interface, _max_transfer, OK, and device::Device::read().
Referenced by detect(), handle_motor_test(), init(), io_get_raw_rc_input(), io_get_status(), io_publish_pwm_outputs(), io_reg_get(), io_reg_modify(), io_set_rc_config(), ioctl(), mixer_send(), and print_status().
|
private |
read a register
page | Register page to read from. |
offset | Register offset to start reading from. |
Definition at line 2022 of file px4io.cpp.
References _io_reg_get_error, io_reg_get(), and OK.
|
private |
modify a register
page | Register page to modify. |
offset | Register offset to modify. |
clearbits | Bits to clear in the register. |
setbits | Bits to set in the register. |
Definition at line 2034 of file px4io.cpp.
References io_reg_get(), io_reg_set(), and OK.
Referenced by init(), io_disable_rc_handling(), io_handle_status(), io_set_arming_state(), ioctl(), mixer_send(), and task_main().
|
private |
write register(s)
page | Register page to write to. |
offset | Register offset to start writing at. |
values | Pointer to array of values to write. |
num_values | The number of values to write. |
Definition at line 1978 of file px4io.cpp.
References _interface, _max_transfer, OK, and device::Device::write().
Referenced by handle_motor_test(), init(), io_reg_modify(), io_reg_set(), io_set_control_state(), io_set_rc_config(), ioctl(), mixer_send(), print_status(), task_main(), and write().
|
private |
write a register
page | Register page to write to. |
offset | Register offset to write to. |
value | Value to write. |
Definition at line 1997 of file px4io.cpp.
References io_reg_set().
|
private |
Update IO's arming-related state.
Definition at line 1403 of file px4io.cpp.
References _armed, _in_esc_calibration_mode, _last_written_arming_c, _last_written_arming_s, _lockdown_override, _override_available, _t_actuator_armed, _t_vehicle_control_mode, actuator_armed_s::armed, armed, uORB::Subscription::copy(), vehicle_control_mode_s::flag_external_manual_override_ok, actuator_armed_s::force_failsafe, actuator_armed_s::in_esc_calibration_mode, io_reg_modify(), actuator_armed_s::lockdown, actuator_armed_s::manual_lockdown, actuator_armed_s::prearmed, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, PX4IO_P_SETUP_ARMING_FMU_PREARMED, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, PX4IO_P_SETUP_ARMING_IO_ARM_OK, PX4IO_P_SETUP_ARMING_LOCKDOWN, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, PX4IO_PAGE_SETUP, and actuator_armed_s::ready_to_arm.
Referenced by task_main().
|
private |
Send all controls to IO.
Definition at line 1254 of file px4io.cpp.
References io_set_control_state().
Referenced by task_main().
|
private |
Send controls for one group to IO.
< actuator outputs
Definition at line 1267 of file px4io.cpp.
References _in_esc_calibration_mode, _max_actuators, _max_controls, _motor_test, _perf_sample_latency, _t_actuator_controls_0, _t_actuator_controls_1, _t_actuator_controls_2, _t_actuator_controls_3, _test_fmu_fail, math::constrain(), FLOAT_TO_REG, hrt_elapsed_time(), PX4IO::MotorTest::in_test_mode, io_reg_set(), OK, orb_check(), orb_copy(), ORB_ID, perf_set_elapsed(), PX4IO_PAGE_CONTROLS, PX4IO_PROTOCOL_MAX_CONTROL_COUNT, and uORB::Subscription::update().
Referenced by io_set_control_groups().
|
private |
Push RC channel configuration to IO.
Definition at line 1500 of file px4io.cpp.
References _mavlink_log_pub, _max_rc_input, io_reg_get(), io_reg_set(), mavlink_log_critical, OK, param_find(), param_get(), PX4IO_P_RC_CONFIG_ASSIGNMENT, PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH, PX4IO_P_RC_CONFIG_CENTER, PX4IO_P_RC_CONFIG_DEADZONE, PX4IO_P_RC_CONFIG_MAX, PX4IO_P_RC_CONFIG_MIN, PX4IO_P_RC_CONFIG_OPTIONS, PX4IO_P_RC_CONFIG_OPTIONS_ENABLED, PX4IO_P_RC_CONFIG_OPTIONS_REVERSE, PX4IO_P_RC_CONFIG_STRIDE, PX4IO_P_STATUS_FLAGS, PX4IO_P_STATUS_FLAGS_INIT_OK, PX4IO_PAGE_RC_CONFIG, and PX4IO_PAGE_STATUS.
Referenced by init(), and task_main().
|
virtual |
IO Control handler.
Handle all IOCTL calls to the PX4IO file descriptor.
[in] | filp | file handle (not used). This function is always called directly through object reference |
[in] | cmd | the IOCTL command |
[in] | the | IOCTL command parameter (optional) |
Definition at line 2437 of file px4io.cpp.
References _io_reg_get_error, cdev::CDev::_lock, _lockdown_override, _max_actuators, _motor_test, _test_fmu_fail, pwm_output_values::channel_count, DSM2_BIND_PULSES, dsm_bind_power_down, DSM_BIND_POWER_UP, dsm_bind_power_up, dsm_bind_reinit_uart, dsm_bind_send_pulses, dsm_bind_set_rx_out, DSM_BIND_START, DSMX8_BIND_PULSES, DSMX_BIND_PULSES, PX4IO::MotorTest::in_test_mode, io_reg_get(), io_reg_modify(), io_reg_set(), mixer_send(), MIXERIOCLOADBUF, MIXERIOCRESET, OK, PWM_HIGHEST_MAX, PWM_LOWEST_MIN, PWM_OUTPUT_MAX_CHANNELS, 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_DISABLE_LOCKDOWN, 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_SET, PWM_SERVO_SET_ARM_OK, PWM_SERVO_SET_DISABLE_LOCKDOWN, PWM_SERVO_SET_DISARMED_PWM, PWM_SERVO_SET_FAILSAFE_PWM, PWM_SERVO_SET_FORCE_FAILSAFE, 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_OVERRIDE_IMMEDIATE, PWM_SERVO_SET_SBUS_RATE, PWM_SERVO_SET_SELECT_UPDATE_RATE, PWM_SERVO_SET_TERMINATION_FAILSAFE, PWM_SERVO_SET_TRIM_PWM, PWM_SERVO_SET_UPDATE_RATE, PX4IO_CHECK_CRC, PX4IO_FORCE_SAFETY_MAGIC, PX4IO_INAIR_RESTART_ENABLE, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, PX4IO_P_SETUP_ARMING_IO_ARM_OK, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, PX4IO_P_SETUP_CRC, PX4IO_P_SETUP_DSM, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, PX4IO_P_SETUP_FEATURES_PWM_RSSI, PX4IO_P_SETUP_FEATURES_SBUS1_OUT, PX4IO_P_SETUP_FEATURES_SBUS2_OUT, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_P_SETUP_PWM_ALTRATE, PX4IO_P_SETUP_PWM_DEFAULTRATE, PX4IO_P_SETUP_PWM_RATES, PX4IO_P_SETUP_REBOOT_BL, PX4IO_P_SETUP_SBUS_RATE, PX4IO_P_SETUP_SET_DEBUG, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR, PX4IO_P_STATUS_FLAGS_SAFETY_OFF, PX4IO_PAGE_CONTROL_MAX_PWM, PX4IO_PAGE_CONTROL_MIN_PWM, PX4IO_PAGE_CONTROL_TRIM_PWM, PX4IO_PAGE_DIRECT_PWM, PX4IO_PAGE_DISARMED_PWM, PX4IO_PAGE_FAILSAFE_PWM, PX4IO_PAGE_PWM_INFO, PX4IO_PAGE_SERVOS, PX4IO_PAGE_SETUP, PX4IO_PAGE_STATUS, PX4IO_RATE_MAP_BASE, PX4IO_REBOOT_BOOTLOADER, PX4IO_SET_DEBUG, RC_INPUT_ENABLE_RSSI_ANALOG, RC_INPUT_ENABLE_RSSI_PWM, SBUS_SET_PROTO_VERSION, system_status(), and pwm_output_values::values.
Referenced by dsm_bind_ioctl(), and set_update_rate().
|
private |
Send mixer definition text to IO.
Definition at line 2096 of file px4io.cpp.
References _mavlink_log_pub, _max_transfer, px4io_mixdata::action, F2I_MIXER_ACTION_APPEND, F2I_MIXER_ACTION_RESET, px4io_mixdata::f2i_mixer_magic, F2I_MIXER_MAGIC, io_reg_get(), io_reg_modify(), io_reg_set(), mavlink_and_console_log_info, msg, print_debug(), PX4IO_P_SETUP_SET_DEBUG, PX4IO_P_STATUS_FLAGS, PX4IO_P_STATUS_FLAGS_MIXER_OK, PX4IO_PAGE_MIXERLOAD, PX4IO_PAGE_SETUP, PX4IO_PAGE_STATUS, px4io_mixdata::text, and warnx.
Referenced by ioctl().
int PX4IO::print_debug | ( | ) |
Fetch and print debug console output.
Definition at line 2052 of file px4io.cpp.
References cdev::CDev::close(), cdev::CDev::open(), cdev::CDev::poll(), cdev::CDev::read(), and warnx.
Referenced by mixer_send().
void PX4IO::print_status | ( | bool | extended_status | ) |
Print IO status.
Print all relevant IO status information
extended_status | Shows more verbose information (in particular RC config) |
Definition at line 2212 of file px4io.cpp.
References _hardware, _hitl_mode, _max_actuators, _max_controls, _max_rc_input, io_reg_get(), io_reg_set(), PX4IO_P_CONFIG_ACTUATOR_COUNT, PX4IO_P_CONFIG_ADC_INPUT_COUNT, PX4IO_P_CONFIG_BOOTLOADER_VERSION, PX4IO_P_CONFIG_CONTROL_COUNT, PX4IO_P_CONFIG_HARDWARE_VERSION, PX4IO_P_CONFIG_MAX_TRANSFER, PX4IO_P_CONFIG_PROTOCOL_VERSION, PX4IO_P_CONFIG_RC_INPUT_COUNT, PX4IO_P_CONFIG_RELAY_COUNT, PX4IO_P_RAW_RC_BASE, PX4IO_P_RAW_RC_COUNT, PX4IO_P_RAW_RC_DATA, PX4IO_P_RAW_RC_FLAGS, PX4IO_P_RAW_RC_FLAGS_FAILSAFE, PX4IO_P_RAW_RC_FLAGS_FRAME_DROP, PX4IO_P_RAW_RC_FLAGS_MAPPING_OK, PX4IO_P_RAW_RC_FLAGS_RC_DSM11, PX4IO_P_RC_BASE, PX4IO_P_RC_CONFIG_ASSIGNMENT, PX4IO_P_RC_CONFIG_CENTER, PX4IO_P_RC_CONFIG_DEADZONE, PX4IO_P_RC_CONFIG_MAX, PX4IO_P_RC_CONFIG_MIN, PX4IO_P_RC_CONFIG_OPTIONS, PX4IO_P_RC_CONFIG_OPTIONS_ENABLED, PX4IO_P_RC_CONFIG_OPTIONS_REVERSE, PX4IO_P_RC_CONFIG_STRIDE, PX4IO_P_RC_VALID, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM, PX4IO_P_SETUP_ARMING_FMU_ARMED, PX4IO_P_SETUP_ARMING_FMU_PREARMED, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, PX4IO_P_SETUP_ARMING_IO_ARM_OK, PX4IO_P_SETUP_ARMING_LOCKDOWN, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, PX4IO_P_SETUP_CRC, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, PX4IO_P_SETUP_FEATURES_PWM_RSSI, PX4IO_P_SETUP_FEATURES_SBUS1_OUT, PX4IO_P_SETUP_FEATURES_SBUS2_OUT, PX4IO_P_SETUP_PWM_ALTRATE, PX4IO_P_SETUP_PWM_DEFAULTRATE, PX4IO_P_SETUP_PWM_RATES, PX4IO_P_SETUP_PWM_REVERSE, PX4IO_P_SETUP_SBUS_RATE, PX4IO_P_SETUP_SET_DEBUG, PX4IO_P_SETUP_THERMAL, PX4IO_P_SETUP_TRIM_PITCH, PX4IO_P_SETUP_TRIM_ROLL, PX4IO_P_SETUP_TRIM_YAW, PX4IO_P_SETUP_VSERVO_SCALE, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_ACC_CURRENT, PX4IO_P_STATUS_ALARMS_FMU_LOST, PX4IO_P_STATUS_ALARMS_PWM_ERROR, PX4IO_P_STATUS_ALARMS_RC_LOST, PX4IO_P_STATUS_ALARMS_SERVO_CURRENT, PX4IO_P_STATUS_ALARMS_TEMPERATURE, PX4IO_P_STATUS_ALARMS_VBATT_LOW, PX4IO_P_STATUS_ALARMS_VSERVO_FAULT, PX4IO_P_STATUS_FLAGS, PX4IO_P_STATUS_FLAGS_ARM_SYNC, PX4IO_P_STATUS_FLAGS_FAILSAFE, PX4IO_P_STATUS_FLAGS_FMU_OK, PX4IO_P_STATUS_FLAGS_INIT_OK, PX4IO_P_STATUS_FLAGS_MIXER_OK, PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED, PX4IO_P_STATUS_FLAGS_OVERRIDE, PX4IO_P_STATUS_FLAGS_RAW_PWM, PX4IO_P_STATUS_FLAGS_RC_DSM, PX4IO_P_STATUS_FLAGS_RC_OK, PX4IO_P_STATUS_FLAGS_RC_PPM, PX4IO_P_STATUS_FLAGS_RC_SBUS, PX4IO_P_STATUS_FLAGS_RC_ST24, PX4IO_P_STATUS_FLAGS_SAFETY_OFF, PX4IO_P_STATUS_FREEMEM, PX4IO_P_STATUS_VRSSI, PX4IO_P_STATUS_VSERVO, PX4IO_PAGE_ACTUATORS, PX4IO_PAGE_CONFIG, PX4IO_PAGE_CONTROLS, PX4IO_PAGE_DISARMED_PWM, PX4IO_PAGE_FAILSAFE_PWM, PX4IO_PAGE_RAW_ADC_INPUT, PX4IO_PAGE_RAW_RC_INPUT, PX4IO_PAGE_RC_CONFIG, PX4IO_PAGE_RC_INPUT, PX4IO_PAGE_SERVOS, PX4IO_PAGE_SETUP, PX4IO_PAGE_STATUS, PX4IO_PROTOCOL_MAX_CONTROL_COUNT, PX4IO_THERMAL_OFF, REG_TO_FLOAT, and REG_TO_SIGNED.
int PX4IO::set_update_rate | ( | int | rate | ) |
Set the update rate for actuator outputs from FMU to IO.
[in] | rate | The rate in Hz actuator outpus are sent to IO. Min 10 Hz, max 400 Hz |
Definition at line 2947 of file px4io.cpp.
References _update_interval, cdev::CDev::close(), math::constrain(), detect(), DSM2_BIND_PULSES, DSM_BIND_START, DSMX8_BIND_PULSES, DSMX_BIND_PULSES, err, errx, fd, ets_airspeed::g_dev, hrt_absolute_time(), hrt_abstime, hrt_elapsed_time(), Airspeed::init(), Airspeed::ioctl(), device::Device::ioctl(), ioctl(), OK, cdev::CDev::open(), cdev::CDev::poll(), PWM_SERVO_ARM, PWM_SERVO_GET, PWM_SERVO_GET_COUNT, PWM_SERVO_SET_ARM_OK, PWM_SERVO_SET_DISABLE_LOCKDOWN, PX4IO(), PX4IO_CHECK_CRC, PX4IO_DEVICE_PATH, px4io_main(), PX4IO_P_STATUS_FLAGS_SAFETY_OFF, PX4IO_serial_interface(), cdev::CDev::read(), start(), lps25h::test(), UPDATE_INTERVAL_MAX, UPDATE_INTERVAL_MIN, warn, warnx, and write().
|
inline |
|
private |
worker task
Definition at line 902 of file px4io.cpp.
References _armed, _cb_flighttermination, _lockdown_override, _mavlink_log_pub, _max_actuators, _motor_test, _param_update_force, _parameter_update_sub, _perf_update, _primary_pwm_device, _rc_handling_disabled, _rssi_pwm_chan, _rssi_pwm_max, _rssi_pwm_min, _t_actuator_armed, _t_actuator_controls_0, _t_vehicle_command, _t_vehicle_control_mode, _task, _task_should_exit, _thermal_control, _update_interval, CBRK_FLIGHTTERM_KEY, CBRK_IO_SAFETY_KEY, circuit_breaker_enabled(), math::constrain(), uORB::Subscription::copy(), dsm_bind_ioctl(), FLOAT_TO_REG, handle_motor_test(), hrt_absolute_time(), hrt_abstime, PX4IO::MotorTest::in_test_mode, io_get_status(), IO_POLL_INTERVAL, io_publish_pwm_outputs(), io_publish_raw_rc(), io_reg_modify(), io_reg_set(), io_set_arming_state(), io_set_control_groups(), io_set_rc_config(), cdev::CDev::lock(), mavlink_log_critical, OK, ORB_CHECK_INTERVAL, ORB_ID, orb_set_interval(), orb_subscribe(), param_find(), param_get(), PARAM_INVALID, perf_begin(), perf_end(), cdev::CDev::poll(), PWM_OUTPUT0_DEVICE_PATH, PX4IO_P_SETUP_AIRMODE, PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_SBUS1_OUT, PX4IO_P_SETUP_FEATURES_SBUS2_OUT, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_P_SETUP_MOTOR_SLEW_MAX, PX4IO_P_SETUP_PWM_REVERSE, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, PX4IO_P_SETUP_SCALE_PITCH, PX4IO_P_SETUP_SCALE_ROLL, PX4IO_P_SETUP_SCALE_YAW, PX4IO_P_SETUP_THERMAL, PX4IO_P_SETUP_THR_MDL_FAC, PX4IO_P_SETUP_TRIM_PITCH, PX4IO_P_SETUP_TRIM_ROLL, PX4IO_P_SETUP_TRIM_YAW, PX4IO_PAGE_CONTROL_TRIM_PWM, PX4IO_PAGE_SETUP, PX4IO_THERMAL_IGNORE, PX4IO_THERMAL_OFF, SIGNED_TO_REG, cdev::CDev::unlock(), unregister_driver(), UPDATE_INTERVAL_MAX, UPDATE_INTERVAL_MIN, uORB::Subscription::updated(), pwm_output_values::values, and warnx.
|
staticprivate |
Trampoline to the worker task.
Definition at line 896 of file px4io.cpp.
References ets_airspeed::g_dev.
Referenced by init().
|
virtual |
write handler.
Handle writes to the PX4IO file descriptor.
[in] | filp | file handle (not used). This function is always called directly through object reference |
[in] | buffer | pointer to the data buffer to be written |
[in] | len | size in bytes to be written |
Definition at line 2916 of file px4io.cpp.
References _max_actuators, _perf_write, _test_fmu_fail, io_reg_set(), OK, perf_begin(), perf_end(), and PX4IO_PAGE_DIRECT_PWM.
Referenced by set_update_rate().
|
private |
|
private |
true when analog RSSI input is stable
Definition at line 290 of file px4io.cpp.
Referenced by io_get_raw_rc_input(), and io_handle_vservo().
|
private |
analog RSSI voltage
Definition at line 291 of file px4io.cpp.
Referenced by io_get_raw_rc_input(), and io_handle_vservo().
|
private |
wether the system is armed
Definition at line 280 of file px4io.cpp.
Referenced by io_set_arming_state(), and task_main().
|
private |
true if the flight termination circuit breaker is enabled
Definition at line 283 of file px4io.cpp.
Referenced by task_main().
|
private |
Hardware revision.
Definition at line 231 of file px4io.cpp.
Referenced by init(), and print_status().
|
private |
Hardware-in-the-loop simulation mode - don't publish actuator_outputs.
Definition at line 301 of file px4io.cpp.
Referenced by init(), io_publish_pwm_outputs(), and print_status().
|
private |
do not send control outputs to IO (used for esc calibration)
Definition at line 284 of file px4io.cpp.
Referenced by io_set_arming_state(), and io_set_control_state().
|
private |
Definition at line 229 of file px4io.cpp.
Referenced by io_reg_get(), io_reg_set(), and ~PX4IO().
|
staticprivate |
|
private |
the last written arming state reg
Definition at line 256 of file px4io.cpp.
Referenced by io_set_arming_state().
|
private |
the last written arming state reg
Definition at line 255 of file px4io.cpp.
Referenced by io_set_arming_state().
|
private |
allow to override the safety lockdown
Definition at line 279 of file px4io.cpp.
Referenced by io_set_arming_state(), ioctl(), and task_main().
|
private |
mavlink log pub
Definition at line 246 of file px4io.cpp.
Referenced by detect(), dsm_bind_ioctl(), init(), io_set_rc_config(), mixer_send(), and task_main().
|
private |
Maximum # of actuators supported by PX4IO.
Definition at line 232 of file px4io.cpp.
Referenced by handle_motor_test(), init(), io_publish_pwm_outputs(), io_set_control_state(), ioctl(), print_status(), task_main(), and write().
|
private |
Maximum # of controls supported by PX4IO.
Definition at line 233 of file px4io.cpp.
Referenced by init(), io_set_control_state(), and print_status().
|
private |
Maximum receiver channels supported by PX4IO.
Definition at line 234 of file px4io.cpp.
Referenced by init(), io_set_rc_config(), and print_status().
|
private |
|
private |
Maximum number of I2C transfers supported by PX4IO.
Definition at line 236 of file px4io.cpp.
Referenced by init(), io_reg_get(), io_reg_set(), and mixer_send().
|
private |
Definition at line 300 of file px4io.cpp.
Referenced by handle_motor_test(), io_set_control_state(), ioctl(), and task_main().
|
private |
true if manual reversion mode is enabled
Definition at line 281 of file px4io.cpp.
Referenced by io_handle_status(), and io_set_arming_state().
|
private |
|
private |
|
private |
total system latency (based on passed-through timestamp)
Definition at line 250 of file px4io.cpp.
Referenced by io_set_control_state(), and ~PX4IO().
|
private |
local performance counter for status updates
Definition at line 248 of file px4io.cpp.
Referenced by task_main(), and ~PX4IO().
|
private |
|
private |
true if we are the default PWM output
Definition at line 278 of file px4io.cpp.
Referenced by init(), and task_main().
|
private |
Internal copy of the last seen number of RC channels.
Definition at line 240 of file px4io.cpp.
Referenced by io_get_raw_rc_input().
|
private |
If set, IO does not evaluate, but only forward the RC values.
Definition at line 239 of file px4io.cpp.
Referenced by disable_rc_handling(), init(), and task_main().
|
private |
last valid timestamp
Definition at line 241 of file px4io.cpp.
Referenced by io_get_raw_rc_input(), and io_publish_raw_rc().
|
private |
RSSI PWM input channel.
Definition at line 286 of file px4io.cpp.
Referenced by init(), io_get_raw_rc_input(), and task_main().
|
private |
max RSSI input on PWM channel
Definition at line 287 of file px4io.cpp.
Referenced by init(), io_get_raw_rc_input(), and task_main().
|
private |
min RSSI input on PWM channel
Definition at line 288 of file px4io.cpp.
Referenced by init(), io_get_raw_rc_input(), and task_main().
|
private |
Various IO status flags.
Definition at line 253 of file px4io.cpp.
Referenced by dsm_bind_ioctl(), io_handle_status(), and io_publish_raw_rc().
|
private |
actuator controls group 3 topic
system armed control topic
Definition at line 264 of file px4io.cpp.
Referenced by io_set_arming_state(), and task_main().
|
private |
actuator controls group 0 topic
Definition at line 259 of file px4io.cpp.
Referenced by io_set_control_state(), and task_main().
|
private |
actuator controls group 1 topic
Definition at line 261 of file px4io.cpp.
Referenced by io_set_control_state().
|
private |
Definition at line 262 of file px4io.cpp.
Referenced by io_set_control_state().
|
private |
actuator controls group 2 topic
Definition at line 263 of file px4io.cpp.
Referenced by io_set_control_state().
|
private |
|
private |
vehicle control mode topic
Definition at line 265 of file px4io.cpp.
Referenced by io_set_arming_state(), and task_main().
|
private |
|
private |
worker terminate flag
Definition at line 244 of file px4io.cpp.
Referenced by task_main(), and ~PX4IO().
|
private |
To test what happens if IO loses FMU.
Definition at line 293 of file px4io.cpp.
Referenced by io_set_control_state(), ioctl(), and write().
|
private |
|
private |
Definition at line 272 of file px4io.cpp.
Referenced by io_publish_raw_rc().
|
private |
Definition at line 274 of file px4io.cpp.
Referenced by io_publish_pwm_outputs().
|
private |
Definition at line 273 of file px4io.cpp.
Referenced by io_publish_pwm_outputs().
|
private |
Definition at line 276 of file px4io.cpp.
Referenced by io_handle_status().
|
private |
Definition at line 275 of file px4io.cpp.
Referenced by io_handle_vservo().
|
private |
Subscription interval limiting send rate.
Definition at line 238 of file px4io.cpp.
Referenced by set_update_rate(), and task_main().