From cb3a4f12670ce8d6cf608eb0ec97298a3bcb0919 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:23:40 +0100 Subject: [PATCH 01/55] px4io driver: publish input_rc even if RC connection has been lost --- src/drivers/px4io/px4io.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 82f3ba044a..2c00785038 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,10 +1479,9 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, only publish if RC OK flag is set */ - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; + /* we do not know the RC input, but have to publish timestamp_published + * and rc_lost flag, so do not prematurely return here + */ } /* lazily advertise on first publication */ From 409fa565f48ffe164b7332c9186a876b2771922a Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:42:44 +0100 Subject: [PATCH 02/55] px4io: do not include failsafe condition into rc_lost flag --- src/modules/px4iofirmware/controls.c | 138 +++++++++++++-------------- 1 file changed, 67 insertions(+), 71 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 941500f0d9..609dd33126 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -201,94 +201,90 @@ controls_tick() { /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* do not command anything in failsafe, kick in the RC loss counter */ - if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; - /* update RC-received timestamp */ - system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint16_t raw = r_raw_rc_values[i]; - uint16_t raw = r_raw_rc_values[i]; + int16_t scaled; - int16_t scaled; + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + 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; - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); - /* 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); - - } } } + } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* set RC OK flag, as we got an update */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + /* set RC OK flag, as we got an update */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - /* if we have enough channels (5) to control the vehicle, the mapping is ok */ - if (assigned_channels > 4) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); - } + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } /* @@ -316,8 +312,8 @@ controls_tick() { * Handle losing RC input */ - /* this kicks in if the receiver is gone or the system went to failsafe */ - if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ + if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | From 367ce63b86b863d72a3f6b4250d9da780a85f40b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 11:45:57 +0400 Subject: [PATCH 03/55] 'signal_lost' flag added to manual_control_setpoint and rc_channels topics to indicate signal loss immediately --- src/modules/commander/commander.cpp | 13 +- src/modules/sensors/sensors.cpp | 250 +++++++++--------- .../uORB/topics/manual_control_setpoint.h | 2 + src/modules/uORB/topics/rc_channels.h | 1 + 4 files changed, 136 insertions(+), 130 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e2..edd77231dc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -118,8 +118,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ -#define RC_TIMEOUT 100000 -#define RC_TIMEOUT_HIL 500000 +#define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[]) } } - - /* - * XXX workaround: - * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) - * which can trigger RC loss if the computer/simulator lags. - */ - uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; - /* start RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) { + if (!status.rc_input_blocked && !sp_man.signal_lost && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f890c4c7f8..9f816f5e1b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -474,6 +474,7 @@ Sensors::Sensors() : _battery_discharged(0), _battery_current_timestamp(0) { + memset(&_rc, 0, sizeof(_rc)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1283,12 +1284,6 @@ Sensors::rc_poll() if (rc_updated) { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; - - orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - - if (rc_input.rc_lost) - return; - struct manual_control_setpoint_s manual_control; struct actuator_controls_s actuator_group_3; @@ -1311,19 +1306,28 @@ Sensors::rc_poll() manual_control.aux4 = NAN; manual_control.aux5 = NAN; - /* require at least four channels to consider the signal valid */ - if (rc_input.channel_count < 4) - return; + manual_control.signal_lost = true; - /* failsafe check */ - if (_parameters.rc_fs_ch != 0) { - if (_parameters.rc_fs_mode == 0) { - if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) - return; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - } else if (_parameters.rc_fs_mode == 1) { - if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) - return; + /* detect RC signal loss */ + /* check flags and require at least four channels to consider the signal valid */ + if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { + /* signal looks good */ + manual_control.signal_lost = false; + + /* check throttle failsafe */ + if (_parameters.rc_fs_ch != 0) { + if (_parameters.rc_fs_mode == 0) { + if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { + manual_control.signal_lost = true; + } + + } else if (_parameters.rc_fs_mode == 1) { + if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) { + manual_control.signal_lost = true; + } + } } } @@ -1332,10 +1336,7 @@ Sensors::rc_poll() if (channel_limit > _rc_max_chan_count) channel_limit = _rc_max_chan_count; - /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp_last_signal; - - /* Read out values from raw message */ + /* read out and scale values from raw message even if signal is invalid */ for (unsigned int i = 0; i < channel_limit; i++) { /* @@ -1382,105 +1383,124 @@ Sensors::rc_poll() } _rc.chan_count = rc_input.channel_count; - _rc.timestamp = rc_input.timestamp_last_signal; + _rc.rssi = rc_input.rssi; + _rc.signal_lost = manual_control.signal_lost; - manual_control.timestamp = rc_input.timestamp_last_signal; - - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = limit_minus_one_to_one(_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 = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); - /* 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; - - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; + if (!manual_control.signal_lost) { + _rc_last_valid = rc_input.timestamp_last_signal; } - if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } + _rc.timestamp = _rc_last_valid; - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } + manual_control.timestamp = _rc_last_valid; - /* flaps */ - if (_rc.function[FLAPS] >= 0) { + if (!manual_control.signal_lost) { + /* fill values in manual_control_setpoint topic only if signal is valid */ - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = limit_minus_one_to_one(_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 = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + + /* scale output */ + if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { + manual_control.roll *= _parameters.rc_scale_roll; + } + + if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } + + if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { + manual_control.yaw *= _parameters.rc_scale_yaw; + } + + /* flaps */ + if (_rc.function[FLAPS] >= 0) { + + manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); + + if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { + manual_control.flaps *= _parameters.rc_scale_flaps; + } + } + + /* mode switch input */ + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + } + + /* assisted switch input */ + if (_rc.function[ASSISTED] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + } + + /* mission switch input */ + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); + } + + /* return switch input */ + if (_rc.function[RETURN] >= 0) { + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + } + + // if (_rc.function[OFFBOARD_MODE] >= 0) { + // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + // } + + /* aux functions, only assign if valid mapping is present */ + if (_rc.function[AUX_1] >= 0) { + manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); + } + + if (_rc.function[AUX_2] >= 0) { + manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); + } + + if (_rc.function[AUX_3] >= 0) { + manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); + } + + if (_rc.function[AUX_4] >= 0) { + manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); + } + + if (_rc.function[AUX_5] >= 0) { + manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + } + + /* copy from mapped manual control to control group 3 */ + actuator_group_3.control[0] = manual_control.roll; + actuator_group_3.control[1] = manual_control.pitch; + actuator_group_3.control[2] = manual_control.yaw; + actuator_group_3.control[3] = manual_control.throttle; + actuator_group_3.control[4] = manual_control.flaps; + actuator_group_3.control[5] = manual_control.aux1; + actuator_group_3.control[6] = manual_control.aux2; + actuator_group_3.control[7] = manual_control.aux3; + + /* publish actuator_controls_3 topic only if control signal is valid */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); } } - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } - - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } - - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } - - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } - -// if (_rc.function[OFFBOARD_MODE] >= 0) { -// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); -// } - - /* aux functions, only assign if valid mapping is present */ - if (_rc.function[AUX_1] >= 0) { - manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); - } - - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); - } - - if (_rc.function[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); - } - - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); - } - - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); - } - - /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; - - /* check if ready for publishing */ + /* publish rc_channels topic even if signal is invalid, for debug */ if (_rc_pub > 0) { orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); @@ -1489,21 +1509,13 @@ Sensors::rc_poll() _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); } - /* check if ready for publishing */ + /* publish manual_control_setpoint topic even if signal is not valid to update 'signal_lost' flag */ if (_manual_control_pub > 0) { orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); } else { _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); } - - /* check if ready for publishing */ - if (_actuator_group_3_pub > 0) { - orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); - - } else { - _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); - } } } diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e986..5d13843800 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -51,6 +51,8 @@ struct manual_control_setpoint_s { uint64_t timestamp; + bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ + float roll; /**< ailerons roll / roll rate input */ float pitch; /**< elevator / pitch / pitch rate */ float yaw; /**< rudder / yaw rate / yaw */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 6eb20efd1b..3246a39dd6 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -94,6 +94,7 @@ struct rc_channels_s { char function_name[RC_CHANNELS_FUNCTION_MAX][20]; int8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ + bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ }; /**< radio control channels. */ /** From 2c4792d48ee98cf46d9f9cf8ec43a759d6cc15d0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 11:46:21 +0400 Subject: [PATCH 04/55] sdlog2: added 'signal_lost' logging --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 13981ac544..112a4bd3ef 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1176,6 +1176,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); log_msg.body.log_RC.channel_count = buf.rc.chan_count; + log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; LOGBUFFER_WRITE_AND_COUNT(RC); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2b41755de9..de12e623a6 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -161,6 +161,7 @@ struct log_STAT_s { struct log_RC_s { float channel[8]; uint8_t channel_count; + uint8_t signal_lost; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ @@ -323,7 +324,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), - LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), + LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), From 1d5f62d890d1d85cef5e0f8e282d8e9e70717d46 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 17:26:07 +0400 Subject: [PATCH 05/55] sensors: use enum for switches position and -1..1 for values in 'manual_control_setpoint' topic --- src/modules/sensors/sensors.cpp | 163 ++++++++---------- .../uORB/topics/manual_control_setpoint.h | 37 ++-- 2 files changed, 92 insertions(+), 108 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9f816f5e1b..e08d8618f0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -135,7 +135,7 @@ */ #define PCB_TEMP_ESTIMATE_DEG 5.0f -#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) +#define STICK_ON_OFF_LIMIT 0.75f /** * Sensor app start / stop handling function @@ -169,6 +169,16 @@ private: hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ + /** + * Get and limit value for specified RC function. Returns NAN if not mapped. + */ + float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value); + + /** + * Get switch position for specified function. + */ + switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func); + /** * Gather and publish RC input data. */ @@ -1275,6 +1285,45 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } +float +Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value < min_value) { + return min_value; + + } else if (value > max_value) { + return max_value; + + } else { + return value; + } + } else { + return NAN; + } +} + +switch_pos_t +Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value > STICK_ON_OFF_LIMIT) { + return SWITCH_POS_ON; + + } else if (value < STICK_ON_OFF_LIMIT) { + return SWITCH_POS_OFF; + + } else { + return SWITCH_POS_MIDDLE; + } + + } else { + return SWITCH_POS_NONE; + } +} + void Sensors::rc_poll() { @@ -1292,13 +1341,6 @@ Sensors::rc_poll() manual_control.pitch = NAN; manual_control.yaw = NAN; manual_control.throttle = NAN; - - manual_control.mode_switch = NAN; - manual_control.return_switch = NAN; - manual_control.assisted_switch = NAN; - manual_control.mission_switch = NAN; -// manual_control.auto_offboard_input_switch = NAN; - manual_control.flaps = NAN; manual_control.aux1 = NAN; manual_control.aux2 = NAN; @@ -1306,6 +1348,11 @@ Sensors::rc_poll() manual_control.aux4 = NAN; manual_control.aux5 = NAN; + manual_control.mode_switch = SWITCH_POS_NONE; + manual_control.return_switch = SWITCH_POS_NONE; + manual_control.assisted_switch = SWITCH_POS_NONE; + manual_control.mission_switch = SWITCH_POS_NONE; + manual_control.signal_lost = true; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1316,7 +1363,7 @@ Sensors::rc_poll() /* signal looks good */ manual_control.signal_lost = false; - /* check throttle failsafe */ + /* check for throttle failsafe */ if (_parameters.rc_fs_ch != 0) { if (_parameters.rc_fs_mode == 0) { if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { @@ -1397,89 +1444,23 @@ Sensors::rc_poll() if (!manual_control.signal_lost) { /* fill values in manual_control_setpoint topic only if signal is valid */ - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = limit_minus_one_to_one(_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 = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + /* limit controls */ + manual_control.roll = get_rc_value(ROLL, -1.0, 1.0); + manual_control.pitch = get_rc_value(PITCH, -1.0, 1.0); + manual_control.yaw = get_rc_value(YAW, -1.0, 1.0); + manual_control.throttle = get_rc_value(THROTTLE, 0.0, 1.0); + manual_control.flaps = get_rc_value(FLAPS, -1.0, 1.0); + manual_control.aux1 = get_rc_value(AUX_1, -1.0, 1.0); + manual_control.aux2 = get_rc_value(AUX_2, -1.0, 1.0); + manual_control.aux3 = get_rc_value(AUX_3, -1.0, 1.0); + manual_control.aux4 = get_rc_value(AUX_4, -1.0, 1.0); + manual_control.aux5 = get_rc_value(AUX_5, -1.0, 1.0); - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; - } - - if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } - - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - - /* flaps */ - if (_rc.function[FLAPS] >= 0) { - - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); - - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; - } - } - - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } - - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } - - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } - - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } - - // if (_rc.function[OFFBOARD_MODE] >= 0) { - // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); - // } - - /* aux functions, only assign if valid mapping is present */ - if (_rc.function[AUX_1] >= 0) { - manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); - } - - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); - } - - if (_rc.function[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); - } - - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); - } - - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); - } + /* mode switches */ + manual_control.mode_switch = get_rc_switch_position(MODE); + manual_control.assisted_switch = get_rc_switch_position(ASSISTED); + manual_control.mission_switch = get_rc_switch_position(MISSION); + manual_control.return_switch = get_rc_switch_position(RETURN); /* copy from mapped manual control to control group 3 */ actuator_group_3.control[0] = manual_control.roll; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 5d13843800..985d3923fb 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -43,6 +43,16 @@ #include #include "../uORB.h" +/** + * Switch position + */ +typedef enum { + SWITCH_POS_NONE = 0, /**< switch is not mapped */ + SWITCH_POS_ON, /**< switch activated (value = 1) */ + SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ + SWITCH_POS_OFF /**< switch not activated (value = -1) */ +} switch_pos_t; + /** * @addtogroup topics * @{ @@ -53,32 +63,25 @@ struct manual_control_setpoint_s { bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ - float roll; /**< ailerons roll / roll rate input */ - float pitch; /**< elevator / pitch / pitch rate */ - float yaw; /**< rudder / yaw rate / yaw */ - float throttle; /**< throttle / collective thrust / altitude */ - - float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - float return_switch; /**< land 2 position switch (mandatory): land, no effect */ - float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ - /** - * Any of the channels below may not be available and be set to NaN + * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. */ - - // XXX needed or parameter? - //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - - float flaps; /**< flap position */ - + float roll; /**< ailerons roll / roll rate input, -1..1 */ + float pitch; /**< elevator / pitch / pitch rate, -1..1 */ + float yaw; /**< rudder / yaw rate / yaw, -1..1 */ + float throttle; /**< throttle / collective thrust / altitude, 0..1 */ + float flaps; /**< flap position */ float aux1; /**< default function: camera yaw / azimuth */ float aux2; /**< default function: camera pitch / tilt */ float aux3; /**< default function: camera trigger */ float aux4; /**< default function: camera roll */ float aux5; /**< default function: payload drop */ + switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ + switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** From 6f38ed3b4b6f266098d0616b6bd3c18ffe082755 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 20:23:34 +0400 Subject: [PATCH 06/55] commander, navigator: use updated manual_control_setpoint --- src/modules/commander/commander.cpp | 136 +++++++++++------------ src/modules/navigator/navigator_main.cpp | 98 ++++++---------- src/modules/uORB/topics/vehicle_status.h | 28 ----- 3 files changed, 94 insertions(+), 168 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index edd77231dc..7e469c9c19 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -112,8 +113,7 @@ extern struct system_load_s system_load; #define MAVLINK_OPEN_INTERVAL 50000 -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_LIMIT 0.9f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) @@ -207,7 +207,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t set_main_state_rc(struct vehicle_status_s *status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); @@ -814,6 +814,11 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); + /* Subscribe to position setpoint triplet */ + int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + struct position_setpoint_triplet_s pos_sp_triplet; + memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -1005,6 +1010,13 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } + /* update subsystem */ + orb_check(pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); + } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; @@ -1135,7 +1147,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1153,7 +1165,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1193,11 +1205,8 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } - /* fill status according to mode switches */ - check_mode_switches(&sp_man, &status); - /* evaluate the main state machine according to mode switches */ - res = set_main_state_rc(&status); + res = set_main_state_rc(&status, &sp_man); /* play tune on mode change only if armed, blink LED always */ if (res == TRANSITION_CHANGED) { @@ -1208,6 +1217,33 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } + /* set navigation state */ + /* RETURN switch, overrides MISSION switch */ + if (sp_man.return_switch == SWITCH_POS_ON) { + /* switch to RTL if not already landed after RTL and home position set */ + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else { + /* MISSION switch */ + if (sp_man.mission_switch == SWITCH_POS_ON) { + /* stick is in LOITER position */ + status.set_nav_state = NAV_STATE_LOITER; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + /* stick is in MISSION position */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if (sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE && + pos_sp_triplet.nav_state == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + } + } + } else { if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); @@ -1255,7 +1291,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1470,76 +1506,26 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a leds_counter++; } -void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) -{ - /* main mode switch */ - if (!isfinite(sp_man->mode_switch)) { - /* default to manual if signal is invalid */ - status->mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_AUTO; - - } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_MANUAL; - - } else { - status->mode_switch = MODE_SWITCH_ASSISTED; - } - - /* return switch */ - if (!isfinite(sp_man->return_switch)) { - status->return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - status->return_switch = RETURN_SWITCH_RETURN; - - } else { - status->return_switch = RETURN_SWITCH_NORMAL; - } - - /* assisted switch */ - if (!isfinite(sp_man->assisted_switch)) { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - - } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - status->assisted_switch = ASSISTED_SWITCH_EASY; - - } else { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - } - - /* mission switch */ - if (!isfinite(sp_man->mission_switch)) { - status->mission_switch = MISSION_SWITCH_NONE; - - } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - status->mission_switch = MISSION_SWITCH_LOITER; - - } else { - status->mission_switch = MISSION_SWITCH_MISSION; - } -} - transition_result_t -set_main_state_rc(struct vehicle_status_s *status) +set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; - switch (status->mode_switch) { - case MODE_SWITCH_MANUAL: + switch (sp_man->mode_switch) { + case SWITCH_POS_NONE: + case SWITCH_POS_OFF: // MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_ASSISTED: - if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + case SWITCH_POS_MIDDLE: // ASSISTED + if (sp_man->assisted_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_EASY); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to SEATBELT print_reject_mode(status, "EASY"); @@ -1547,29 +1533,33 @@ set_main_state_rc(struct vehicle_status_s *status) res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode + } - if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + if (sp_man->assisted_switch != SWITCH_POS_ON) { print_reject_mode(status, "SEATBELT"); + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_AUTO: + case SWITCH_POS_ON: // AUTO res = main_state_transition(status, MAIN_STATE_AUTO); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to SEATBELT (EASY likely will not work too) print_reject_mode(status, "AUTO"); res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1ba..b7f26198b6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -690,84 +690,46 @@ Navigator::task_main() if (fds[6].revents & POLLIN) { vehicle_status_update(); - /* evaluate state machine from commander and set the navigator mode accordingly */ + /* evaluate state requested by commander */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - bool stick_mode = false; + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - /* switch to RTL if not already landed after RTL and home position set */ + case NAV_STATE_LOITER: + request_loiter_or_ready(); + break; + + case NAV_STATE_MISSION: + request_mission_if_available(); + break; + + case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } - stick_mode = true; + break; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - request_loiter_or_ready(); - stick_mode = true; + case NAV_STATE_LAND: + dispatch(EVENT_LAND_REQUESTED); - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - request_mission_if_available(); - stick_mode = true; - } + break; - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - request_mission_if_available(); - stick_mode = true; - } + default: + warnx("ERROR: Requested navigation state not supported"); + break; } - } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - - case NAV_STATE_MISSION: - request_mission_if_available(); - break; - - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } + } else { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + request_mission_if_available(); } } @@ -775,6 +737,8 @@ Navigator::task_main() /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } + + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; } /* parameters updated */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 56be4d2412..48af4c9e2d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -93,29 +93,6 @@ typedef enum { FAILSAFE_STATE_MAX } failsafe_state_t; -typedef enum { - MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_ASSISTED, - MODE_SWITCH_AUTO -} mode_switch_pos_t; - -typedef enum { - ASSISTED_SWITCH_SEATBELT = 0, - ASSISTED_SWITCH_EASY -} assisted_switch_pos_t; - -typedef enum { - RETURN_SWITCH_NONE = 0, - RETURN_SWITCH_NORMAL, - RETURN_SWITCH_RETURN -} return_switch_pos_t; - -typedef enum { - MISSION_SWITCH_NONE = 0, - MISSION_SWITCH_LOITER, - MISSION_SWITCH_MISSION -} mission_switch_pos_t; - enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -187,11 +164,6 @@ struct vehicle_status_s { bool is_rotary_wing; - mode_switch_pos_t mode_switch; - return_switch_pos_t return_switch; - assisted_switch_pos_t assisted_switch; - mission_switch_pos_t mission_switch; - bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; From e2ac5222d812bdbfaf33fc2d320ee22ab861d433 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 20:54:28 +0400 Subject: [PATCH 07/55] mc_att_control, mc_pos_control: update manual_control_setpoint usage --- .../mc_att_control/mc_att_control_main.cpp | 54 +++++++++---------- .../mc_att_control/mc_att_control_params.c | 29 ++++++++++ .../mc_pos_control/mc_pos_control_main.cpp | 14 +---- src/modules/sensors/sensor_params.c | 22 -------- src/modules/sensors/sensors.cpp | 19 ------- 5 files changed, 57 insertions(+), 81 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 9cb8e83445..01f1631a97 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -157,7 +157,9 @@ private: param_t yaw_rate_d; param_t yaw_ff; - param_t rc_scale_yaw; + param_t man_scale_roll; + param_t man_scale_pitch; + param_t man_scale_yaw; } _params_handles; /**< handles for interesting parameters */ struct { @@ -167,7 +169,9 @@ private: math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ - float rc_scale_yaw; + float man_scale_roll; + float man_scale_pitch; + float man_scale_yaw; } _params; /** @@ -295,7 +299,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _params_handles.man_scale_roll = param_find("MC_SCALE_ROLL"); + _params_handles.man_scale_pitch = param_find("MC_SCALE_PITCH"); + _params_handles.man_scale_yaw = param_find("MC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -330,7 +336,7 @@ MulticopterAttitudeControl::parameters_update() { float v; - /* roll */ + /* roll gains */ param_get(_params_handles.roll_p, &v); _params.att_p(0) = v; param_get(_params_handles.roll_rate_p, &v); @@ -340,7 +346,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; - /* pitch */ + /* pitch gains */ param_get(_params_handles.pitch_p, &v); _params.att_p(1) = v; param_get(_params_handles.pitch_rate_p, &v); @@ -350,7 +356,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; - /* yaw */ + /* yaw gains */ param_get(_params_handles.yaw_p, &v); _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); @@ -362,7 +368,14 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_ff, &_params.yaw_ff); - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + /* manual control scale */ + param_get(_params_handles.man_scale_roll, &_params.man_scale_roll); + param_get(_params_handles.man_scale_pitch, &_params.man_scale_pitch); + param_get(_params_handles.man_scale_yaw, &_params.man_scale_yaw); + + _params.man_scale_roll *= M_PI / 180.0; + _params.man_scale_pitch *= M_PI / 180.0; + _params.man_scale_yaw *= M_PI / 180.0; return OK; } @@ -404,7 +417,6 @@ MulticopterAttitudeControl::vehicle_manual_poll() orb_check(_manual_control_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); } } @@ -483,24 +495,10 @@ MulticopterAttitudeControl::control_attitude(float dt) // reset_yaw_sp = true; //} } else { - float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; - - if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; - - if (_manual_control_sp.yaw > 0.0f) { - yaw_sp_move_rate -= YAW_DEADZONE; - - } else { - yaw_sp_move_rate += YAW_DEADZONE; - } - - yaw_sp_move_rate *= _params.rc_scale_yaw; - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); - _v_att_sp.R_valid = false; - publish_att_sp = true; - } + /* move yaw setpoint */ + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_scale_yaw * dt); + _v_att_sp.R_valid = false; + publish_att_sp = true; } /* reset yaw setpint to current position if needed */ @@ -513,8 +511,8 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.roll; - _v_att_sp.pitch_body = _manual_control_sp.pitch; + _v_att_sp.roll_body = _manual_control_sp.roll * _params.man_scale_roll; + _v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_scale_pitch; _v_att_sp.R_valid = false; publish_att_sp = true; } diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 488107d585..9acf8bfa3e 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -173,3 +173,32 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); * @group Multicopter Attitude Control */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); + +/** + * Manual control scaling factor for roll + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_SCALE_ROLL, 35.0f); + +/** + * Manual control scaling factor for pitch + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_SCALE_PITCH, 35.0f); + +/** + * Manual control scaling factor for yaw + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_SCALE_YAW, 120.0f); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 78d06ba5b5..2bd2d356a0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -148,9 +148,6 @@ private: param_t tilt_max; param_t land_speed; param_t land_tilt_max; - - param_t rc_scale_pitch; - param_t rc_scale_roll; } _params_handles; /**< handles for interesting parameters */ struct { @@ -160,9 +157,6 @@ private: float land_speed; float land_tilt_max; - float rc_scale_pitch; - float rc_scale_roll; - math::Vector<3> pos_p; math::Vector<3> vel_p; math::Vector<3> vel_i; @@ -306,8 +300,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.tilt_max = param_find("MPC_TILT_MAX"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); - _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); - _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); /* fetch initial parameter values */ parameters_update(true); @@ -354,8 +346,6 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.tilt_max, &_params.tilt_max); param_get(_params_handles.land_speed, &_params.land_speed); param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); - param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); - param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); float v; param_get(_params_handles.xy_p, &v); @@ -608,8 +598,8 @@ MulticopterPositionControl::task_main() reset_lat_lon_sp(); /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); - sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz); + sp_move_rate(0) = _manual.pitch; + sp_move_rate(1) = _manual.roll; } /* limit setpoint move rate */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 288c6e00a0..09afaf9498 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -647,28 +647,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); -/** - * Roll scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); - -/** - * Pitch scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); - -/** - * Yaw scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); - - /** * Failsafe channel mapping. * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e08d8618f0..8f350751c1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -268,11 +268,6 @@ private: int rc_map_aux4; int rc_map_aux5; - float rc_scale_roll; - float rc_scale_pitch; - float rc_scale_yaw; - float rc_scale_flaps; - int rc_fs_ch; int rc_fs_mode; float rc_fs_thr; @@ -318,11 +313,6 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; - param_t rc_scale_roll; - param_t rc_scale_pitch; - param_t rc_scale_yaw; - param_t rc_scale_flaps; - param_t rc_fs_ch; param_t rc_fs_mode; param_t rc_fs_thr; @@ -536,11 +526,6 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); - _parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); - _parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); - _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); - _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); - /* RC failsafe */ _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); @@ -696,10 +681,6 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); - param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); - param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); - param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); - param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); From ef8b97437342401a464bcc844d6c347c33919d73 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 22:58:57 +0400 Subject: [PATCH 08/55] fw_att_control: update manual_control_setpoint usage --- .../fw_att_control/fw_att_control_main.cpp | 17 ++++++++++++++--- .../fw_att_control/fw_att_control_params.c | 10 ++++++++++ 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index f139c25f48..4464b5e012 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -173,6 +173,8 @@ private: float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float roll_max; /**< Max Roll in rad */ + float pitch_max; /**< Max Pitch in rad */ } _parameters; /**< local copies of interesting parameters */ @@ -211,6 +213,8 @@ private: param_t trim_yaw; param_t rollsp_offset_deg; param_t pitchsp_offset_deg; + param_t roll_max; + param_t pitch_max; } _parameter_handles; /**< handles for interesting parameters */ @@ -354,6 +358,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); + _parameter_handles.roll_max = param_find("FW_R_MAX"); + _parameter_handles.pitch_max = param_find("FW_P_MAX"); + /* fetch initial parameter values */ parameters_update(); } @@ -421,6 +428,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); + param_get(_parameter_handles.roll_max, &(_parameters.roll_max)); + param_get(_parameter_handles.pitch_max, &(_parameters.pitch_max)); + _parameters.roll_max = math::radians(_parameters.roll_max); + _parameters.pitch_max = math::radians(_parameters.pitch_max); /* pitch control parameters */ @@ -700,8 +711,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad; - pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.roll * _parameters.roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; + pitch_sp = (-_manual.pitch * _parameters.pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; @@ -809,7 +820,7 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ _actuators.control[0] = _manual.roll; - _actuators.control[1] = _manual.pitch; + _actuators.control[1] = -_manual.pitch; _actuators.control[2] = _manual.yaw; _actuators.control[3] = _manual.throttle; _actuators.control[4] = _manual.flaps; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index c80a44f2a5..14815745cd 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -186,3 +186,13 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); // @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe // @Range -90.0 to 90.0 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); + +// @DisplayName Max Roll +// @Description Max roll for manual control in attitude stabilized mode +// @Range 0.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_R_MAX, 45.0f); + +// @DisplayName Max Pitch +// @Description Max pitch for manual control in attitude stabilized mode +// @Range 0.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_P_MAX, 45.0f); From 3641faed0c696f1a88f88a17b5666ed5c3ba8b1c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 23:18:43 +0400 Subject: [PATCH 09/55] sensors: publish last valid manual control values when signal lost --- src/modules/sensors/sensors.cpp | 111 ++++++++++++++------------------ 1 file changed, 50 insertions(+), 61 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8f350751c1..669f4e174d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -167,8 +167,6 @@ public: private: 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 */ - /** * Get and limit value for specified RC function. Returns NAN if not mapped. */ @@ -216,6 +214,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ + struct manual_control_setpoint_s _manual; /**< manual control setpoint */ struct battery_status_s _battery_status; /**< battery status */ struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; @@ -438,8 +437,6 @@ Sensors *g_sensors = nullptr; } Sensors::Sensors() : - _rc_last_valid(0), - _fd_adc(-1), _last_adc(0), @@ -475,6 +472,21 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); + memset(&_manual, 0, sizeof(_manual)); + + /* set NANs instead of zeroes */ + _manual.roll = NAN; + _manual.pitch = NAN; + _manual.yaw = NAN; + _manual.throttle = NAN; + _manual.flaps = NAN; + _manual.aux1 = NAN; + _manual.aux2 = NAN; + _manual.aux3 = NAN; + _manual.aux4 = NAN; + _manual.aux5 = NAN; + + _manual.signal_lost = true; /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1314,27 +1326,8 @@ Sensors::rc_poll() if (rc_updated) { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; - struct manual_control_setpoint_s manual_control; - struct actuator_controls_s actuator_group_3; - /* initialize to default values */ - manual_control.roll = NAN; - manual_control.pitch = NAN; - manual_control.yaw = NAN; - manual_control.throttle = NAN; - manual_control.flaps = NAN; - manual_control.aux1 = NAN; - manual_control.aux2 = NAN; - manual_control.aux3 = NAN; - manual_control.aux4 = NAN; - manual_control.aux5 = NAN; - - manual_control.mode_switch = SWITCH_POS_NONE; - manual_control.return_switch = SWITCH_POS_NONE; - manual_control.assisted_switch = SWITCH_POS_NONE; - manual_control.mission_switch = SWITCH_POS_NONE; - - manual_control.signal_lost = true; + _manual.signal_lost = true; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1342,18 +1335,18 @@ Sensors::rc_poll() /* check flags and require at least four channels to consider the signal valid */ if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { /* signal looks good */ - manual_control.signal_lost = false; + _manual.signal_lost = false; /* check for throttle failsafe */ if (_parameters.rc_fs_ch != 0) { if (_parameters.rc_fs_mode == 0) { if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { - manual_control.signal_lost = true; + _manual.signal_lost = true; } } else if (_parameters.rc_fs_mode == 1) { if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) { - manual_control.signal_lost = true; + _manual.signal_lost = true; } } } @@ -1412,46 +1405,42 @@ Sensors::rc_poll() _rc.chan_count = rc_input.channel_count; _rc.rssi = rc_input.rssi; - _rc.signal_lost = manual_control.signal_lost; + _rc.signal_lost = _manual.signal_lost; - if (!manual_control.signal_lost) { - _rc_last_valid = rc_input.timestamp_last_signal; - } - - _rc.timestamp = _rc_last_valid; - - manual_control.timestamp = _rc_last_valid; - - if (!manual_control.signal_lost) { + if (!_manual.signal_lost) { /* fill values in manual_control_setpoint topic only if signal is valid */ + _manual.timestamp = rc_input.timestamp_last_signal; + _rc.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual_control.roll = get_rc_value(ROLL, -1.0, 1.0); - manual_control.pitch = get_rc_value(PITCH, -1.0, 1.0); - manual_control.yaw = get_rc_value(YAW, -1.0, 1.0); - manual_control.throttle = get_rc_value(THROTTLE, 0.0, 1.0); - manual_control.flaps = get_rc_value(FLAPS, -1.0, 1.0); - manual_control.aux1 = get_rc_value(AUX_1, -1.0, 1.0); - manual_control.aux2 = get_rc_value(AUX_2, -1.0, 1.0); - manual_control.aux3 = get_rc_value(AUX_3, -1.0, 1.0); - manual_control.aux4 = get_rc_value(AUX_4, -1.0, 1.0); - manual_control.aux5 = get_rc_value(AUX_5, -1.0, 1.0); + _manual.roll = get_rc_value(ROLL, -1.0, 1.0); + _manual.pitch = get_rc_value(PITCH, -1.0, 1.0); + _manual.yaw = get_rc_value(YAW, -1.0, 1.0); + _manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0); + _manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); + _manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); + _manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); + _manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0); + _manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0); + _manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ - manual_control.mode_switch = get_rc_switch_position(MODE); - manual_control.assisted_switch = get_rc_switch_position(ASSISTED); - manual_control.mission_switch = get_rc_switch_position(MISSION); - manual_control.return_switch = get_rc_switch_position(RETURN); + _manual.mode_switch = get_rc_switch_position(MODE); + _manual.assisted_switch = get_rc_switch_position(ASSISTED); + _manual.mission_switch = get_rc_switch_position(MISSION); + _manual.return_switch = get_rc_switch_position(RETURN); /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; + struct actuator_controls_s actuator_group_3; + + actuator_group_3.control[0] = _manual.roll; + actuator_group_3.control[1] = _manual.pitch; + actuator_group_3.control[2] = _manual.yaw; + actuator_group_3.control[3] = _manual.throttle; + actuator_group_3.control[4] = _manual.flaps; + actuator_group_3.control[5] = _manual.aux1; + actuator_group_3.control[6] = _manual.aux2; + actuator_group_3.control[7] = _manual.aux3; /* publish actuator_controls_3 topic only if control signal is valid */ if (_actuator_group_3_pub > 0) { @@ -1473,10 +1462,10 @@ Sensors::rc_poll() /* publish manual_control_setpoint topic even if signal is not valid to update 'signal_lost' flag */ if (_manual_control_pub > 0) { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &_manual); } else { - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual); } } From 60355b4e6c0a6916084442d0557cacd74b008b82 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 23:47:09 +0400 Subject: [PATCH 10/55] sensors: switch position reading bug fixed --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 669f4e174d..79b53a79ce 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1305,7 +1305,7 @@ Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) if (value > STICK_ON_OFF_LIMIT) { return SWITCH_POS_ON; - } else if (value < STICK_ON_OFF_LIMIT) { + } else if (value < -STICK_ON_OFF_LIMIT) { return SWITCH_POS_OFF; } else { From c6d98a32f83383e6204fd6cefbfcc1fd7e1cf159 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:27:43 +0200 Subject: [PATCH 11/55] Proper failsafe handling onboard, including throttle failsafe condition if enabled --- src/modules/px4iofirmware/controls.c | 36 ++++++++++++++++++---------- src/modules/px4iofirmware/protocol.h | 9 +++---- 2 files changed, 29 insertions(+), 16 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 609dd33126..b860fc587d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -134,8 +134,6 @@ controls_tick() { perf_begin(c_gather_sbus); - bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); - bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); @@ -261,8 +259,20 @@ controls_tick() { 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 */ + if (mapped == 1) { + /* roll, pitch, yaw, throttle, override is the standard order */ scaled = -scaled; + } + + if (mapped == 3 && (r_setup_features & PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)) { + /* throttle failsafe detection */ + if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < 800)) || + ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > 2200))) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + } r_rc_values[mapped] = SIGNED_TO_REG(scaled); assigned_channels |= (1 << mapped); @@ -312,6 +322,11 @@ controls_tick() { * Handle losing RC input */ + /* if we are in failsafe, clear the override flag */ + if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + } + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ @@ -322,27 +337,24 @@ controls_tick() { /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; + /* Set raw channel count to zero */ + r_raw_rc_count = 0; + /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; } - /* this kicks in if the receiver is completely gone */ - if (rc_input_lost) { - - /* Set channel count to zero */ - r_raw_rc_count = 0; - } - /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input. + * must have R/C input (NO FAILSAFE!). * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index d48c6c5297..b69f681070 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -164,10 +164,11 @@ /* setup page */ #define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 -#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */ -#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */ -#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */ -#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT (1 << 4) /**< enable RC fail detection based on channel value */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ From 64ffafb48ee45452070dfb37be8d0de6098915a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:28:01 +0200 Subject: [PATCH 12/55] Only publish RC inputs if we have seen some valid inputs at some point --- src/drivers/px4io/px4io.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2c00785038..8eee1cbca0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,9 +1479,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, but have to publish timestamp_published - * and rc_lost flag, so do not prematurely return here - */ + /* only keep publishing RC input if we ever got a valid input */ + if (_rc_last_valid == 0) { + /* we have never seen valid RC signals, abort */ + return OK; + } } /* lazily advertise on first publication */ From fb44ad8e22f2a0862f1d7bda66643a6336247024 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:29:17 +0200 Subject: [PATCH 13/55] Simplify the failsafe handling, reduce 3 params to one --- src/modules/sensors/sensor_params.c | 28 +++------------------------- src/modules/sensors/sensors.cpp | 26 +++++++------------------- 2 files changed, 10 insertions(+), 44 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 288c6e00a0..e4564aa25c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -668,33 +668,11 @@ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); */ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); - -/** - * Failsafe channel mapping. - * - * @min 0 - * @max 18 - * @group Radio Calibration - */ -PARAM_DEFINE_INT32(RC_FS_CH, 0); - -/** - * Failsafe channel mode. - * - * 0 = too low means signal loss, - * 1 = too high means signal loss - * - * @min 0 - * @max 1 - * @group Radio Calibration - */ -PARAM_DEFINE_INT32(RC_FS_MODE, 0); - /** * Failsafe channel PWM threshold. * - * @min 0 - * @max 1 + * @min 800 + * @max 2200 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 800); +PARAM_DEFINE_FLOAT(RC_FS_THR, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f890c4c7f8..2442acd6b0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -263,8 +263,6 @@ private: float rc_scale_yaw; float rc_scale_flaps; - int rc_fs_ch; - int rc_fs_mode; float rc_fs_thr; float battery_voltage_scaling; @@ -313,8 +311,6 @@ private: param_t rc_scale_yaw; param_t rc_scale_flaps; - param_t rc_fs_ch; - param_t rc_fs_mode; param_t rc_fs_thr; param_t battery_voltage_scaling; @@ -531,8 +527,6 @@ Sensors::Sensors() : _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* RC failsafe */ - _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); - _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); /* gyro offsets */ @@ -689,8 +683,6 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); - param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); - param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); /* update RC function mappings */ @@ -1312,19 +1304,15 @@ Sensors::rc_poll() manual_control.aux5 = NAN; /* require at least four channels to consider the signal valid */ - if (rc_input.channel_count < 4) + if (rc_input.channel_count < 4) { return; + } - /* failsafe check */ - if (_parameters.rc_fs_ch != 0) { - if (_parameters.rc_fs_mode == 0) { - if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) - return; - - } else if (_parameters.rc_fs_mode == 1) { - if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) - return; - } + /* check for failsafe */ + if (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { + /* do not publish manual control setpoints when there are none */ + return; } unsigned channel_limit = rc_input.channel_count; From 797698a7a114032c7eea62c5f48f2b229ca973b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:34:35 +0200 Subject: [PATCH 14/55] Trigger failsafe action also on failsafe flag --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2442acd6b0..4c34d853fc 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,7 +1309,7 @@ Sensors::rc_poll() } /* check for failsafe */ - if (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + if (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { /* do not publish manual control setpoints when there are none */ return; From 9123ebce8cbc618899ece31d31c97e022038beb2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:45:02 +0200 Subject: [PATCH 15/55] px4io: Allow RC failsafe detection as valid feature --- src/modules/px4iofirmware/registers.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 97d25bbfa8..42b863b539 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -175,7 +175,8 @@ volatile uint16_t r_page_setup[] = #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI) + PX4IO_P_SETUP_FEATURES_PWM_RSSI | \ + PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif From 9a0b2b7610d39f88b627046e0d90f66aada1e88f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:02:22 +0200 Subject: [PATCH 16/55] Make throttle failsafe depend on the failsafe threshold parameter. Make the parameter optional (no harm if not found). --- src/drivers/px4io/px4io.cpp | 17 ++++++++++++++++- src/modules/px4iofirmware/controls.c | 6 +++--- src/modules/px4iofirmware/protocol.h | 21 +++++++++++---------- src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/registers.c | 4 ++-- 5 files changed, 33 insertions(+), 16 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8eee1cbca0..f6125d273d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -944,8 +944,23 @@ PX4IO::task_main() int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); if (pret != OK) { - log("voltage scaling upload failed"); + log("vscale upload failed"); } + + /* send RC throttle failsafe value to IO */ + float failsafe_param_val; + param_t failsafe_param = param_find("RC_FS_THR"); + + if (failsafe_param > 0) + + param_get(failsafe_param, &failsafe_param_val); + uint16_t failsafe_thr = failsafe_param_val; + pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); + if (pret != OK) { + log("failsafe upload failed"); + } + } + } } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index b860fc587d..356fe44cd5 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -264,10 +264,10 @@ controls_tick() { scaled = -scaled; } - if (mapped == 3 && (r_setup_features & PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)) { + if (mapped == 3 && r_setup_rc_thr_failsafe) { /* throttle failsafe detection */ - if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < 800)) || - ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > 2200))) { + if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || + ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b69f681070..a978d483a4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -168,7 +168,6 @@ #define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ #define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ #define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ -#define PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT (1 << 4) /**< enable RC fail detection based on channel value */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ @@ -202,13 +201,15 @@ enum { /* DSM bind states */ dsm_bind_send_pulses, dsm_bind_reinit_uart }; - /* 8 */ -#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + /* 8 */ +#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) */ +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ -#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ + /* 12 occupied by CRC */ +#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ @@ -218,10 +219,10 @@ enum { /* DSM bind states */ #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 */ +#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 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 54c5663a5b..88ad793980 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -108,6 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #endif +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]; #define r_control_values (&r_page_controls[0]) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 42b863b539..6752e7b4b3 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -169,14 +169,14 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, + [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI | \ - PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT) + PX4IO_P_SETUP_FEATURES_PWM_RSSI) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif From 5e0d687b566022c12270f68facbf7ca35f62306c Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:23:40 +0100 Subject: [PATCH 17/55] px4io driver: publish input_rc even if RC connection has been lost --- src/drivers/px4io/px4io.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 82f3ba044a..2c00785038 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,10 +1479,9 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, only publish if RC OK flag is set */ - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; + /* we do not know the RC input, but have to publish timestamp_published + * and rc_lost flag, so do not prematurely return here + */ } /* lazily advertise on first publication */ From 745ef4f4856af60aca5625860e5e3f25ea189dc9 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:42:44 +0100 Subject: [PATCH 18/55] px4io: do not include failsafe condition into rc_lost flag --- src/modules/px4iofirmware/controls.c | 138 +++++++++++++-------------- 1 file changed, 67 insertions(+), 71 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 941500f0d9..609dd33126 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -201,94 +201,90 @@ controls_tick() { /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* do not command anything in failsafe, kick in the RC loss counter */ - if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; - /* update RC-received timestamp */ - system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint16_t raw = r_raw_rc_values[i]; - uint16_t raw = r_raw_rc_values[i]; + int16_t scaled; - int16_t scaled; + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + 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; - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); - /* 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); - - } } } + } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* set RC OK flag, as we got an update */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + /* set RC OK flag, as we got an update */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - /* if we have enough channels (5) to control the vehicle, the mapping is ok */ - if (assigned_channels > 4) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); - } + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } /* @@ -316,8 +312,8 @@ controls_tick() { * Handle losing RC input */ - /* this kicks in if the receiver is gone or the system went to failsafe */ - if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ + if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | From 09d106432749ec02866241863eb912f05b903e64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:06:07 +0200 Subject: [PATCH 19/55] px4io: Remove unused variable --- src/modules/px4iofirmware/controls.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 609dd33126..f56f630bce 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -134,8 +134,6 @@ controls_tick() { perf_begin(c_gather_sbus); - bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); - bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); From 3e4841b6fe2d8d6d06b167be49cbe76ab7e04a46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:10:41 +0200 Subject: [PATCH 20/55] px4io: Guard against the RC failsafe value of channel 5 causing a manual override action if set to manual in failsafe --- src/modules/px4iofirmware/controls.c | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index f56f630bce..aae7bb951a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -310,6 +310,11 @@ controls_tick() { * Handle losing RC input */ + /* if we are in failsafe, clear the override flag */ + if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + } + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ @@ -320,27 +325,24 @@ controls_tick() { /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; - /* Set the RC_LOST alarm */ - r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; - } - - /* this kicks in if the receiver is completely gone */ - if (rc_input_lost) { - /* Set channel count to zero */ r_raw_rc_count = 0; + + /* Set the RC_LOST alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; } /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input. + * must have R/C input (NO FAILSAFE!). * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; @@ -363,10 +365,10 @@ controls_tick() { mixer_tick(); } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } } From 73d04f7a37ba52ce1891e740db554557bd71940a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:15:22 +0200 Subject: [PATCH 21/55] px4io driver: Only publish RC signal if it was at least once valid. --- src/drivers/px4io/px4io.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2c00785038..8eee1cbca0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,9 +1479,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, but have to publish timestamp_published - * and rc_lost flag, so do not prematurely return here - */ + /* only keep publishing RC input if we ever got a valid input */ + if (_rc_last_valid == 0) { + /* we have never seen valid RC signals, abort */ + return OK; + } } /* lazily advertise on first publication */ From 3b5e6f98338fded2cbe7be1c301dc2698f7239aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:28:07 +0200 Subject: [PATCH 22/55] sensors and px4io driver: Guard against failsafe trigger for inverted remotes --- src/drivers/px4io/px4io.cpp | 4 ++-- src/modules/sensors/sensor_params.c | 2 +- src/modules/sensors/sensors.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f6125d273d..a30bfb2dec 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -948,8 +948,8 @@ PX4IO::task_main() } /* send RC throttle failsafe value to IO */ - float failsafe_param_val; - param_t failsafe_param = param_find("RC_FS_THR"); + int32_t failsafe_param_val; + param_t failsafe_param = param_find("RC_FAILS_THR"); if (failsafe_param > 0) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index e4564aa25c..14f7ac812f 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -675,4 +675,4 @@ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); * @max 2200 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 0); +PARAM_DEFINE_INT32(RC_FAILS_THR, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4c34d853fc..44a91ca67a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -263,7 +263,7 @@ private: float rc_scale_yaw; float rc_scale_flaps; - float rc_fs_thr; + int32_t rc_fs_thr; float battery_voltage_scaling; float battery_current_scaling; @@ -527,7 +527,7 @@ Sensors::Sensors() : _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* RC failsafe */ - _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); + _parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { + if (_parameters.rc_fs_thr && (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr)))) { /* do not publish manual control setpoints when there are none */ return; } From ea5279389f2b13110735083f511afb630ef5e3d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:16:51 +1100 Subject: [PATCH 23/55] uORB: added an ORB topic for system_power holds power supply state and 5V rail voltage on FMUv2 --- src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/system_power.h | 71 ++++++++++++++++++++++++++ 2 files changed, 74 insertions(+) create mode 100644 src/modules/uORB/topics/system_power.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4b31cc8a49..c8a589bb7c 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s); #include "topics/servorail_status.h" ORB_DEFINE(servorail_status, struct servorail_status_s); +#include "topics/system_power.h" +ORB_DEFINE(system_power, struct system_power_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/src/modules/uORB/topics/system_power.h b/src/modules/uORB/topics/system_power.h new file mode 100644 index 0000000000..7763b60043 --- /dev/null +++ b/src/modules/uORB/topics/system_power.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file system_power.h + * + * Definition of the system_power voltage and power status uORB topic. + */ + +#ifndef SYSTEM_POWER_H_ +#define SYSTEM_POWER_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * voltage and power supply status + */ +struct system_power_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage5V_v; /**< peripheral 5V rail voltage */ + uint8_t usb_connected:1; /**< USB is connected when 1 */ + uint8_t brick_valid:1; /**< brick power is good when 1 */ + uint8_t servo_valid:1; /**< servo power is good when 1 */ + uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */ + uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(system_power); + +#endif From 6ea22c8c079db8633d663cbf8ca39b81a434a040 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:17:25 +1100 Subject: [PATCH 24/55] adc: publish system_power ORB topic from ADC --- src/drivers/stm32/adc/adc.cpp | 47 ++++++++++++++++++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 0b8a275e6a..3a60d2cae1 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -41,6 +41,7 @@ */ #include +#include #include #include @@ -64,6 +65,8 @@ #include #include +#include + /* * Register accessors. * For now, no reason not to just use ADC1. @@ -119,6 +122,8 @@ private: unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ + orb_advert_t _to_system_power; + /** work trampoline */ static void _tick_trampoline(void *arg); @@ -134,13 +139,16 @@ private: */ uint16_t _sample(unsigned channel); + // update system_power ORB topic, only on FMUv2 + void update_system_power(void); }; ADC::ADC(uint32_t channels) : CDev("adc", ADC_DEVICE_PATH), _sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")), _channel_count(0), - _samples(nullptr) + _samples(nullptr), + _to_system_power(0) { _debug_enabled = true; @@ -290,6 +298,43 @@ ADC::_tick() /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) _samples[i].am_data = _sample(_samples[i].am_channel); + update_system_power(); +} + +void +ADC::update_system_power(void) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + system_power_s system_power; + system_power.timestamp = hrt_absolute_time(); + + system_power.voltage5V_v = 0; + for (unsigned i = 0; i < _channel_count; i++) { + if (_samples[i].am_channel == 4) { + // it is 2:1 scaled + system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); + } + } + + // these are not ADC related, but it is convenient to + // publish these to the same topic + system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS); + + // note that the valid pins are active low + system_power.brick_valid = !stm32_gpioread(GPIO_VDD_BRICK_VALID); + system_power.servo_valid = !stm32_gpioread(GPIO_VDD_SERVO_VALID); + + // OC pins are active low + system_power.periph_5V_OC = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC); + system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC); + + /* lazily publish */ + if (_to_system_power > 0) { + orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + } else { + _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); + } +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } uint16_t From da105650526f724247aa8db7ee87a460efcd3cd8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 12:42:32 +0200 Subject: [PATCH 25/55] sdlog2: Add system power to logging --- src/modules/sdlog2/sdlog2.c | 25 +++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 14 ++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 13981ac544..7a984821f2 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -796,6 +796,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct telemetry_status_s telemetry; struct range_finder_report range_finder; struct estimator_status_report estimator_status; + struct system_power_s system_power; + struct servorail_status_s servorail_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -828,6 +830,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; struct log_ESTM_s log_ESTM; + struct log_PWR_s log_PWR; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -859,6 +862,8 @@ int sdlog2_thread_main(int argc, char *argv[]) int telemetry_sub; int range_finder_sub; int estimator_status_sub; + int system_power_sub; + int servorail_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -884,6 +889,8 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); + subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); + subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); thread_running = true; @@ -1226,6 +1233,24 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(BATT); } + /* --- SYSTEM POWER RAILS --- */ + if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) { + log_msg.msg_type = LOG_PWR_MSG; + log_msg.body.log_PWR.5v_peripherals = buf.system_power.voltage_5V_v; + log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; + log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid; + log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid; + log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC; + log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC; + + /* copy servo rail status topic here too */ + orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); + log_msg.body.log_PWR.5v_servo_rail = buf.servorail_status.voltage_v; + log_msg.body.log_PWR.5v_servo_rssi = buf.servorail_status.rssi_v; + + LOGBUFFER_WRITE_AND_COUNT(PWR); + } + /* --- TELEMETRY --- */ if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { log_msg.msg_type = LOG_TELE_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 22a6958720..93a150dbea 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -316,6 +316,19 @@ struct log_ESTM_s { // uint8_t kalman_gain_nan; // }; +/* --- PWR - ONBOARD POWER SYSTEM --- */ +#define LOG_PWR_MSG 133 +struct log_PWR_s { + float 5v_peripherals; + float 5v_servo_rail; + float 5v_servo_rssi; + uint8_t usb_ok; + uint8_t brick_ok; + uint8_t servo_ok; + uint8_t low_power_rail_overcurrent; + uint8_t high_power_rail_overcurrent; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -349,6 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"); //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; From 06d5c6ad285a46cfb59ce07bc3cd49507305a997 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 13:03:52 +0200 Subject: [PATCH 26/55] sdlog2: Compile fixes for system power logging --- src/modules/sdlog2/sdlog2.c | 8 +++++--- src/modules/sdlog2/sdlog2_messages.h | 8 ++++---- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7a984821f2..9f2fca4465 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -84,6 +84,8 @@ #include #include #include +#include +#include #include #include @@ -1236,7 +1238,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SYSTEM POWER RAILS --- */ if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) { log_msg.msg_type = LOG_PWR_MSG; - log_msg.body.log_PWR.5v_peripherals = buf.system_power.voltage_5V_v; + log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v; log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid; log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid; @@ -1245,8 +1247,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy servo rail status topic here too */ orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); - log_msg.body.log_PWR.5v_servo_rail = buf.servorail_status.voltage_v; - log_msg.body.log_PWR.5v_servo_rssi = buf.servorail_status.rssi_v; + log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v; + log_msg.body.log_PWR.servo_rssi_5v = buf.servorail_status.rssi_v; LOGBUFFER_WRITE_AND_COUNT(PWR); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 93a150dbea..d629259c0c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -319,9 +319,9 @@ struct log_ESTM_s { /* --- PWR - ONBOARD POWER SYSTEM --- */ #define LOG_PWR_MSG 133 struct log_PWR_s { - float 5v_peripherals; - float 5v_servo_rail; - float 5v_servo_rssi; + float peripherals_5v; + float servo_rail_5v; + float servo_rssi_5v; uint8_t usb_ok; uint8_t brick_ok; uint8_t servo_ok; @@ -362,7 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), - LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"); + LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; From 00c82f0836388ffa1e1c4a5827eefe28c0521df8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:18:20 +1100 Subject: [PATCH 27/55] Merged airspeed changes --- src/drivers/meas_airspeed/meas_airspeed.cpp | 62 ++++++++++++++++++++- 1 file changed, 59 insertions(+), 3 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 2c3efdc35f..2324475ceb 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -87,6 +87,7 @@ #include #include #include +#include #include @@ -121,6 +122,14 @@ protected: virtual int collect(); math::LowPassFilter2p _filter; + + /** + * Correct for 5V rail voltage variations + */ + void voltage_correction(float &diff_pres_pa); + + int _t_system_power; + struct system_power_s system_power; }; /* @@ -129,10 +138,11 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path), - _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ) + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), + _t_system_power(-1) { - + memset(&system_power, 0, sizeof(system_power)); } int @@ -207,6 +217,9 @@ MEASAirspeed::collect() diff_press_pa = 0.0f; } + // correct for 5V rail voltage if possible + voltage_correction(diff_press_pa); + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -287,6 +300,49 @@ MEASAirspeed::cycle() USEC2TICK(CONVERSION_INTERVAL)); } +/** + correct for 5V rail voltage if the system_power ORB topic is + available + + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of + offset versus voltage for 3 sensors + */ +void +MEASAirspeed::voltage_correction(float &diff_press_pa) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + if (_t_system_power == -1) { + _t_system_power = orb_subscribe(ORB_ID(system_power)); + } + if (_t_system_power == -1) { + // not available + return; + } + bool updated = false; + orb_check(_t_system_power, &updated); + if (updated) { + orb_copy(ORB_ID(system_power), _t_system_power, &system_power); + } + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { + // not valid, skip correction + return; + } + + const float slope = 70.0f; + /* + apply a piecewise linear correction, flattening at 0.5V from 5V + */ + float voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -0.5f) { + voltage_diff = -0.5f; + } + diff_press_pa -= voltage_diff * slope; +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +} + /** * Local functions in support of the shell command. */ From 9719af623d92ceab0c5eebe21906b4d5cf515682 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 13:48:37 +1100 Subject: [PATCH 28/55] Merged voltage compensation --- src/drivers/meas_airspeed/meas_airspeed.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 2324475ceb..0b7986383f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -126,7 +126,7 @@ protected: /** * Correct for 5V rail voltage variations */ - void voltage_correction(float &diff_pres_pa); + void voltage_correction(float &diff_pres_pa, float &temperature); int _t_system_power; struct system_power_s system_power; @@ -204,7 +204,7 @@ MEASAirspeed::collect() dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; - float temperature = ((200 * dT_raw) / 2047) - 50; + float temperature = ((200.0f * dT_raw) / 2047) - 50; /* calculate differential pressure. As its centered around 8000 * and can go positive or negative, enforce absolute value @@ -218,7 +218,7 @@ MEASAirspeed::collect() } // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa); + voltage_correction(diff_press_pa, temperature); struct differential_pressure_s report; @@ -308,7 +308,7 @@ MEASAirspeed::cycle() offset versus voltage for 3 sensors */ void -MEASAirspeed::voltage_correction(float &diff_press_pa) +MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 if (_t_system_power == -1) { @@ -340,6 +340,19 @@ MEASAirspeed::voltage_correction(float &diff_press_pa) voltage_diff = -0.5f; } diff_press_pa -= voltage_diff * slope; + + /* + the temperature masurement varies as well + */ + const float temp_slope = 0.887f; + voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -1.0f) { + voltage_diff = -1.0f; + } + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } From cdec5e1d052b587a2e2a5d9d3dfef0bc507cc5b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Feb 2014 14:25:52 +1100 Subject: [PATCH 29/55] Included raw differential pressure field --- src/drivers/meas_airspeed/meas_airspeed.cpp | 49 ++++++++++++++----- .../uORB/topics/differential_pressure.h | 9 ++-- 2 files changed, 43 insertions(+), 15 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 0b7986383f..58b128948f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -206,20 +206,46 @@ MEASAirspeed::collect() dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200.0f * dT_raw) / 2047) - 50; - /* calculate differential pressure. As its centered around 8000 - * and can go positive or negative, enforce absolute value - */ + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + const float PSI_to_Pa = 6894.757f; + /* + this equation is an inversion of the equation in the + pressure transfer function figure on page 4 of the datasheet - if (diff_press_pa < 0.0f) { - diff_press_pa = 0.0f; - } + We negate the result so that positive differential pressures + are generated when the bottom port is used as the static + port on the pitot and top port is used as the dynamic port + */ + float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa, temperature); + voltage_correction(diff_press_pa_raw, temperature); + float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); + + /* + note that we return both the absolute value with offset + applied and a raw value without the offset applied. This + makes it possible for higher level code to detect if the + user has the tubes connected backwards, and also makes it + possible to correctly use offsets calculated by a higher + level airspeed driver. + + With the above calculation the MS4525 sensor will produce a + positive number when the top port is used as a dynamic port + and bottom port is used as the static port + + Also note that the _diff_pres_offset is applied before the + fabsf() not afterwards. It needs to be done this way to + prevent a bias at low speeds, but this also means that when + setting a offset you must set it based on the raw value, not + the offset value + */ + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -232,6 +258,7 @@ MEASAirspeed::collect() report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -328,7 +355,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) return; } - const float slope = 70.0f; + const float slope = 65.0f; /* apply a piecewise linear correction, flattening at 0.5V from 5V */ @@ -478,7 +505,7 @@ test() } warnx("single read"); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -506,7 +533,7 @@ test() } warnx("periodic read %u", i); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index ff88b04c6e..01e14cda9e 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -52,13 +52,14 @@ * Differential pressure. */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - uint64_t error_count; + uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */ + uint64_t error_count; /**< Number of errors detected by driver */ float differential_pressure_pa; /**< Differential pressure reading */ + float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ - float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ + float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ + float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; From 568eb8962d3ab5798a5048047da75b696e0a3af9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 15:42:07 +0400 Subject: [PATCH 30/55] px4io: typos fixed --- src/drivers/px4io/px4io.cpp | 3 +-- src/modules/px4iofirmware/px4io.h | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a30bfb2dec..71aa5a0a4d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -951,8 +951,7 @@ PX4IO::task_main() int32_t failsafe_param_val; param_t failsafe_param = param_find("RC_FAILS_THR"); - if (failsafe_param > 0) - + if (failsafe_param > 0) { param_get(failsafe_param, &failsafe_param_val); uint16_t failsafe_thr = failsafe_param_val; pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 88ad793980..4db9484848 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -108,7 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #endif -#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]; +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] #define r_control_values (&r_page_controls[0]) From 77190f5052405ba8ef10c89f3193802d3d59e66f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 15:42:23 +0400 Subject: [PATCH 31/55] sensors: bug fixed --- src/modules/sensors/sensors.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2c1b1258c5..37255c4bf3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1327,9 +1327,10 @@ Sensors::rc_poll() /* check flags and require at least four channels to consider the signal valid */ if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { /* signal looks good, but check for throttle failsafe */ - if (_parameters.rc_fs_thr == 0 || - !((_parameters.rc_fs_thr < _parameters.min[i] && rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[i] && rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { + int8_t thr_ch = _rc.function[THROTTLE]; + if (_parameters.rc_fs_thr == 0 || thr_ch < 0 || + !((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || + (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr))) { /* valid signal, throttle failsafe not configured or not triggered */ signal_lost = false; } From b00b70aeb2b7f20e74ca185e357e796f54031640 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 16:06:10 +0400 Subject: [PATCH 32/55] manual_control_setpoint: signal_lost flag removed, sensors: bugs fixed --- src/modules/sensors/sensors.cpp | 36 ++++++------------- .../uORB/topics/manual_control_setpoint.h | 2 -- 2 files changed, 10 insertions(+), 28 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 37255c4bf3..5b826a9194 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -214,7 +214,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ - struct manual_control_setpoint_s _manual; /**< manual control setpoint */ struct battery_status_s _battery_status; /**< battery status */ struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; @@ -468,21 +467,6 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); - memset(&_manual, 0, sizeof(_manual)); - - /* set NANs instead of zeroes */ - _manual.roll = NAN; - _manual.pitch = NAN; - _manual.yaw = NAN; - _manual.throttle = NAN; - _manual.flaps = NAN; - _manual.aux1 = NAN; - _manual.aux2 = NAN; - _manual.aux3 = NAN; - _manual.aux4 = NAN; - _manual.aux5 = NAN; - - _manual.signal_lost = true; /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1427,10 +1411,10 @@ Sensors::rc_poll() /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &_manual); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual); } else { - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual); + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } /* copy from mapped manual control to control group 3 */ @@ -1439,14 +1423,14 @@ Sensors::rc_poll() actuator_group_3.timestamp = rc_input.timestamp_publication; - actuator_group_3.control[0] = _manual.roll; - actuator_group_3.control[1] = _manual.pitch; - actuator_group_3.control[2] = _manual.yaw; - actuator_group_3.control[3] = _manual.throttle; - actuator_group_3.control[4] = _manual.flaps; - actuator_group_3.control[5] = _manual.aux1; - actuator_group_3.control[6] = _manual.aux2; - actuator_group_3.control[7] = _manual.aux3; + actuator_group_3.control[0] = manual.roll; + actuator_group_3.control[1] = manual.pitch; + actuator_group_3.control[2] = manual.yaw; + actuator_group_3.control[3] = manual.throttle; + actuator_group_3.control[4] = manual.flaps; + actuator_group_3.control[5] = manual.aux1; + actuator_group_3.control[6] = manual.aux2; + actuator_group_3.control[7] = manual.aux3; /* publish actuator_controls_3 topic */ if (_actuator_group_3_pub > 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 985d3923fb..2b3a337b29 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -61,8 +61,6 @@ typedef enum { struct manual_control_setpoint_s { uint64_t timestamp; - bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ - /** * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. From 3da219c3db638e0a57d18e892575df13d8c11f47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 14:14:03 +0200 Subject: [PATCH 33/55] Update airspeed calibration routine to account for new signedness options --- src/modules/commander/airspeed_calibration.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 6039d92a76..c8c7a42e79 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2500; + const int calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd) if (fd > 0) { if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; + } else { + mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } close(fd); } if (!paramreset_successful) { - warn("WARNING: failed to set scale / offsets for airspeed sensor"); - mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); + warn("FAILED to set scale / offsets for airspeed"); + mavlink_log_critical(mavlink_fd, "dpress reset failed"); mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } @@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; + diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) From be38372be17925e14fa864ad26e8f487a402d46f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 14:28:44 +0200 Subject: [PATCH 34/55] sf0x driver: Stop emitting error messages once there is no hope this sensor will recover - continue to output error messages if the errors are just intermittent --- src/drivers/sf0x/sf0x.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 07163035b5..a0cf98340a 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -124,6 +124,8 @@ private: orb_advert_t _range_finder_topic; + unsigned _consecutive_fail_count; + perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; @@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) : _linebuf_index(0), _last_read(0), _range_finder_topic(-1), + _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows")) @@ -720,12 +723,17 @@ SF0X::cycle() if (OK != collect_ret) { /* we know the sensor needs about four seconds to initialize */ - if (hrt_absolute_time() > 5 * 1000 * 1000LL) { - log("collection error"); + if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { + log("collection error #%u", _consecutive_fail_count); } + _consecutive_fail_count++; + /* restart the measurement state machine */ start(); return; + } else { + /* apparently success */ + _consecutive_fail_count = 0; } /* next phase is measurement */ From 671d35f67ccae5c35ef4fcb573f5fa52395601c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:05:58 +0200 Subject: [PATCH 35/55] Fix logic for S.Bus failsafe detection --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 44a91ca67a..c910062620 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if (_parameters.rc_fs_thr && (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr)))) { + if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { /* do not publish manual control setpoints when there are none */ return; } From de7a6b057f5ced2cf118a919912327f878492cdb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:08:28 +0200 Subject: [PATCH 36/55] Cleanups on system power logging format --- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/sdlog2/sdlog2_messages.h | 64 ++++++++++++---------------- 2 files changed, 29 insertions(+), 37 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9f2fca4465..d2cf6d662b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1248,7 +1248,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy servo rail status topic here too */ orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v; - log_msg.body.log_PWR.servo_rssi_5v = buf.servorail_status.rssi_v; + log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v; LOGBUFFER_WRITE_AND_COUNT(PWR); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d629259c0c..354bb08e8d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -277,6 +277,29 @@ struct log_TELE_s { uint8_t txbuf; }; +/* --- ESTM - ESTIMATOR STATUS --- */ +#define LOG_ESTM_MSG 23 +struct log_ESTM_s { + float s[10]; + uint8_t n_states; + uint8_t states_nan; + uint8_t covariance_nan; + uint8_t kalman_gain_nan; +}; + +/* --- PWR - ONBOARD POWER SYSTEM --- */ +#define LOG_PWR_MSG 24 +struct log_PWR_s { + float peripherals_5v; + float servo_rail_5v; + float servo_rssi; + uint8_t usb_ok; + uint8_t brick_ok; + uint8_t servo_ok; + uint8_t low_power_rail_overcurrent; + uint8_t high_power_rail_overcurrent; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -299,36 +322,6 @@ struct log_PARM_s { float value; }; -/* --- ESTM - ESTIMATOR STATUS --- */ -#define LOG_ESTM_MSG 132 -struct log_ESTM_s { - float s[10]; - uint8_t n_states; - uint8_t states_nan; - uint8_t covariance_nan; - uint8_t kalman_gain_nan; -}; -// struct log_ESTM_s { -// float s[32]; -// uint8_t n_states; -// uint8_t states_nan; -// uint8_t covariance_nan; -// uint8_t kalman_gain_nan; -// }; - -/* --- PWR - ONBOARD POWER SYSTEM --- */ -#define LOG_PWR_MSG 133 -struct log_PWR_s { - float peripherals_5v; - float servo_rail_5v; - float servo_rssi_5v; - uint8_t usb_ok; - uint8_t brick_ok; - uint8_t servo_ok; - uint8_t low_power_rail_overcurrent; - uint8_t high_power_rail_overcurrent; -}; - #pragma pack(pop) /* construct list of all message formats */ @@ -355,17 +348,16 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), /* system-level messages, ID >= 0x80 */ - // FMT: don't write format of format message, it's useless + /* FMT: don't write format of format message, it's useless */ LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), - LOG_FORMAT(PARM, "Nf", "Name,Value"), - LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), - LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), - //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PARM, "Nf", "Name,Value") }; -static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); +static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]); #endif /* SDLOG2_MESSAGES_H_ */ From 7b95d36405cb63b53fd1fea2c25e29aedca5a3a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:45:23 +0200 Subject: [PATCH 37/55] navigator hotfix: Increase acceptance range for yaw setpoints. --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1ba..5a94b66717 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1539,7 +1539,7 @@ Navigator::check_mission_item_reached() /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); - if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } From fc39af08a1d6673aa727a84b17afd6c4485dff19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:02:37 +0200 Subject: [PATCH 38/55] airspeed: Prevent the filter from overshooting into the negative airspeed range --- src/drivers/meas_airspeed/meas_airspeed.cpp | 6 ++++++ .../mathlib/math/filter/LowPassFilter2p.cpp | 7 ++++++- .../mathlib/math/filter/LowPassFilter2p.hpp | 20 +++++++++++++++---- 3 files changed, 28 insertions(+), 5 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 58b128948f..1ad383ee0b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -258,6 +258,12 @@ MEASAirspeed::collect() report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + + /* the dynamics of the filter can make it overshoot into the negative range */ + if (report.differential_pressure_filtered_pa < 0.0f) { + report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); + } + report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 3699d9bce3..6f640c9f9a 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample) // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { - // don't allow bad values to propogate via the filter + // don't allow bad values to propagate via the filter delay_element_0 = sample; } float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; @@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample) return output; } +float LowPassFilter2p::reset(float sample) { + _delay_element_1 = _delay_element_2 = sample; + return apply(sample); +} + } // namespace math diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 208ec98d4e..74cd5d78c0 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -52,18 +52,30 @@ public: _delay_element_1 = _delay_element_2 = 0; } - // change parameters + /** + * Change filter parameters + */ void set_cutoff_frequency(float sample_freq, float cutoff_freq); - // apply - Add a new raw value to the filter - // and retrieve the filtered result + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ float apply(float sample); - // return the cutoff frequency + /** + * Return the cutoff frequency + */ float get_cutoff_freq(void) const { return _cutoff_freq; } + /** + * Reset the filter state to this value + */ + float reset(float sample); + private: float _cutoff_freq; float _a1; From 024de1fec431fbd065aeb31035245e7851450a0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:04:36 +0200 Subject: [PATCH 39/55] Remove unwanted colon --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 88ad793980..4db9484848 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -108,7 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #endif -#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]; +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] #define r_control_values (&r_page_controls[0]) From 262485a5e87ccdc8ff645ba45992bdbe13363fab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:09:48 +0200 Subject: [PATCH 40/55] px4io: Typo fixed --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a30bfb2dec..e318e206ad 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -951,7 +951,7 @@ PX4IO::task_main() int32_t failsafe_param_val; param_t failsafe_param = param_find("RC_FAILS_THR"); - if (failsafe_param > 0) + if (failsafe_param > 0) { param_get(failsafe_param, &failsafe_param_val); uint16_t failsafe_thr = failsafe_param_val; From e6d48c4f3276af252315118286556b3a000274b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:12:21 +0200 Subject: [PATCH 41/55] Fix failsafe assignment in sensors app --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c910062620..b171b995d2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { + if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { /* do not publish manual control setpoints when there are none */ return; } From 6319ec2036c52f39a6fded6480836fe79e3ba35f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:23:34 +0200 Subject: [PATCH 42/55] Add celsius air temperature field to airspeed --- src/modules/uORB/topics/airspeed.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index a3da3758fd..d2ee754cdf 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -52,9 +52,10 @@ * Airspeed */ struct airspeed_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */ }; /** From c17201afbff06f2be278e202d23eb136f38d1ae1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:23:54 +0200 Subject: [PATCH 43/55] Log air temperature --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d2cf6d662b..e62b0fafc6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1193,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_AIRS_MSG; log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; + log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius; LOGBUFFER_WRITE_AND_COUNT(AIRS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 354bb08e8d..a1a5856bc0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -174,6 +174,7 @@ struct log_OUT0_s { struct log_AIRS_s { float indicated_airspeed; float true_airspeed; + float air_temperature_celsius; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ @@ -338,7 +339,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), - LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), + LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), From a1a4013d02ca7ec3f62c6c3f2e4b95181f365c35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:24:11 +0200 Subject: [PATCH 44/55] Populate air temperature field --- src/modules/sensors/sensors.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b171b995d2..4083b8b4f5 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1025,12 +1025,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; - float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celcius); + raw.baro_pres_mbar * 1e2f, air_temperature_celsius); + _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { From 4a0c6600887e900932f6888f1b8948816a1f00b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 21:06:21 +0200 Subject: [PATCH 45/55] Do not make minimum airspeed assumptions, as we can trust our digital sensor a bit. A blocked pitot also most likely will result in more than just 6.5 m/s airspeed and so the check is based on a bunch of weak assumptions --- src/modules/fw_att_control/fw_att_control_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index f139c25f48..522e8c3e6d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -660,14 +660,13 @@ FixedwingAttitudeControl::task_main() float airspeed; - /* if airspeed is smaller than min, the sensor is not giving good readings */ - if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s) || + /* if airspeed is not updating, we assume the normal average speed */ + if (!isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { - airspeed = _airspeed.indicated_airspeed_m_s; + airspeed = _airspeed.true_airspeed_m_s; } float airspeed_scaling = _parameters.airspeed_trim / airspeed; From ab60b13b6dbcf636b1889d2150d96aff8b26cfc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 21:26:17 +0200 Subject: [PATCH 46/55] fw_att_controller: Forcing actuator scaling to at least minimum speed --- src/modules/fw_att_control/fw_att_control_main.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 522e8c3e6d..2f84dc963b 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -669,8 +669,15 @@ FixedwingAttitudeControl::task_main() airspeed = _airspeed.true_airspeed_m_s; } - float airspeed_scaling = _parameters.airspeed_trim / airspeed; - //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; From 7c4f1c90dc441e57d9af55beb33bb6d91ece0c90 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Apr 2014 14:57:45 +0400 Subject: [PATCH 47/55] mavlink_receiver: fixed bug in manual control publication, minor refactoring --- src/modules/mavlink/mavlink_receiver.cpp | 60 ++++++++++++------------ src/modules/mavlink/mavlink_receiver.h | 1 - 2 files changed, 29 insertions(+), 32 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d7e300670a..3ec40ee0a9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -222,12 +222,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = cmd_mavlink.confirmation; - /* check if topic is advertised */ - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { - /* publish */ orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } } @@ -253,7 +251,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) f.quality = flow.quality; f.sensor_id = flow.sensor_id; - if (_flow_pub <= 0) { + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); } else { @@ -287,7 +285,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = 1; - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { @@ -312,7 +310,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) vicon_position.pitch = pos.pitch; vicon_position.yaw = pos.yaw; - if (_vicon_position_pub <= 0) { + if (_vicon_position_pub < 0) { _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); } else { @@ -373,7 +371,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message offboard_control_sp.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub <= 0) { + if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { @@ -401,7 +399,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (_telemetry_status_pub <= 0) { + if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { @@ -428,7 +426,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.chan[2].scaled = man.r / 1000.0f; rc.chan[3].scaled = man.z / 1000.0f; - if (_rc_pub == 0) { + if (_rc_pub < 0) { _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); } else { @@ -450,7 +448,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.yaw = man.r / 1000.0f; manual.throttle = man.z / 1000.0f; - if (_manual_pub == 0) { + if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } else { @@ -619,11 +617,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ - if (_sensors_pub > 0) { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); + if (_sensors_pub < 0) { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } else { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); } } @@ -638,11 +636,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } @@ -694,11 +692,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; - if (_gps_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + if (_gps_pub < 0) { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } else { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); } } @@ -752,11 +750,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; - if (_attitude_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + if (_attitude_pub < 0) { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } else { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); } } @@ -775,11 +773,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; - if (_global_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + if (_global_pos_pub < 0) { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } else { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); } } @@ -816,11 +814,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? hil_local_pos.landed = landed; - if (_local_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); + if (_local_pos_pub < 0) { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); } else { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); } } @@ -857,11 +855,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 0a5a1b5c7a..72ce4560fb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -120,7 +120,6 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; - int _manual_sub; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub; From a4ba705e2f64616e25456c71572661f6b5e7cc3b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Apr 2014 20:04:18 +0400 Subject: [PATCH 48/55] commander: don't use mode switch if it's not mapped --- src/modules/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 73e1a17929..aade0d862e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1514,6 +1514,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin switch (sp_man->mode_switch) { case SWITCH_POS_NONE: + res = TRANSITION_NOT_CHANGED; + break; + case SWITCH_POS_OFF: // MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here From e4cfdb4f9f30e0eef36f9e18e8d656d57994141e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Apr 2014 19:34:32 +0200 Subject: [PATCH 49/55] mavlink: Add manual SP subscription --- src/modules/mavlink/mavlink_receiver.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 72ce4560fb..8ccb2a0354 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -138,6 +138,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + int _manual_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; From c77a7b11628de9ccca20a444bf38582726d1668d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Apr 2014 22:23:33 +0400 Subject: [PATCH 50/55] mavlink_receiver: don't publish rc_channels on manual_control from mavlink, set switches to unmapped state instead of using previous values --- src/modules/mavlink/mavlink_receiver.cpp | 53 +++++------------------- src/modules/mavlink/mavlink_receiver.h | 1 - 2 files changed, 11 insertions(+), 43 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ec40ee0a9..f6f5e4848c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -88,8 +88,6 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), - _manual_sub(-1), - _global_pos_pub(-1), _local_pos_pub(-1), _attitude_pub(-1), @@ -413,47 +411,20 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) mavlink_manual_control_t man; mavlink_msg_manual_control_decode(msg, &man); - /* rc channels */ - { - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); - rc.timestamp = hrt_absolute_time(); - rc.chan_count = 4; + manual.timestamp = hrt_absolute_time(); + manual.roll = man.x / 1000.0f; + manual.pitch = man.y / 1000.0f; + manual.yaw = man.r / 1000.0f; + manual.throttle = man.z / 1000.0f; - rc.chan[0].scaled = man.x / 1000.0f; - rc.chan[1].scaled = man.y / 1000.0f; - rc.chan[2].scaled = man.r / 1000.0f; - rc.chan[3].scaled = man.z / 1000.0f; + if (_manual_pub < 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - if (_rc_pub < 0) { - _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); - - } else { - orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); - } - } - - /* manual control */ - { - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - - /* get a copy first, to prevent altering values that are not sent by the mavlink command */ - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual); - - manual.timestamp = hrt_absolute_time(); - manual.roll = man.x / 1000.0f; - manual.pitch = man.y / 1000.0f; - manual.yaw = man.r / 1000.0f; - manual.throttle = man.z / 1000.0f; - - if (_manual_pub < 0) { - _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - - } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); - } + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); } } @@ -883,8 +854,6 @@ MavlinkReceiver::receive_thread(void *arg) sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); prctl(PR_SET_NAME, thread_name, getpid()); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct pollfd fds[1]; fds[0].fd = uart_fd; fds[0].events = POLLIN; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8ccb2a0354..72ce4560fb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -138,7 +138,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - int _manual_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; From 606660d94a0f2b2968c0bd3a8f035a6db97d9e10 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:07:16 +0400 Subject: [PATCH 51/55] fw_att_control: renamed FW_R_MAX/FW_P_MAX to FW_MAN_R_MAX/FW_MAN_P_MAX --- .../fw_att_control/fw_att_control_main.cpp | 24 +++++++++---------- .../fw_att_control/fw_att_control_params.c | 8 +++---- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 4ab5c19cf8..5276b1c134 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -173,8 +173,8 @@ private: float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float roll_max; /**< Max Roll in rad */ - float pitch_max; /**< Max Pitch in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ } _parameters; /**< local copies of interesting parameters */ @@ -213,8 +213,8 @@ private: param_t trim_yaw; param_t rollsp_offset_deg; param_t pitchsp_offset_deg; - param_t roll_max; - param_t pitch_max; + param_t man_roll_max; + param_t man_pitch_max; } _parameter_handles; /**< handles for interesting parameters */ @@ -358,8 +358,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); - _parameter_handles.roll_max = param_find("FW_R_MAX"); - _parameter_handles.pitch_max = param_find("FW_P_MAX"); + _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); + _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); /* fetch initial parameter values */ parameters_update(); @@ -428,10 +428,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); - param_get(_parameter_handles.roll_max, &(_parameters.roll_max)); - param_get(_parameter_handles.pitch_max, &(_parameters.pitch_max)); - _parameters.roll_max = math::radians(_parameters.roll_max); - _parameters.pitch_max = math::radians(_parameters.pitch_max); + param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); + param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); + _parameters.man_roll_max = math::radians(_parameters.man_roll_max); + _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); /* pitch control parameters */ @@ -717,8 +717,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll * _parameters.roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; - pitch_sp = (-_manual.pitch * _parameters.pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; + pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 14815745cd..aa637e2074 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -187,12 +187,12 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); // @Range -90.0 to 90.0 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); -// @DisplayName Max Roll +// @DisplayName Max Manual Roll // @Description Max roll for manual control in attitude stabilized mode // @Range 0.0 to 90.0 -PARAM_DEFINE_FLOAT(FW_R_MAX, 45.0f); +PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); -// @DisplayName Max Pitch +// @DisplayName Max Manual Pitch // @Description Max pitch for manual control in attitude stabilized mode // @Range 0.0 to 90.0 -PARAM_DEFINE_FLOAT(FW_P_MAX, 45.0f); +PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); From 9a579fa8707361e38c3681b2d23d6bbab1f9c298 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:16:10 +0400 Subject: [PATCH 52/55] mc_att_control: parameters MC_SCALE_XXX renamed to MC_MAN_X_MAX --- .../mc_att_control/mc_att_control_main.cpp | 36 +++++++++---------- .../mc_att_control/mc_att_control_params.c | 12 +++---- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 01f1631a97..6b0c44fb31 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -157,9 +157,9 @@ private: param_t yaw_rate_d; param_t yaw_ff; - param_t man_scale_roll; - param_t man_scale_pitch; - param_t man_scale_yaw; + param_t man_roll_max; + param_t man_pitch_max; + param_t man_yaw_max; } _params_handles; /**< handles for interesting parameters */ struct { @@ -169,9 +169,9 @@ private: math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ - float man_scale_roll; - float man_scale_pitch; - float man_scale_yaw; + float man_roll_max; + float man_pitch_max; + float man_yaw_max; } _params; /** @@ -299,9 +299,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); - _params_handles.man_scale_roll = param_find("MC_SCALE_ROLL"); - _params_handles.man_scale_pitch = param_find("MC_SCALE_PITCH"); - _params_handles.man_scale_yaw = param_find("MC_SCALE_YAW"); + _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); + _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); + _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); /* fetch initial parameter values */ parameters_update(); @@ -369,13 +369,13 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_ff, &_params.yaw_ff); /* manual control scale */ - param_get(_params_handles.man_scale_roll, &_params.man_scale_roll); - param_get(_params_handles.man_scale_pitch, &_params.man_scale_pitch); - param_get(_params_handles.man_scale_yaw, &_params.man_scale_yaw); + param_get(_params_handles.man_roll_max, &_params.man_roll_max); + param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); + param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); - _params.man_scale_roll *= M_PI / 180.0; - _params.man_scale_pitch *= M_PI / 180.0; - _params.man_scale_yaw *= M_PI / 180.0; + _params.man_roll_max *= M_PI / 180.0; + _params.man_pitch_max *= M_PI / 180.0; + _params.man_yaw_max *= M_PI / 180.0; return OK; } @@ -496,7 +496,7 @@ MulticopterAttitudeControl::control_attitude(float dt) //} } else { /* move yaw setpoint */ - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_scale_yaw * dt); + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_yaw_max * dt); _v_att_sp.R_valid = false; publish_att_sp = true; } @@ -511,8 +511,8 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.roll * _params.man_scale_roll; - _v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_scale_pitch; + _v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max; _v_att_sp.R_valid = false; publish_att_sp = true; } diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 9acf8bfa3e..e52c39368b 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -175,30 +175,30 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); /** - * Manual control scaling factor for roll + * Max manual roll * * @unit deg * @min 0.0 * @max 90.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_SCALE_ROLL, 35.0f); +PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f); /** - * Manual control scaling factor for pitch + * Max manual pitch * * @unit deg * @min 0.0 * @max 90.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_SCALE_PITCH, 35.0f); +PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f); /** - * Manual control scaling factor for yaw + * Max manual yaw rate * * @unit deg/s * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_SCALE_YAW, 120.0f); +PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f); From eb5cd54023c39d3adc266d68e02c1f10e77ac62a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:32:37 +0400 Subject: [PATCH 53/55] sensors: lost signal detection rewritten to be more clear --- src/modules/sensors/sensors.cpp | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f20c328d31..91bf92da64 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1307,17 +1307,26 @@ Sensors::rc_poll() orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); /* detect RC signal loss */ - bool signal_lost = true; + bool signal_lost; /* check flags and require at least four channels to consider the signal valid */ - if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { - /* signal looks good, but check for throttle failsafe */ + if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) { + /* signal is lost or no enough channels */ + signal_lost = true; + + } else { + /* signal looks good */ + signal_lost = false; + + /* check throttle failsafe */ int8_t thr_ch = _rc.function[THROTTLE]; - if (_parameters.rc_fs_thr == 0 || thr_ch < 0 || - !((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr))) { - /* valid signal, throttle failsafe not configured or not triggered */ - signal_lost = false; + if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) { + /* throttle failsafe configured */ + if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || + (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) { + /* throttle failsafe triggered, signal is lost by receiver */ + signal_lost = true; + } } } From 9f52c4459331bc0fd32764a35387abb2cab88b4a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:34:14 +0400 Subject: [PATCH 54/55] sensors: use timestamp_last_signal for actuator_group_3 timestamping --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 91bf92da64..ba233dfd00 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1431,7 +1431,7 @@ Sensors::rc_poll() struct actuator_controls_s actuator_group_3; memset(&actuator_group_3, 0 , sizeof(actuator_group_3)); - actuator_group_3.timestamp = rc_input.timestamp_publication; + actuator_group_3.timestamp = rc_input.timestamp_last_signal; actuator_group_3.control[0] = manual.roll; actuator_group_3.control[1] = manual.pitch; From 0b97dd2b776ce61fd53776f036230ea0089e26e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 9 Apr 2014 18:55:55 +0400 Subject: [PATCH 55/55] commander: brackets added to return switch check --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index aade0d862e..d4567c4f11 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1236,7 +1236,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE && + } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && pos_sp_triplet.nav_state == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ status.set_nav_state = NAV_STATE_MISSION;