From 126e6ac2073ffb96c3867e7cbdd4e51e8408d0ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Dec 2012 16:30:21 +0100 Subject: Enabled manual override switch, work in progress. Added initial demix testing code to support delta mixing on the remote for convenient manual override --- apps/px4io/comms.c | 4 +- apps/px4io/controls.c | 21 +++++++++- apps/px4io/mixer.c | 5 ++- apps/px4io/px4io.h | 31 ++++++++++---- apps/px4io/safety.c | 2 +- apps/sensors/sensor_params.c | 2 + apps/sensors/sensors.cpp | 86 +++++++++++++++++++++++++++------------ apps/uORB/topics/vehicle_status.h | 2 +- 8 files changed, 113 insertions(+), 40 deletions(-) (limited to 'apps') diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 40ea38cf7..f4eddbdd3 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -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; diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index d4eace3df..0e0210373 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -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(); diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 483e9fe4d..d9626c1e3 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -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); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 483b9bcc8..9bfe3b1e4 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -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; diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index d5bd103c1..916988af1 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -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 diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index c83d6358e..37ba0ec3e 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -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)); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 71feb377b..b63bfb195 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -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; diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 23172d7cf..b70b322d8 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -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, -- cgit v1.2.3