fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Enabled manual override switch, work in progress. Added initial demix testing code to support delta mixing on the remote for convenient manual override

Browse Source
This commit is contained in:
Lorenz Meier 2012-12-01 16:30:21 +01:00
parent 2bfb672791
commit 126e6ac207
8 changed files with 113 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
apps/px4io/comms.c
View File

@ -177,13 +177,13 @@ comms_handle_command(const void *buffer, size_t length)
for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
system_state.fmu_channel_data[i] = cmd->servo_command[i];
/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
/* if the IO is armed and FMU gets disarmed, IO must also disarm */
if(system_state.arm_ok && !cmd->arm_ok) {
system_state.armed = false;
}
system_state.arm_ok = cmd->arm_ok;
system_state.mixer_use_fmu = true;
system_state.mixer_fmu_available = true;
system_state.fmu_data_received = true;

21
apps/px4io/controls.c
View File

@ -59,6 +59,10 @@
#define DEBUG
#include "px4io.h"
#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */
#define RC_CHANNEL_HIGH_THRESH 1600
#define RC_CHANNEL_LOW_THRESH 1400
void
controls_main(void)
{
@ -80,7 +84,22 @@ controls_main(void)
if (fds[1].revents & POLLIN)
sbus_input();
/* XXX do ppm processing, bypass mode, etc. here */
/* force manual input override */
if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
system_state.mixer_use_fmu = false;
} else {
/* override not engaged, use FMU */
system_state.mixer_use_fmu = true;
}
/* detect rc loss event */
if (hrt_absolute_time() - system_state.rc_channels_timestamp > RC_FAILSAFE_TIMEOUT) {
system_state.rc_lost = true;
}
/* XXX detect fmu loss event */
/* XXX handle failsave events - RC loss and FMU loss - here */
/* do PWM output updates */
mixer_tick();

5
apps/px4io/mixer.c
View File

@ -97,7 +97,7 @@ mixer_tick(void)
/*
* Decide which set of inputs we're using.
*/
if (system_state.mixer_use_fmu) {
if (system_state.mixer_use_fmu && system_state.mixer_fmu_available) {
/* we have recent control data from the FMU */
control_count = PX4IO_OUTPUT_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
@ -141,9 +141,10 @@ mixer_tick(void)
/*
* Decide whether the servos should be armed right now.
* A sufficient reason is armed state and either FMU or RC control inputs
*/
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu;
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);

31
apps/px4io/px4io.h
View File

@ -76,43 +76,58 @@ struct sys_state_s
bool dsm_input_ok; /* valid Spektrum DSM data */
bool sbus_input_ok; /* valid Futaba S.Bus data */
/*
/**
* Data from the remote control input(s)
*/
int rc_channels;
uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS];
uint64_t rc_channels_timestamp;
/*
/**
* Control signals from FMU.
*/
uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS];
/*
/**
* Relay controls
*/
bool relays[PX4IO_RELAY_CHANNELS];
/*
* If true, we are using the FMU controls.
/**
* If true, we are using the FMU controls, else RC input if available.
*/
bool mixer_use_fmu;
/*
/**
* If true, FMU input is available.
*/
bool mixer_fmu_available;
/**
* If true, state that should be reported to FMU has been updated.
*/
bool fmu_report_due;
/*
/**
* If true, new control data from the FMU has been received.
*/
bool fmu_data_received;
/*
/**
* Current serial interface mode, per the serial_rx_mode parameter
* in the config packet.
*/
uint8_t serial_rx_mode;
/**
* If true, the RC signal has been lost for more than a timeout interval
*/
bool rc_lost;
/**
* If true, the connection to FMU has been lost for more than a timeout interval
*/
bool fmu_lost;
};
extern struct sys_state_s system_state;

2
apps/px4io/safety.c
View File

@ -58,7 +58,7 @@ static struct hrt_call failsafe_call;
* Count the number of times in a row that we see the arming button
* held down.
*/
static unsigned counter;
static unsigned counter = 0;
#define ARM_COUNTER_THRESHOLD 10
#define DISARM_COUNTER_THRESHOLD 2

2
apps/sensors/sensor_params.c
View File

@ -122,6 +122,8 @@ PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA
PARAM_DEFINE_INT32(RC_DEMIX, 0); /**< 0 = off, 1 = auto, 2 = delta */
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));

86
apps/sensors/sensors.cpp
View File

@ -95,6 +95,12 @@
#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */
enum RC_DEMIX {
RC_DEMIX_NONE = 0,
RC_DEMIX_AUTO = 1,
RC_DEMIX_DELTA = 2
};
/**
* Sensor app start / stop handling function
*
@ -178,6 +184,8 @@ private:
int rc_type;
int rc_demix; /**< enabled de-mixing of RC mixers */
int rc_map_roll;
int rc_map_pitch;
int rc_map_yaw;
@ -204,6 +212,8 @@ private:
param_t ex[_rc_max_chan_count];
param_t rc_type;
param_t rc_demix;
param_t gyro_offset[3];
param_t accel_offset[3];
param_t accel_scale[3];
@ -392,6 +402,8 @@ Sensors::Sensors() :
_parameter_handles.rc_type = param_find("RC_TYPE");
_parameter_handles.rc_demix = param_find("RC_DEMIX");
_parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL");
_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
_parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW");
@ -496,21 +508,16 @@ Sensors::parameters_update()
}
/* update RC function mappings */
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
_rc.function[ROLL] = _parameters.rc_map_roll - 1;
_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[YAW] = _parameters.rc_map_yaw - 1;
_rc.function[OVERRIDE] = _parameters.rc_map_mode_sw - 1;
_rc.function[FUNC_0] = _parameters.rc_map_aux1 - 1;
_rc.function[FUNC_1] = _parameters.rc_map_aux2 - 1;
_rc.function[FUNC_2] = _parameters.rc_map_aux3 - 1;
/* remote control type */
if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) {
warnx("Failed getting remote control type");
}
/* de-mixing */
if (param_get(_parameter_handles.rc_demix, &(_parameters.rc_demix)) != OK) {
warnx("Failed getting demix setting");
}
/* channel mapping */
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
warnx("Failed getting roll chan index");
@ -547,6 +554,16 @@ Sensors::parameters_update()
warnx("Failed getting rc scaling for yaw");
}
/* update RC function mappings */
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
_rc.function[ROLL] = _parameters.rc_map_roll - 1;
_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
_rc.function[YAW] = _parameters.rc_map_yaw - 1;
_rc.function[OVERRIDE] = _parameters.rc_map_mode_sw - 1;
_rc.function[FUNC_0] = _parameters.rc_map_aux1 - 1;
_rc.function[FUNC_1] = _parameters.rc_map_aux2 - 1;
_rc.function[FUNC_2] = _parameters.rc_map_aux3 - 1;
/* gyro offsets */
param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
@ -905,7 +922,7 @@ Sensors::ppm_poll()
*/
if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
for (int i = 0; i < ppm_decoded_channels; i++) {
for (unsigned int i = 0; i < ppm_decoded_channels; i++) {
raw.values[i] = ppm_buffer[i];
}
@ -979,35 +996,54 @@ Sensors::ppm_poll()
manual_control.timestamp = rc_input.timestamp;
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
/* check if input needs to be de-mixed */
if (_parameters.rc_demix == (int)RC_DEMIX_DELTA) {
// XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS
/* roll input - mixed roll and pitch channels */
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled;
/* pitch input - mixed roll and pitch channels */
manual_control.pitch = -0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled);
/* yaw input - stick right is positive and positive rotation */
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
/* throttle input */
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
/* direct pass-through of manual input */
} else {
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
/*
* pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
* so reverse sign.
*/
manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
/* yaw input - stick right is positive and positive rotation */
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
/* throttle input */
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
}
/* limit outputs */
if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
if (manual_control.roll > 1.0f) manual_control.roll = 1.0f;
if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) {
if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) {
manual_control.roll *= _parameters.rc_scale_roll;
}
/*
* pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
* so reverse sign.
*/
manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f;
if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) {
if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
manual_control.pitch *= _parameters.rc_scale_pitch;
}
/* yaw input - stick right is positive and positive rotation */
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
manual_control.yaw *= _parameters.rc_scale_yaw;
}
/* throttle input */
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;

2
apps/uORB/topics/vehicle_status.h
View File

@ -79,7 +79,7 @@ enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
VEHICLE_MODE_FLAG_HIL_ENABLED = 32,
VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16,
VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16,
VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8,
VEHICLE_MODE_FLAG_AUTO_ENABLED = 4,
VEHICLE_MODE_FLAG_TEST_ENABLED = 2,