42 #include <px4_platform_common/px4_config.h> 50 #include <stm32_pwr.h> 168 [PX4IO_P_SETUP_VBATT_SCALE] = 10000,
188 #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ 189 PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ 190 PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ 191 PX4IO_P_SETUP_FEATURES_PWM_RSSI) 193 #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ 194 PX4IO_P_SETUP_ARMING_FMU_PREARMED | \ 195 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ 196 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ 197 PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ 198 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ 199 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ 200 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ 201 PX4IO_P_SETUP_ARMING_LOCKDOWN | \ 202 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ 203 PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ 204 PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) 205 #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) 206 #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) 227 #define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) 275 registers_set(uint8_t page, uint8_t offset,
const uint16_t *values,
unsigned num_values)
304 while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
420 bool all_disarmed_off =
true;
431 all_disarmed_off =
false;
435 all_disarmed_off =
false;
439 all_disarmed_off =
false;
447 if (all_disarmed_off) {
469 if ((offset + num_values) > 255) {
470 num_values = 255 - offset;
474 while (num_values--) {
540 #ifdef ENABLE_SBUS_OUT 669 dsm_bind(value & 0x0f, (value >> 4) & 0xF);
693 if (value > 650 && value < 2350) {
773 bool disabled =
false;
807 isr_debug(0,
"ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
810 }
else if (!disabled) {
844 registers_get(uint8_t page, uint8_t offset, uint16_t **values,
unsigned *num_values)
846 #define SELECT_PAGE(_page_name) \ 848 *values = (uint16_t *)&_page_name[0]; \ 849 *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \ 883 if (counts != 0xffff) {
884 unsigned mV = (166460 + (counts * 45934)) / 10000;
885 unsigned corrected = (mV *
r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
904 if (counts != 0xffff) {
914 if (counts != 0xffff) {
916 unsigned mV = counts * 9900 / 4096;
926 if (counts != 0xffff) {
928 unsigned mV = counts * 3300 / 4096;
1039 if (offset >= *num_values) {
1045 *num_values -= offset;
1056 for (
unsigned pass = 0; pass < 2; pass++) {
1067 uint32_t alt = map & mask;
1071 if ((alt != 0) && (alt != mask)) {
#define PX4IO_P_STATUS_VSERVO
#define PWM_DEFAULT_MAX
Default maximum PWM in us.
static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
#define PX4IO_PAGE_RAW_RC_INPUT
void schedule_reboot(uint32_t time_delta_usec)
schedule a reboot in time_delta_usec microseconds
#define PX4IO_PAGE_DISARMED_PWM
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE
#define PX4IO_P_SETUP_FEATURES_VALID
#define PX4IO_P_SETUP_TRIM_PITCH
Pitch trim, in actuator units.
#define PWM_LOWEST_MAX
Lowest PWM allowed as the maximum PWM.
#define PX4IO_P_CONFIG_CONTROL_COUNT
#define PX4IO_PAGE_ACTUATORS
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT
#define PX4IO_P_SETUP_FORCE_SAFETY_OFF
RC protocol definition for Spektrum RC.
#define PX4IO_P_SETUP_RELAYS_PAD
#define PX4IO_P_SETUP_SET_DEBUG
__EXPORT uint32_t up_pwm_servo_get_rate_group(unsigned group)
Get a bitmap of output channels assigned to a given rate group.
#define PX4IO_P_SETUP_PWM_DEFAULTRATE
uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT]
PAGE 107.
#define PX4IO_P_RAW_FRAME_COUNT
#define PX4IO_PAGE_RC_INPUT
#define PX4IO_P_SETUP_SCALE_PITCH
Pitch scale, in actuator units.
#define PX4IO_P_SETUP_PWM_REVERSE
Bitmask to reverse PWM channels 1-8.
#define PX4IO_PAGE_FAILSAFE_PWM
0..CONFIG_ACTUATOR_COUNT-1
#define PX4IO_P_SETUP_SBUS_RATE
frame rate of SBUS1 output in Hz
uint16_t r_page_scratch[32]
Scratch page; used for registers that are constructed as-read.
#define PX4IO_P_CONFIG_RC_INPUT_COUNT
int mixer_handle_text(const void *buffer, size_t length)
#define PX4IO_THERMAL_IGNORE
#define PX4IO_P_STATUS_PRSSI
#define PX4IO_P_RAW_RC_BASE
volatile uint16_t r_page_setup[]
PAGE 100.
#define PX4IO_P_CONFIG_RELAY_COUNT
int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
Register space.
#define PX4IO_P_SETUP_FORCE_SAFETY_ON
void isr_debug(uint8_t level, const char *fmt,...)
send a debug message to the console
#define PX4IO_PAGE_MIXERLOAD
#define PX4IO_P_SETUP_PWM_ALTRATE
static const uint16_t r_page_config[]
PAGE 0.
#define PX4IO_P_STATUS_CPULOAD
uint16_t r_page_servos[PX4IO_SERVO_COUNT]
PAGE 3.
#define PX4IO_P_SETUP_CRC
#define PX4IO_RC_INPUT_CHANNELS
#define ENABLE_SBUS_OUT(_s)
#define r_setup_pwm_defaultrate
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR
PX4IO interface protocol.
#define PX4IO_P_SETUP_TRIM_YAW
Yaw trim, in actuator units.
High-resolution timer with callouts and timekeeping.
#define PX4IO_P_STATUS_MIXER
uint16_t r_page_direct_pwm[PX4IO_SERVO_COUNT]
PAGE 8.
#define PX4IO_RC_MAPPED_CONTROL_CHANNELS
This is the maximum number of channels mapped/used.
#define PX4IO_P_STATUS_FLAGS
#define PX4IO_P_RC_CONFIG_MAX
highest input value
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM
int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
#define PX4IO_P_SETUP_PWM_RATES
#define PX4IO_PAGE_CONTROL_MAX_PWM
0..CONFIG_ACTUATOR_COUNT-1
#define PX4IO_P_RAW_LOST_FRAME_COUNT
#define PX4IO_P_CONFIG_MAX_TRANSFER
#define SELECT_PAGE(_page_name)
#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION
flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set
#define PX4IO_P_SETUP_REBOOT_BL
#define PX4IO_P_RAW_RC_FLAGS
uint16_t r_page_raw_rc_input[]
PAGE 4.
#define PX4IO_P_SETUP_THR_MDL_FAC
factor for modelling motor control signal output to static thrust relationship
#define PX4IO_PAGE_CONTROL_MIN_PWM
0..CONFIG_ACTUATOR_COUNT-1
#define PX4IO_P_SETUP_DSM
#define PX4IO_P_SETUP_FEATURES
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED
uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT]
PAGE 106.
#define PX4IO_PAGE_SERVOS
#define PX4IO_P_RAW_RC_COUNT
__EXPORT int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
Set the update rate for a given rate group.
#define PX4IO_P_STATUS_VRSSI
#define PX4IO_P_STATUS_FLAGS_MIXER_OK
#define PX4IO_P_CONFIG_PROTOCOL_VERSION
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC
#define PX4IO_P_SETUP_MOTOR_SLEW_MAX
max motor slew rate
#define PX4IO_P_SETUP_VSERVO_SCALE
#define PX4IO_P_RC_CONFIG_DEADZONE
band around center that is ignored
uint16_t r_page_actuators[PX4IO_SERVO_COUNT]
PAGE 2.
#define PX4IO_PAGE_PWM_INFO
#define PX4IO_CONTROL_CHANNELS
#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH
magic value for mode switch
#define PX4IO_P_CONFIG_ACTUATOR_COUNT
#define PX4IO_PAGE_RC_CONFIG
R/C input configuration.
#define PX4IO_P_TEST_LED
set the amber LED on/off
#define PX4IO_P_STATUS_FREEMEM
#define PX4IO_MAX_TRANSFER_LEN
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF
#define PX4IO_ADC_CHANNEL_COUNT
#define PX4IO_FORCE_SAFETY_MAGIC
#define PX4IO_P_SETUP_TRIM_ROLL
Roll trim, in actuator units.
void mixer_set_failsafe()
#define PX4IO_P_STATUS_ALARMS
#define PX4IO_PAGE_STATUS
#define PX4IO_P_RC_CONFIG_ASSIGNMENT
mapped input value
General defines and structures for the PX4IO module firmware.
struct sys_state_s system_state
#define PX4IO_P_RC_CONFIG_STRIDE
spacing between channel config data
#define PX4IO_PAGE_CONTROL_TRIM_PWM
0..CONFIG_ACTUATOR_COUNT-1
#define PX4IO_PAGE_DIRECT_PWM
0..CONFIG_ACTUATOR_COUNT-1
#define PX4IO_P_RC_CONFIG_CENTER
center input value
#define PX4IO_RELAY_CHANNELS
#define PX4IO_REBOOT_BL_MAGIC
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED
#define PX4IO_SERVO_COUNT
#define PWM_IGNORE_THIS_CHANNEL
Do not output a channel with this value.
#define PX4IO_P_STATUS_FLAGS_RAW_PWM
#define PX4IO_P_SETUP_THERMAL
thermal management
#define PX4IO_P_SETUP_ARMING_VALID
#define PX4IO_P_STATUS_FLAGS_INIT_OK
#define PX4IO_PROTOCOL_VERSION
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US
the throttle failsafe pulse length in microseconds
static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
#define PWM_HIGHEST_MIN
Highest PWM allowed as the minimum PWM.
RC protocol definition for S.BUS.
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID
#define PX4IO_P_SETUP_ARMING
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI
enable PWM RSSI parsing
bool update_mc_thrust_param
void sbus1_set_output_rate_hz(uint16_t rate_hz)
#define PX4IO_PAGE_CONFIG
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI
enable ADC RSSI parsing
#define PX4IO_P_SETUP_SCALE_ROLL
Roll scale, in actuator units.
#define PX4IO_P_SETUP_SCALE_YAW
Yaw scale, in actuator units.
uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS *PX4IO_P_RC_CONFIG_STRIDE]
PAGE 103.
#define PX4IO_P_RAW_RC_NRSSI
#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE
volatile uint16_t r_page_status[]
PAGE 1.
#define PX4IO_PAGE_CONTROLS
actuator control groups, one after the other, 8 wide
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT]
PAGE 105.
#define PX4IO_RATE_MAP_BASE
#define PX4IO_P_SETUP_RATES_VALID
#define PWM_HIGHEST_MAX
Highest maximum PWM in us.
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT
enable S.Bus v2 output
#define PX4IO_P_RAW_RC_DATA
#define PWM_DEFAULT_TRIM
Default trim PWM in us.
uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT]
PAGE 109.
#define r_setup_pwm_rates
#define r_setup_pwm_altrate
int16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT]
PAGE 108.
#define PX4IO_P_RC_CONFIG_MIN
lowest input value
#define PX4IO_P_RC_CONFIG_OPTIONS
channel options bitmask
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION
#define PWM_DEFAULT_MIN
Default minimum PWM in us.
#define PX4IO_CONTROL_GROUPS
__EXPORT void up_pwm_update(void)
Trigger all timer's channels in Oneshot mode to fire the oneshot with updated values.
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT
enable S.Bus v1 output
uint16_t r_page_rc_input[]
PAGE 5.
#define PX4IO_P_SETUP_AIRMODE
air-mode
#define PX4IO_PAGE_RAW_ADC_INPUT
#define PX4IO_P_CONFIG_HARDWARE_VERSION
uint16_t adc_measure(unsigned channel)
volatile uint64_t fmu_data_received_time
Last FMU receive time, in microseconds since system boot.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint16_t r_page_controls[PX4IO_CONTROL_GROUPS *PX4IO_CONTROL_CHANNELS]
PAGE 101.
#define PWM_LOWEST_MIN
Lowest minimum PWM in us.
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE