mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Merge pull request #548 from PX4/control_groups
Added support for a total of four control groups to the IO driver
This commit is contained in:
Lorenz Meier
commit
4e3e309a8a
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
||||
@ -228,33 +228,36 @@ private:
|
||||
device::Device *_interface;
|
||||
|
||||
// XXX
|
||||
unsigned _hardware; ///< Hardware revision
|
||||
unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO
|
||||
unsigned _max_controls; ///<Maximum # of controls supported by PX4IO
|
||||
unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO
|
||||
unsigned _max_relays; ///<Maximum relays supported by PX4IO
|
||||
unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO
|
||||
unsigned _hardware; ///< Hardware revision
|
||||
unsigned _max_actuators; ///< Maximum # of actuators supported by PX4IO
|
||||
unsigned _max_controls; ///< Maximum # of controls supported by PX4IO
|
||||
unsigned _max_rc_input; ///< Maximum receiver channels supported by PX4IO
|
||||
unsigned _max_relays; ///< Maximum relays supported by PX4IO
|
||||
unsigned _max_transfer; ///< Maximum number of I2C transfers supported by PX4IO
|
||||
|
||||
unsigned _update_interval; ///< Subscription interval limiting send rate
|
||||
bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values
|
||||
unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels
|
||||
|
||||
volatile int _task; ///<worker task id
|
||||
volatile bool _task_should_exit; ///<worker terminate flag
|
||||
volatile int _task; ///< worker task id
|
||||
volatile bool _task_should_exit; ///< worker terminate flag
|
||||
|
||||
int _mavlink_fd; ///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
|
||||
int _thread_mavlink_fd; ///<mavlink file descriptor for thread.
|
||||
int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
|
||||
int _thread_mavlink_fd; ///< mavlink file descriptor for thread.
|
||||
|
||||
perf_counter_t _perf_update; ///<local performance counter for status updates
|
||||
perf_counter_t _perf_write; ///<local performance counter for PWM control writes
|
||||
perf_counter_t _perf_chan_count; ///<local performance counter for channel number changes
|
||||
|
||||
/* cached IO state */
|
||||
uint16_t _status; ///<Various IO status flags
|
||||
uint16_t _alarms; ///<Various IO alarms
|
||||
uint16_t _status; ///< Various IO status flags
|
||||
uint16_t _alarms; ///< Various IO alarms
|
||||
|
||||
/* subscribed topics */
|
||||
int _t_actuators; ///< actuator controls topic
|
||||
int _t_actuator_controls_0; ///< actuator controls group 0 topic
|
||||
int _t_actuator_controls_1; ///< actuator controls group 1 topic
|
||||
int _t_actuator_controls_2; ///< actuator controls group 2 topic
|
||||
int _t_actuator_controls_3; ///< actuator controls group 3 topic
|
||||
int _t_actuator_armed; ///< system armed control topic
|
||||
int _t_vehicle_control_mode;///< vehicle control mode topic
|
||||
int _t_param; ///< parameter update topic
|
||||
@ -269,15 +272,15 @@ private:
|
||||
|
||||
actuator_outputs_s _outputs; ///<mixed outputs
|
||||
|
||||
bool _primary_pwm_device; ///<true if we are the default PWM output
|
||||
bool _primary_pwm_device; ///< true if we are the default PWM output
|
||||
|
||||
float _battery_amp_per_volt; ///<current sensor amps/volt
|
||||
float _battery_amp_bias; ///<current sensor bias
|
||||
float _battery_mamphour_total;///<amp hours consumed so far
|
||||
uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp
|
||||
float _battery_amp_per_volt; ///< current sensor amps/volt
|
||||
float _battery_amp_bias; ///< current sensor bias
|
||||
float _battery_mamphour_total;///< amp hours consumed so far
|
||||
uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power
|
||||
bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power
|
||||
#endif
|
||||
|
||||
/**
|
||||
@ -291,9 +294,14 @@ private:
|
||||
void task_main();
|
||||
|
||||
/**
|
||||
* Send controls to IO
|
||||
* Send controls for one group to IO
|
||||
*/
|
||||
int io_set_control_state();
|
||||
int io_set_control_state(unsigned group);
|
||||
|
||||
/**
|
||||
* Send all controls to IO
|
||||
*/
|
||||
int io_set_control_groups();
|
||||
|
||||
/**
|
||||
* Update IO's arming-related state
|
||||
@ -467,7 +475,10 @@ PX4IO::PX4IO(device::Device *interface) :
|
||||
_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
|
||||
_status(0),
|
||||
_alarms(0),
|
||||
_t_actuators(-1),
|
||||
_t_actuator_controls_0(-1),
|
||||
_t_actuator_controls_1(-1),
|
||||
_t_actuator_controls_2(-1),
|
||||
_t_actuator_controls_3(-1),
|
||||
_t_actuator_armed(-1),
|
||||
_t_vehicle_control_mode(-1),
|
||||
_t_param(-1),
|
||||
@ -769,15 +780,20 @@ PX4IO::task_main()
|
||||
* Subscribe to the appropriate PWM output topic based on whether we are the
|
||||
* primary PWM output or not.
|
||||
*/
|
||||
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
|
||||
ORB_ID(actuator_controls_1));
|
||||
orb_set_interval(_t_actuators, 20); /* default to 50Hz */
|
||||
_t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0));
|
||||
orb_set_interval(_t_actuator_controls_0, 20); /* default to 50Hz */
|
||||
_t_actuator_controls_1 = orb_subscribe(ORB_ID(actuator_controls_1));
|
||||
orb_set_interval(_t_actuator_controls_1, 33); /* default to 30Hz */
|
||||
_t_actuator_controls_2 = orb_subscribe(ORB_ID(actuator_controls_2));
|
||||
orb_set_interval(_t_actuator_controls_2, 33); /* default to 30Hz */
|
||||
_t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3));
|
||||
orb_set_interval(_t_actuator_controls_3, 33); /* default to 30Hz */
|
||||
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
|
||||
_t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
_t_param = orb_subscribe(ORB_ID(parameter_update));
|
||||
_t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
|
||||
|
||||
if ((_t_actuators < 0) ||
|
||||
if ((_t_actuator_controls_0 < 0) ||
|
||||
(_t_actuator_armed < 0) ||
|
||||
(_t_vehicle_control_mode < 0) ||
|
||||
(_t_param < 0) ||
|
||||
@ -788,7 +804,7 @@ PX4IO::task_main()
|
||||
|
||||
/* poll descriptor */
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _t_actuators;
|
||||
fds[0].fd = _t_actuator_controls_0;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
log("ready");
|
||||
@ -807,7 +823,11 @@ PX4IO::task_main()
|
||||
if (_update_interval > 100)
|
||||
_update_interval = 100;
|
||||
|
||||
orb_set_interval(_t_actuators, _update_interval);
|
||||
orb_set_interval(_t_actuator_controls_0, _update_interval);
|
||||
/*
|
||||
* NOT changing the rate of groups 1-3 here, because only attitude
|
||||
* really needs to run fast.
|
||||
*/
|
||||
_update_interval = 0;
|
||||
}
|
||||
|
||||
@ -827,7 +847,10 @@ PX4IO::task_main()
|
||||
|
||||
/* if we have new control data from the ORB, handle it */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
io_set_control_state();
|
||||
|
||||
/* we're not nice to the lower-priority control groups and only check them
|
||||
when the primary group updated (which is now). */
|
||||
(void)io_set_control_groups();
|
||||
}
|
||||
|
||||
if (now >= poll_last + IO_POLL_INTERVAL) {
|
||||
@ -870,7 +893,7 @@ PX4IO::task_main()
|
||||
orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
|
||||
|
||||
// Check for a DSM pairing command
|
||||
if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) {
|
||||
if (((int)cmd.command == VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) {
|
||||
dsm_bind_ioctl((int)cmd.param2);
|
||||
}
|
||||
}
|
||||
@ -937,20 +960,74 @@ out:
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO::io_set_control_state()
|
||||
PX4IO::io_set_control_groups()
|
||||
{
|
||||
int ret = io_set_control_state(0);
|
||||
|
||||
/* send auxiliary control groups */
|
||||
(void)io_set_control_state(1);
|
||||
(void)io_set_control_state(2);
|
||||
(void)io_set_control_state(3);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO::io_set_control_state(unsigned group)
|
||||
{
|
||||
actuator_controls_s controls; ///< actuator outputs
|
||||
uint16_t regs[_max_actuators];
|
||||
|
||||
/* get controls */
|
||||
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
|
||||
ORB_ID(actuator_controls_1), _t_actuators, &controls);
|
||||
bool changed;
|
||||
|
||||
switch (group) {
|
||||
case 0:
|
||||
{
|
||||
orb_check(_t_actuator_controls_0, &changed);
|
||||
|
||||
if (changed) {
|
||||
orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
{
|
||||
orb_check(_t_actuator_controls_1, &changed);
|
||||
|
||||
if (changed) {
|
||||
orb_copy(ORB_ID(actuator_controls_1), _t_actuator_controls_1, &controls);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
{
|
||||
orb_check(_t_actuator_controls_2, &changed);
|
||||
|
||||
if (changed) {
|
||||
orb_copy(ORB_ID(actuator_controls_2), _t_actuator_controls_2, &controls);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
{
|
||||
orb_check(_t_actuator_controls_3, &changed);
|
||||
|
||||
if (changed) {
|
||||
orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3, &controls);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
if (!changed)
|
||||
return -1;
|
||||
|
||||
for (unsigned i = 0; i < _max_controls; i++)
|
||||
regs[i] = FLOAT_TO_REG(controls.control[i]);
|
||||
|
||||
/* copy values to registers in IO */
|
||||
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
|
||||
return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
|
||||
}
|
||||
|
||||
|
||||
@ -1018,8 +1095,10 @@ PX4IO::io_set_rc_config()
|
||||
* assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical
|
||||
* controls.
|
||||
*/
|
||||
|
||||
/* fill the mapping with an error condition triggering value */
|
||||
for (unsigned i = 0; i < _max_rc_input; i++)
|
||||
input_map[i] = -1;
|
||||
input_map[i] = UINT8_MAX;
|
||||
|
||||
/*
|
||||
* NOTE: The indices for mapped channels are 1-based
|
||||
@ -1051,12 +1130,6 @@ PX4IO::io_set_rc_config()
|
||||
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
|
||||
input_map[ichan - 1] = 4;
|
||||
|
||||
ichan = 5;
|
||||
|
||||
for (unsigned i = 0; i < _max_rc_input; i++)
|
||||
if (input_map[i] == -1)
|
||||
input_map[i] = ichan++;
|
||||
|
||||
/*
|
||||
* Iterate all possible RC inputs.
|
||||
*/
|
||||
|
||||
@ -499,8 +499,8 @@ l_actuator_outputs(const struct listener *l)
|
||||
act_outputs.output[6],
|
||||
act_outputs.output[7]);
|
||||
|
||||
/* only send in HIL mode */
|
||||
if (mavlink_hil_enabled && armed.armed) {
|
||||
/* only send in HIL mode and only send first group for HIL */
|
||||
if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
|
||||
|
||||
/* translate the current syste state to mavlink state and mode */
|
||||
uint8_t mavlink_state = 0;
|
||||
|
||||
@ -66,7 +66,7 @@ controls_init(void)
|
||||
sbus_init("/dev/ttyS2");
|
||||
|
||||
/* default to a 1:1 input map, all enabled */
|
||||
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
|
||||
for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
|
||||
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
|
||||
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
|
||||
@ -113,7 +113,7 @@ controls_tick() {
|
||||
perf_end(c_gather_dsm);
|
||||
|
||||
perf_begin(c_gather_sbus);
|
||||
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */);
|
||||
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
|
||||
if (sbus_updated) {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
|
||||
}
|
||||
@ -136,8 +136,8 @@ controls_tick() {
|
||||
perf_end(c_gather_ppm);
|
||||
|
||||
/* limit number of channels to allowable data size */
|
||||
if (r_raw_rc_count > PX4IO_INPUT_CHANNELS)
|
||||
r_raw_rc_count = PX4IO_INPUT_CHANNELS;
|
||||
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
|
||||
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
|
||||
|
||||
/*
|
||||
* In some cases we may have received a frame, but input has still
|
||||
@ -210,14 +210,16 @@ controls_tick() {
|
||||
|
||||
/* and update the scaled/mapped version */
|
||||
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
|
||||
ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
|
||||
if (mapped < PX4IO_CONTROL_CHANNELS) {
|
||||
|
||||
/* invert channel if pitch - pulling the lever down means pitching up by convention */
|
||||
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
|
||||
scaled = -scaled;
|
||||
/* invert channel if pitch - pulling the lever down means pitching up by convention */
|
||||
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
|
||||
scaled = -scaled;
|
||||
|
||||
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
|
||||
assigned_channels |= (1 << mapped);
|
||||
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
|
||||
assigned_channels |= (1 << mapped);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -334,8 +336,8 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
|
||||
|
||||
/* PPM data exists, copy it */
|
||||
*num_values = ppm_decoded_channels;
|
||||
if (*num_values > PX4IO_CONTROL_CHANNELS)
|
||||
*num_values = PX4IO_CONTROL_CHANNELS;
|
||||
if (*num_values > PX4IO_RC_INPUT_CHANNELS)
|
||||
*num_values = PX4IO_RC_INPUT_CHANNELS;
|
||||
|
||||
for (unsigned i = 0; i < *num_values; i++)
|
||||
values[i] = ppm_buffer[i];
|
||||
|
||||
@ -355,7 +355,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
continue;
|
||||
|
||||
/* ignore channels out of range */
|
||||
if (channel >= PX4IO_INPUT_CHANNELS)
|
||||
if (channel >= PX4IO_RC_INPUT_CHANNELS)
|
||||
continue;
|
||||
|
||||
/* update the decoded channel count */
|
||||
|
||||
@ -77,7 +77,8 @@ enum mixer_source {
|
||||
MIX_NONE,
|
||||
MIX_FMU,
|
||||
MIX_OVERRIDE,
|
||||
MIX_FAILSAFE
|
||||
MIX_FAILSAFE,
|
||||
MIX_OVERRIDE_FMU_OK
|
||||
};
|
||||
static mixer_source source;
|
||||
|
||||
@ -135,10 +136,19 @@ mixer_tick(void)
|
||||
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
|
||||
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
|
||||
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
|
||||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
|
||||
|
||||
/* if allowed, mix from RC inputs directly */
|
||||
source = MIX_OVERRIDE;
|
||||
} else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
|
||||
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
|
||||
|
||||
/* if allowed, mix from RC inputs directly up to available rc channels */
|
||||
source = MIX_OVERRIDE_FMU_OK;
|
||||
}
|
||||
}
|
||||
|
||||
@ -240,24 +250,35 @@ mixer_callback(uintptr_t handle,
|
||||
uint8_t control_index,
|
||||
float &control)
|
||||
{
|
||||
if (control_group != 0)
|
||||
if (control_group > 3)
|
||||
return -1;
|
||||
|
||||
switch (source) {
|
||||
case MIX_FMU:
|
||||
if (control_index < PX4IO_CONTROL_CHANNELS) {
|
||||
control = REG_TO_FLOAT(r_page_controls[control_index]);
|
||||
if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) {
|
||||
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
|
||||
break;
|
||||
}
|
||||
return -1;
|
||||
|
||||
case MIX_OVERRIDE:
|
||||
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) {
|
||||
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
|
||||
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
|
||||
break;
|
||||
}
|
||||
return -1;
|
||||
|
||||
case MIX_OVERRIDE_FMU_OK:
|
||||
/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
|
||||
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
|
||||
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
|
||||
break;
|
||||
} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
|
||||
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
|
||||
break;
|
||||
}
|
||||
return -1;
|
||||
|
||||
case MIX_FAILSAFE:
|
||||
case MIX_NONE:
|
||||
control = 0.0f;
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -63,7 +63,7 @@
|
||||
* readable pages to be densely packed. Page numbers do not need to be
|
||||
* packed.
|
||||
*
|
||||
* Definitions marked 1 are only valid on PX4IOv1 boards. Likewise,
|
||||
* Definitions marked [1] are only valid on PX4IOv1 boards. Likewise,
|
||||
* [2] denotes definitions specific to the PX4IOv2 board.
|
||||
*/
|
||||
|
||||
@ -76,6 +76,9 @@
|
||||
|
||||
#define PX4IO_PROTOCOL_VERSION 4
|
||||
|
||||
/* maximum allowable sizes on this protocol version */
|
||||
#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
|
||||
|
||||
/* static configuration page */
|
||||
#define PX4IO_PAGE_CONFIG 0
|
||||
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */
|
||||
@ -87,6 +90,7 @@
|
||||
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
|
||||
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
|
||||
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */
|
||||
#define PX4IO_P_CONFIG_CONTROL_GROUP_COUNT 8 /**< hardcoded # of control groups*/
|
||||
|
||||
/* dynamic status page */
|
||||
#define PX4IO_PAGE_STATUS 1
|
||||
@ -186,7 +190,7 @@ enum { /* DSM bind states */
|
||||
dsm_bind_reinit_uart
|
||||
};
|
||||
/* 8 */
|
||||
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
|
||||
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
|
||||
|
||||
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
|
||||
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
|
||||
@ -194,41 +198,51 @@ enum { /* DSM bind states */
|
||||
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
|
||||
|
||||
/* autopilot control values, -10000..10000 */
|
||||
#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */
|
||||
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
|
||||
#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
|
||||
#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
|
||||
#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
|
||||
#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
|
||||
|
||||
#define PX4IO_P_CONTROLS_GROUP_VALID 64
|
||||
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */
|
||||
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */
|
||||
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */
|
||||
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */
|
||||
|
||||
/* raw text load to the mixer parser - ignores offset */
|
||||
#define PX4IO_PAGE_MIXERLOAD 52
|
||||
#define PX4IO_PAGE_MIXERLOAD 52
|
||||
|
||||
/* R/C channel config */
|
||||
#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */
|
||||
#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */
|
||||
#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */
|
||||
#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */
|
||||
#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */
|
||||
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */
|
||||
#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */
|
||||
#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */
|
||||
#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */
|
||||
#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */
|
||||
#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */
|
||||
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0)
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
|
||||
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
|
||||
#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */
|
||||
|
||||
/* PWM output - overrides mixer */
|
||||
#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM failsafe values - zero disables the output */
|
||||
#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* Debug and test page - not used in normal operation */
|
||||
#define PX4IO_PAGE_TEST 127
|
||||
#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */
|
||||
#define PX4IO_PAGE_TEST 127
|
||||
#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */
|
||||
|
||||
/* PWM minimum values for certain ESCs */
|
||||
#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM maximum values for certain ESCs */
|
||||
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM disarmed values that are active, even when SAFETY_SAFE */
|
||||
#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/**
|
||||
* As-needed mixer data upload.
|
||||
|
||||
@ -53,7 +53,9 @@
|
||||
*/
|
||||
#define PX4IO_SERVO_COUNT 8
|
||||
#define PX4IO_CONTROL_CHANNELS 8
|
||||
#define PX4IO_INPUT_CHANNELS 8 // XXX this should be 18 channels
|
||||
#define PX4IO_CONTROL_GROUPS 2
|
||||
#define PX4IO_RC_INPUT_CHANNELS 18
|
||||
#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */
|
||||
|
||||
/*
|
||||
* Debug logging
|
||||
@ -169,6 +171,8 @@ extern pwm_limit_t pwm_limit;
|
||||
|
||||
#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY)
|
||||
|
||||
#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel)
|
||||
|
||||
/*
|
||||
* Mixer
|
||||
*/
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -70,7 +70,7 @@ static const uint16_t r_page_config[] = {
|
||||
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
|
||||
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
|
||||
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
|
||||
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS,
|
||||
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS,
|
||||
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
|
||||
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
|
||||
};
|
||||
@ -116,7 +116,7 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT];
|
||||
uint16_t r_page_raw_rc_input[] =
|
||||
{
|
||||
[PX4IO_P_RAW_RC_COUNT] = 0,
|
||||
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon
|
||||
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
@ -126,7 +126,7 @@ uint16_t r_page_raw_rc_input[] =
|
||||
*/
|
||||
uint16_t r_page_rc_input[] = {
|
||||
[PX4IO_P_RC_VALID] = 0,
|
||||
[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
|
||||
[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
@ -178,7 +178,7 @@ volatile uint16_t r_page_setup[] =
|
||||
*
|
||||
* Control values from the FMU.
|
||||
*/
|
||||
volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
|
||||
volatile uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
|
||||
|
||||
/*
|
||||
* PAGE 102 does not have a buffer.
|
||||
@ -189,7 +189,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
|
||||
*
|
||||
* R/C channel input configuration.
|
||||
*/
|
||||
uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
/* valid options */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
|
||||
@ -241,7 +241,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
||||
case PX4IO_PAGE_CONTROLS:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
|
||||
while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
|
||||
|
||||
/* XXX range-check value? */
|
||||
r_page_controls[offset] = *values;
|
||||
@ -554,7 +554,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
||||
unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
|
||||
uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
if (channel >= PX4IO_CONTROL_CHANNELS)
|
||||
if (channel >= PX4IO_RC_INPUT_CHANNELS)
|
||||
return -1;
|
||||
|
||||
/* disable the channel until we have a chance to sanity-check it */
|
||||
@ -584,6 +584,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
||||
/* this option is normally set last */
|
||||
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
|
||||
uint8_t count = 0;
|
||||
bool disabled = false;
|
||||
|
||||
/* assert min..center..max ordering */
|
||||
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
|
||||
@ -602,7 +603,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
||||
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
|
||||
count++;
|
||||
}
|
||||
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
|
||||
disabled = true;
|
||||
} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
|
||||
count++;
|
||||
}
|
||||
|
||||
@ -610,7 +614,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
||||
if (count) {
|
||||
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
|
||||
} else {
|
||||
} else if (!disabled) {
|
||||
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
}
|
||||
}
|
||||
|
||||
@ -252,7 +252,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
|
||||
}
|
||||
|
||||
/* decode switch channels if data fields are wide enough */
|
||||
if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) {
|
||||
if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) {
|
||||
chancount = 18;
|
||||
|
||||
/* channel 17 (index 16) */
|
||||
|
||||
@ -190,6 +190,24 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC16_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC16_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC16_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC16_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC16_DZ, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC17_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC17_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC17_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC17_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC17_DZ, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC18_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC18_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC18_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC18_REV, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f);
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
|
||||
#endif
|
||||
|
||||
@ -165,7 +165,7 @@ public:
|
||||
int start();
|
||||
|
||||
private:
|
||||
static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */
|
||||
static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */
|
||||
|
||||
hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */
|
||||
|
||||
@ -602,7 +602,7 @@ Sensors::parameters_update()
|
||||
float tmpRevFactor = 0.0f;
|
||||
|
||||
/* rc values */
|
||||
for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
|
||||
for (unsigned int i = 0; i < _rc_max_chan_count; i++) {
|
||||
|
||||
param_get(_parameter_handles.min[i], &(_parameters.min[i]));
|
||||
param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
|
||||
|
||||
@ -45,7 +45,7 @@
|
||||
#include <systemlib/rc_check.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
|
||||
int rc_calibration_check(int mavlink_fd) {
|
||||
|
||||
@ -66,7 +66,7 @@ int rc_calibration_check(int mavlink_fd) {
|
||||
|
||||
int channel_fail_count = 0;
|
||||
|
||||
for (int i = 0; i < RC_CHANNELS_MAX; i++) {
|
||||
for (int i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
|
||||
/* should the channel be enabled? */
|
||||
uint8_t count = 0;
|
||||
|
||||
|
||||
@ -1,9 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Nils Wenzler <wenzlern@student.ethz.ch>
|
||||
* @author Ivan Ovinnikov <oivan@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -47,13 +44,13 @@
|
||||
|
||||
/**
|
||||
* The number of RC channel inputs supported.
|
||||
* Current (Q1/2013) radios support up to 18 channels,
|
||||
* Current (Q4/2013) radios support up to 18 channels,
|
||||
* leaving at a sane value of 15.
|
||||
* This number can be greater then number of RC channels,
|
||||
* because single RC channel can be mapped to multiple
|
||||
* functions, e.g. for various mode switches.
|
||||
*/
|
||||
#define RC_CHANNELS_MAX 15
|
||||
#define RC_CHANNELS_MAPPED_MAX 15
|
||||
|
||||
/**
|
||||
* This defines the mapping of the RC functions.
|
||||
@ -91,7 +88,7 @@ struct rc_channels_s {
|
||||
uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */
|
||||
struct {
|
||||
float scaled; /**< Scaled to -1..1 (throttle: 0..1) */
|
||||
} chan[RC_CHANNELS_MAX];
|
||||
} chan[RC_CHANNELS_MAPPED_MAX];
|
||||
uint8_t chan_count; /**< number of valid channels */
|
||||
|
||||
/*String array to store the names of the functions*/
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user