From 367ce63b86b863d72a3f6b4250d9da780a85f40b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 11:45:57 +0400 Subject: '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 | 224 ++++++++++++---------- src/modules/uORB/topics/manual_control_setpoint.h | 2 + src/modules/uORB/topics/rc_channels.h | 1 + 4 files changed, 123 insertions(+), 117 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e..edd77231d 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 f890c4c7f..9f816f5e1 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); + + /* 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) - return; + } 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; + if (!manual_control.signal_lost) { + _rc_last_valid = 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; + _rc.timestamp = _rc_last_valid; - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + manual_control.timestamp = _rc_last_valid; - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + if (!manual_control.signal_lost) { + /* fill values in manual_control_setpoint topic only if signal is valid */ - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; - } + /* 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_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - /* flaps */ - if (_rc.function[FLAPS] >= 0) { + /* scale output */ + if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { + manual_control.roll *= _parameters.rc_scale_roll; + } - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); + if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; + if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { + manual_control.yaw *= _parameters.rc_scale_yaw; } - } - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } + /* flaps */ + if (_rc.function[FLAPS] >= 0) { - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } + manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } + if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { + manual_control.flaps *= _parameters.rc_scale_flaps; + } + } - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } + /* mode switch input */ + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].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); -// } + /* assisted switch input */ + if (_rc.function[ASSISTED] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].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); - } + /* mission switch input */ + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); + } - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].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[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].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); + // } - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].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_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].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); + } } - /* 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 eac6d6e98..5d1384380 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 6eb20efd1..3246a39dd 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. */ /** -- cgit v1.2.3 From 2c4792d48ee98cf46d9f9cf8ec43a759d6cc15d0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 11:46:21 +0400 Subject: 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(-) (limited to 'src/modules') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 13981ac54..112a4bd3e 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 2b41755de..de12e623a 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"), -- cgit v1.2.3 From 1d5f62d890d1d85cef5e0f8e282d8e9e70717d46 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 17:26:07 +0400 Subject: sensors: use enum for switches position and -1..1 for values in 'manual_control_setpoint' topic --- src/modules/sensors/sensors.cpp | 165 ++++++++++------------ src/modules/uORB/topics/manual_control_setpoint.h | 37 ++--- 2 files changed, 93 insertions(+), 109 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9f816f5e1..e08d8618f 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; - - 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); - } + /* 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); + + /* 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 5d1384380..985d3923f 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 */ /** -- cgit v1.2.3 From 6f38ed3b4b6f266098d0616b6bd3c18ffe082755 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 20:23:34 +0400 Subject: commander, navigator: use updated manual_control_setpoint --- src/modules/commander/commander.cpp | 136 ++++++++++++++----------------- src/modules/navigator/navigator_main.cpp | 100 ++++++++--------------- src/modules/uORB/topics/vehicle_status.h | 28 ------- 3 files changed, 95 insertions(+), 169 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index edd77231d..7e469c9c1 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 c45cafc1b..b7f26198b 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.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 */ + 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; + + 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 56be4d241..48af4c9e2 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; -- cgit v1.2.3 From e2ac5222d812bdbfaf33fc2d320ee22ab861d433 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 20:54:28 +0400 Subject: mc_att_control, mc_pos_control: update manual_control_setpoint usage --- src/modules/mc_att_control/mc_att_control_main.cpp | 54 +++++++++++----------- src/modules/mc_att_control/mc_att_control_params.c | 29 ++++++++++++ src/modules/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(-) (limited to 'src/modules') 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 9cb8e8344..01f1631a9 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 488107d58..9acf8bfa3 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 78d06ba5b..2bd2d356a 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 288c6e00a..09afaf949 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 e08d8618f..8f350751c 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)); -- cgit v1.2.3 From ef8b97437342401a464bcc844d6c347c33919d73 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 22:58:57 +0400 Subject: fw_att_control: update manual_control_setpoint usage --- src/modules/fw_att_control/fw_att_control_main.cpp | 17 ++++++++++++++--- src/modules/fw_att_control/fw_att_control_params.c | 10 ++++++++++ 2 files changed, 24 insertions(+), 3 deletions(-) (limited to 'src/modules') 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 f139c25f4..4464b5e01 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 c80a44f2a..14815745c 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); -- cgit v1.2.3 From 3641faed0c696f1a88f88a17b5666ed5c3ba8b1c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 23:18:43 +0400 Subject: sensors: publish last valid manual control values when signal lost --- src/modules/sensors/sensors.cpp | 113 ++++++++++++++++++---------------------- 1 file changed, 51 insertions(+), 62 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8f350751c..669f4e174 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; - - if (!manual_control.signal_lost) { - _rc_last_valid = rc_input.timestamp_last_signal; - } - - _rc.timestamp = _rc_last_valid; + _rc.signal_lost = _manual.signal_lost; - 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); } } -- cgit v1.2.3 From 60355b4e6c0a6916084442d0557cacd74b008b82 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 23:47:09 +0400 Subject: sensors: switch position reading bug fixed --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 669f4e174..79b53a79c 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 { -- cgit v1.2.3 From 568eb8962d3ab5798a5048047da75b696e0a3af9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 15:42:07 +0400 Subject: px4io: typos fixed --- src/drivers/px4io/px4io.cpp | 3 +-- src/modules/px4iofirmware/px4io.h | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a30bfb2de..71aa5a0a4 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 88ad79398..4db948484 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]) -- cgit v1.2.3 From 77190f5052405ba8ef10c89f3193802d3d59e66f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 15:42:23 +0400 Subject: sensors: bug fixed --- src/modules/sensors/sensors.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2c1b1258c..37255c4bf 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; } -- cgit v1.2.3 From b00b70aeb2b7f20e74ca185e357e796f54031640 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 16:06:10 +0400 Subject: manual_control_setpoint: signal_lost flag removed, sensors: bugs fixed --- src/modules/sensors/sensors.cpp | 36 +++++++---------------- src/modules/uORB/topics/manual_control_setpoint.h | 2 -- 2 files changed, 10 insertions(+), 28 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 37255c4bf..5b826a919 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 985d3923f..2b3a337b2 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. -- cgit v1.2.3 From a4ba705e2f64616e25456c71572661f6b5e7cc3b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Apr 2014 20:04:18 +0400 Subject: commander: don't use mode switch if it's not mapped --- src/modules/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 73e1a1792..aade0d862 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 -- cgit v1.2.3 From c77a7b11628de9ccca20a444bf38582726d1668d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Apr 2014 22:23:33 +0400 Subject: 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(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ec40ee0a..f6f5e4848 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)); - - rc.timestamp = hrt_absolute_time(); - rc.chan_count = 4; - - 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 (_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); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(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; + 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); + 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 8ccb2a035..72ce4560f 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; -- cgit v1.2.3 From 606660d94a0f2b2968c0bd3a8f035a6db97d9e10 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:07:16 +0400 Subject: fw_att_control: renamed FW_R_MAX/FW_P_MAX to FW_MAN_R_MAX/FW_MAN_P_MAX --- src/modules/fw_att_control/fw_att_control_main.cpp | 24 +++++++++++----------- src/modules/fw_att_control/fw_att_control_params.c | 8 ++++---- 2 files changed, 16 insertions(+), 16 deletions(-) (limited to 'src/modules') 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 4ab5c19cf..5276b1c13 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 14815745c..aa637e207 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); -- cgit v1.2.3 From 9a579fa8707361e38c3681b2d23d6bbab1f9c298 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:16:10 +0400 Subject: mc_att_control: parameters MC_SCALE_XXX renamed to MC_MAN_X_MAX --- src/modules/mc_att_control/mc_att_control_main.cpp | 36 +++++++++++----------- src/modules/mc_att_control/mc_att_control_params.c | 12 ++++---- 2 files changed, 24 insertions(+), 24 deletions(-) (limited to 'src/modules') 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 01f1631a9..6b0c44fb3 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 9acf8bfa3..e52c39368 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); -- cgit v1.2.3 From eb5cd54023c39d3adc266d68e02c1f10e77ac62a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:32:37 +0400 Subject: sensors: lost signal detection rewritten to be more clear --- src/modules/sensors/sensors.cpp | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f20c328d3..91bf92da6 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; + } } } -- cgit v1.2.3 From 9f52c4459331bc0fd32764a35387abb2cab88b4a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:34:14 +0400 Subject: sensors: use timestamp_last_signal for actuator_group_3 timestamping --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 91bf92da6..ba233dfd0 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; -- cgit v1.2.3 From 0b97dd2b776ce61fd53776f036230ea0089e26e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 9 Apr 2014 18:55:55 +0400 Subject: commander: brackets added to return switch check --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index aade0d862..d4567c4f1 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; -- cgit v1.2.3