diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index 682c63e471..0a4f532eb0 100644 Binary files a/Documentation/rc_mode_switch.odg and b/Documentation/rc_mode_switch.odg differ diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf index e795f48705..cbcc59f723 100644 Binary files a/Documentation/rc_mode_switch.pdf and b/Documentation/rc_mode_switch.pdf differ diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 29167f1ede..9489bfd9a2 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -8,10 +8,14 @@ sh /etc/init.d/rc.vtol_defaults set MIXER firefly6 -set MIXER_AUX firefly6 +set PWM_OUT 123456 -set PWM_OUT 12345678 -set PWM_AUX_OUT 1234 +set MIXER_AUX firefly6 set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1000 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + param set VT_MOT_COUNT 6 param set VT_IDLE_PWM_MC 1080 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index efdba9506e..7a424970f6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -45,7 +45,7 @@ then if mixer load $OUTPUT_DEV $MIXER_FILE then - echo "[i] Mixer: $MIXER_FILE" + echo "[i] Mixer: $MIXER_FILE on $OUTPUT_DEV" else echo "[i] Error loading mixer: $MIXER_FILE" tone_alarm $TUNE_ERR @@ -105,7 +105,7 @@ then set MIXER_AUX_FILE none set OUTPUT_AUX_DEV /dev/pwm_output1 - if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ] + if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix ] then set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix else @@ -120,7 +120,12 @@ then then if fmu mode_pwm then - mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE + if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE + then + echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV" + else + echo "[i] Error loading mixer: $MIXER_AUX_FILE" + fi else tone_alarm $TUNE_ERR fi diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index 9ed6eeed91..fda8416403 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -1,4 +1,14 @@ -# mixer for the FireFly6 elevons +# mixer for the FireFly6 tilt mechansim servo, elevons and landing gear +======================================================================= + +Tilt mechanism servo mixer +--------------------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 4 10000 10000 0 -10000 10000 + +Elevon mixers +------------- M: 2 O: 10000 10000 0 -10000 10000 S: 1 0 7500 7500 0 -10000 10000 @@ -8,3 +18,9 @@ M: 2 O: 10000 10000 0 -10000 10000 S: 1 0 7500 7500 0 -10000 10000 S: 1 1 -8000 -8000 0 -10000 10000 + +Landing gear mixer +------------------ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 3 6 10000 10000 0 -10000 10000 diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 77e0ef53db..28e487ea62 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -12,11 +12,11 @@ class DokuWikiTablesOutput(): result += "^ Name ^ Description ^ Min ^ Max ^ Default ^\n" result += "^ ::: ^ Comment ^^^^\n" for param in group.GetParams(): - code = param.GetFieldValue("code") + code = param.GetName() + def_val = param.GetDefault() name = param.GetFieldValue("short_desc") min_val = param.GetFieldValue("min") max_val = param.GetFieldValue("max") - def_val = param.GetFieldValue("default") long_desc = param.GetFieldValue("long_desc") if name == code: diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 891db7ecd2..0d2413a75f 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -37,20 +37,30 @@ class Parameter(object): # Define sorting order of the fields priority = { - "code": 10, - "type": 9, + "board": 9, "short_desc": 8, "long_desc": 7, - "default": 6, "min": 5, "max": 4, "unit": 3, # all others == 0 (sorted alphabetically) } - def __init__(self): + def __init__(self, name, type, default = ""): self.fields = {} + self.name = name + self.type = type + self.default = default + def GetName(self): + return self.name + + def GetType(self): + return self.type + + def GetDefault(self): + return self.default + def SetField(self, code, value): """ Set named field value @@ -88,7 +98,7 @@ class SourceParser(object): re_is_a_number = re.compile(r'^-?[0-9\.]') re_remove_dots = re.compile(r'\.+$') - valid_tags = set(["group", "min", "max", "unit"]) + valid_tags = set(["group", "board", "min", "max", "unit"]) # Order of parameter groups priority = { @@ -177,15 +187,12 @@ class SourceParser(object): # Non-empty line outside the comment m = self.re_parameter_definition.match(line) if m: - tp, code, defval = m.group(1, 2, 3) + tp, name, defval = m.group(1, 2, 3) # Remove trailing type specifier from numbers: 0.1f => 0.1 if self.re_is_a_number.match(defval): defval = self.re_cut_type_specifier.sub('', defval) - param = Parameter() - param.SetField("code", code) - param.SetField("short_desc", code) - param.SetField("type", tp) - param.SetField("default", defval) + param = Parameter(name, tp, defval) + param.SetField("short_desc", name) # If comment was found before the parameter declaration, # inject its data into the newly created parameter. group = "Miscellaneous" @@ -211,11 +218,9 @@ class SourceParser(object): # Nasty code dup, but this will all go away soon, so quick and dirty (DonLakeFlyer) m = self.re_px4_parameter_definition.match(line) if m: - tp, code = m.group(1, 2) - param = Parameter() - param.SetField("code", code) - param.SetField("short_desc", code) - param.SetField("type", tp) + tp, name = m.group(1, 2) + param = Parameter(name, tp) + param.SetField("short_desc", name) # If comment was found before the parameter declaration, # inject its data into the newly created parameter. group = "Miscellaneous" diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index 89f495dc02..07cced4786 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -18,26 +18,36 @@ def indent(elem, level=0): class XMLOutput(): - def __init__(self, groups): + def __init__(self, groups, board): xml_parameters = ET.Element("parameters") xml_version = ET.SubElement(xml_parameters, "version") - xml_version.text = "2" + xml_version.text = "3" + last_param_name = "" + board_specific_param_set = False for group in groups: xml_group = ET.SubElement(xml_parameters, "group") xml_group.attrib["name"] = group.GetName() for param in group.GetParams(): - xml_param = ET.SubElement(xml_group, "parameter") - for code in param.GetFieldCodes(): - value = param.GetFieldValue(code) - if code == "code": - xml_param.attrib["name"] = value - elif code == "default": - xml_param.attrib["default"] = value - elif code == "type": - xml_param.attrib["type"] = value - else: - xml_field = ET.SubElement(xml_param, code) - xml_field.text = value + if (last_param_name == param.GetName() and not board_specific_param_set) or last_param_name != param.GetName(): + xml_param = ET.SubElement(xml_group, "parameter") + xml_param.attrib["name"] = param.GetName() + xml_param.attrib["default"] = param.GetDefault() + xml_param.attrib["type"] = param.GetType() + last_param_name = param.GetName() + for code in param.GetFieldCodes(): + value = param.GetFieldValue(code) + if code == "board": + if value == board: + board_specific_param_set = True + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value + else: + xml_group.remove(xml_param) + else: + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value + if last_param_name != param.GetName(): + board_specific_param_set = False indent(xml_parameters) self.xml_document = ET.ElementTree(xml_parameters) diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index 12128a997e..cb2202d529 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -65,6 +65,11 @@ def main(): metavar="FILENAME", help="Create XML file" " (default FILENAME: parameters.xml)") + parser.add_argument("-b", "--board", + nargs='?', + const="", + metavar="BOARD", + help="Board to create xml parameter xml for") parser.add_argument("-w", "--wiki", nargs='?', const="parameters.wiki", @@ -116,7 +121,7 @@ def main(): # Output to XML file if args.xml: print("Creating XML file " + args.xml) - out = xmlout.XMLOutput(param_groups) + out = xmlout.XMLOutput(param_groups, args.board) out.Save(args.xml) # Output to DokuWiki tables diff --git a/makefiles/nuttx_romfs.mk b/makefiles/nuttx_romfs.mk index ca6fefe29e..42aeb3b8d4 100644 --- a/makefiles/nuttx_romfs.mk +++ b/makefiles/nuttx_romfs.mk @@ -180,7 +180,7 @@ firmware: $(PRODUCT_BUNDLE) $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @$(ECHO) %% Generating $@ ifdef GEN_PARAM_XML - python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml + python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ --git_identity $(PX4_BASE) \ --parameter_xml $(PRODUCT_PARAMXML) \ diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index d44728a712..a24d8814fe 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -65,7 +65,7 @@ /** * Maximum RSSI value */ -#define RC_INPUT_RSSI_MAX 255 +#define RC_INPUT_RSSI_MAX 100 /** * @addtogroup topics diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1be561dc6e..a493813388 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -303,6 +303,10 @@ private: uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled + int32_t _rssi_pwm_chan; ///< RSSI PWM input channel + int32_t _rssi_pwm_max; ///< max RSSI input on PWM channel + int32_t _rssi_pwm_min; ///< min RSSI input on PWM channel + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power #endif @@ -524,7 +528,10 @@ PX4IO::PX4IO(device::Device *interface) : _battery_amp_bias(0), _battery_mamphour_total(0), _battery_last_timestamp(0), - _cb_flighttermination(true) + _cb_flighttermination(true), + _rssi_pwm_chan(0), + _rssi_pwm_max(0), + _rssi_pwm_min(0) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 , _dsm_vcc_ctl(false) #endif @@ -664,6 +671,10 @@ PX4IO::init() if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; + param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); + param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); + param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); + /* * Check for IO flight state - if FMU was flagged to be in * armed state, FMU is recovering from an in-air reset. @@ -1069,6 +1080,10 @@ PX4IO::task_main() /* Update Circuit breakers */ _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); + param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); + param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); + } } @@ -1633,6 +1648,15 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) input_rc.values[i] = regs[prolog + i]; } + /* get RSSI from input channel */ + if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { + int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) / + ((_rssi_pwm_max - _rssi_pwm_min) / 100); + rssi = rssi > 100 ? 100 : rssi; + rssi = rssi < 0 ? 0 : rssi; + input_rc.rssi = rssi; + } + return ret; } diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.c index a98c986bb7..cea7790ec1 100644 --- a/src/lib/rc/sumd.c +++ b/src/lib/rc/sumd.c @@ -269,7 +269,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe uint8_t _cnt = *rx_count + 1; *rx_count = _cnt; - *rssi = 255; + *rssi = 100; /* received Channels */ if ((uint16_t)_rxpacket.length > max_chan_count) { diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 97873e462c..45dc4309ba 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1127,6 +1127,10 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; thread_running = true; + /* update vehicle status to find out vehicle type (required for preflight checks) */ + param_get(_param_sys_type, &(status.system_type)); // get system type + status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status); + bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ @@ -1206,15 +1210,7 @@ int commander_thread_main(int argc, char *argv[]) } /* disable manual override for all systems that rely on electronic stabilization */ - if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL || - status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER || - status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER || - status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR || - status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR || - status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR || - (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || - (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { - + if (is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode)) { status.is_rotary_wing = true; } else { @@ -1222,8 +1218,7 @@ int commander_thread_main(int argc, char *argv[]) } /* set vehicle_status.is_vtol flag */ - status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || - (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR); + status.is_vtol = is_vtol(&status); /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); @@ -1424,8 +1419,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; - /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ - if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) { + /* Make sure that this is only adjusted if vehicle really is of type vtol*/ + if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; } } @@ -1604,7 +1599,11 @@ int commander_thread_main(int argc, char *argv[]) if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; + mavlink_and_console_log_critical(mavlink_fd, "LOW BATTERY, LOCKING ARMING DOWN"); } + + } else { + mavlink_and_console_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); } status_changed = true; @@ -2222,13 +2221,34 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); + /* mode rejected, continue to evaluate the main system mode */ } else { + /* changed successfully or already in this state */ return res; } } - /* offboard switched off or denied, check main mode switch */ + /* RTL switch overrides main switch */ + if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); + + if (res == TRANSITION_DENIED) { + print_reject_mode(status_local, "AUTO_RTL"); + + /* fallback to LOITER if home position not set */ + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); + } + + if (res != TRANSITION_DENIED) { + /* changed successfully or already in this state */ + return res; + } + + /* if we get here mode was rejected, continue to evaluate the main system mode */ + } + + /* offboard and RTL switches off or denied, check main mode switch */ switch (sp_man->mode_switch) { case manual_control_setpoint_s::SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; @@ -2273,23 +2293,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO - if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - print_reject_mode(status_local, "AUTO_RTL"); - - // fallback to LOITER if home position not set - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - } else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index e2680bf9ad..161d516f7b 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -86,6 +86,11 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL); } +bool is_vtol(const struct vehicle_status_s * current_status) { + return current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR || + current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR; +} + static int buzzer = -1; static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 0cefedba7c..bf0c0505d2 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -51,6 +51,7 @@ bool is_multirotor(const struct vehicle_status_s *current_status); bool is_rotary_wing(const struct vehicle_status_s *current_status); +bool is_vtol(const struct vehicle_status_s *current_status); int buzzer_init(void); void buzzer_deinit(void); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7d6b60e227..5adfea36b6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1867,29 +1867,8 @@ protected: struct rc_input_values rc; if (_rc_sub->update(&_rc_time, &rc)) { - const unsigned port_width = 8; - /* deprecated message (but still needed for compatibility!) */ - for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { - /* channels are sent in MAVLink main loop at a fixed interval */ - mavlink_rc_channels_raw_t msg; - - msg.time_boot_ms = rc.timestamp_publication / 1000; - msg.port = i; - msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX; - msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; - msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; - msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; - msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; - msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; - msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; - msg.rssi = rc.rssi; - - _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); - } - - /* new message */ + /* send RC channel data and RSSI */ mavlink_rc_channels_t msg; msg.time_boot_ms = rc.timestamp_publication / 1000; @@ -1913,55 +1892,7 @@ protected: msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; - /* RSSI has a max value of 100, and when Spektrum or S.BUS are - * available, the RSSI field is invalid, as they do not provide - * an RSSI measurement. Use an out of band magic value to signal - * these digital ports. XXX revise MAVLink spec to address this. - * One option would be to use the top bit to toggle between RSSI - * and input source mode. - * - * Full RSSI field: 0b 1 111 1111 - * - * ^ If bit is set, RSSI encodes type + RSSI - * - * ^ These three bits encode a total of 8 - * digital RC input types. - * 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24 - * ^ These four bits encode a total of - * 16 RSSI levels. 15 = full, 0 = no signal - * - */ - - /* Initialize RSSI with the special mode level flag */ - msg.rssi = (1 << 7); - - /* Set RSSI */ - msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; - - switch (rc.input_source) { - case RC_INPUT_SOURCE_PX4FMU_PPM: - /* fallthrough */ - case RC_INPUT_SOURCE_PX4IO_PPM: - msg.rssi |= (0 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: - msg.rssi |= (1 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_SBUS: - msg.rssi |= (2 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_ST24: - msg.rssi |= (3 << 4); - break; - case RC_INPUT_SOURCE_UNKNOWN: - // do nothing - break; - } - - if (rc.rc_lost) { - /* RSSI is by definition zero */ - msg.rssi = 0; - } + msg.rssi = rc.rssi; _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 547e5f43ad..a0722ac4d6 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -99,6 +99,9 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool if (*st24_updated) { + /* ensure ADC RSSI is disabled */ + r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI); + *rssi = st24_rssi; r_raw_rc_count = st24_channel_count; @@ -116,14 +119,14 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ - st24_rssi = RC_INPUT_RSSI_MAX; + sumd_rssi = RC_INPUT_RSSI_MAX; *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*sumd_updated) { - *rssi = sumd_rssi; + /* not setting RSSI since SUMD does not provide one */ r_raw_rc_count = sumd_channel_count; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; @@ -187,12 +190,20 @@ controls_tick() { /* use 1:1 scaling on 3.3V ADC input */ unsigned mV = counts * 3300 / 4096; - /* scale to 0..253 */ - rssi = mV / 13; + /* scale to 0..253 and lowpass */ + rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f); + if (rssi > RC_INPUT_RSSI_MAX) { + rssi = RC_INPUT_RSSI_MAX; + } } } #endif + /* zero RSSI if signal is lost */ + if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_OK))) { + rssi = 0; + } + perf_begin(c_gather_dsm); bool dsm_updated, st24_updated, sumd_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); @@ -215,22 +226,26 @@ controls_tick() { if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - rssi = 255; + unsigned sbus_rssi = RC_INPUT_RSSI_MAX; if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; - rssi = 100; + sbus_rssi = RC_INPUT_RSSI_MAX / 2; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } if (sbus_failsafe) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; - rssi = 0; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } + /* set RSSI to an emulated value if ADC RSSI is off */ + if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + rssi = sbus_rssi; + } + } perf_end(c_gather_sbus); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 12d50809cf..6a127df0ea 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -960,6 +960,7 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); /** * Scaling factor for battery voltage sensor on FMU v2. * + * @board CONFIG_ARCH_BOARD_PX4FMU_V2 * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); @@ -969,6 +970,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); * * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063 * + * @board CONFIG_ARCH_BOARD_AEROCORE * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f); @@ -1360,3 +1362,42 @@ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f); * */ PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f); + +/** + * PWM input channel that provides RSSI. + * + * 0: do not read RSSI from input channel + * 1-18: read RSSI from specified input channel + * + * Specify the range for RSSI input with RC_RSSI_PWM_MIN and RC_RSSI_PWM_MAX parameters. + * + * @min 0 + * @max 18 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_CHAN, 0); + +/** + * Max input value for RSSI reading. + * + * Only used if RC_RSSI_PWM_CHAN > 0 + * + * @min 0 + * @max 2000 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 1000); + +/** + * Min input value for RSSI reading. + * + * Only used if RC_RSSI_PWM_CHAN > 0 + * + * @min 0 + * @max 2000 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000); diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index e499ae27ac..e5cc034bc9 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -43,7 +43,6 @@ */ #include -#include /** * Circuit breaker for power supply check @@ -56,7 +55,7 @@ * @max 894281 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); +PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); /** * Circuit breaker for rate controller output @@ -69,7 +68,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); * @max 140253 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); +PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); /** * Circuit breaker for IO safety @@ -81,7 +80,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); * @max 22027 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); +PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); /** * Circuit breaker for airspeed sensor @@ -93,7 +92,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); * @max 162128 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); +PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); /** * Circuit breaker for flight termination @@ -106,7 +105,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); * @max 121212 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); /** * Circuit breaker for engine failure detection @@ -120,4 +119,4 @@ PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); * @max 284953 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL); +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h deleted file mode 100644 index 768bf7f533..0000000000 --- a/src/modules/systemlib/circuit_breaker_params.h +++ /dev/null @@ -1,7 +0,0 @@ -#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0 -#define PARAM_CBRK_RATE_CTRL_DEFAULT 0 -#define PARAM_CBRK_IO_SAFETY_DEFAULT 0 -#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0 -#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212 -#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953 -#define PARAM_CBRK_GPSFAIL_DEFAULT 240024 diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 4a1f5c1f1f..5fd7985d04 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -176,7 +176,7 @@ private: bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" unsigned _motor_count; // number of motors float _airspeed_tot; - + float _tilt_control; //*****************Member functions*********************************************************************** void task_main(); //main task @@ -241,6 +241,7 @@ VtolAttitudeControl::VtolAttitudeControl() : flag_idle_mc = true; _airspeed_tot = 0.0f; + _tilt_control = 0.0f; memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -521,6 +522,7 @@ void VtolAttitudeControl::fill_mc_att_control_output() //set neutral position for elevons _actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon _actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon + _actuators_out_1.control[4] = _tilt_control; // for tilt-rotor control } /** @@ -763,7 +765,7 @@ void VtolAttitudeControl::task_main() vehicle_battery_poll(); - if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ + if (_manual_control_sp.aux1 < 0.0f) { /* vehicle is in mc mode */ _vtol_vehicle_status.vtol_in_rw_mode = true; if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */