Merged in f5soh/librepilot/LP-407_Add_RSSI_Channel_input (pull request #325)

LP-407 Add Rssi channel input

flight/modules/Receiver/receiver.c

@@ -114,7 +114,8 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
 static void resetRcvrStatus(struct rcvr_activity_fsm *fsm);
 static bool updateRcvrStatus(
     struct rcvr_activity_fsm *fsm,
-    ManualControlSettingsChannelGroupsOptions group);
+    ManualControlSettingsChannelGroupsOptions group,
+    int8_t rssiValue);
 
 #define assumptions \
     ( \
@@ -209,6 +210,9 @@ static void receiverTask(__attribute__((unused)) void *parameters)
     AccessoryDesiredCreateInstance();
     AccessoryDesiredCreateInstance();
 
+    // Rssi input
+    AccessoryDesiredCreateInstance();
+
     // Whenever the configuration changes, make sure it is safe to fly
 
     ManualControlCommandGet(&cmd);
@@ -232,6 +236,8 @@ static void receiverTask(__attribute__((unused)) void *parameters)
         PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
 #endif
 
+        int8_t rssiValue = -1;
+
         // Read settings
         ManualControlSettingsGet(&settings);
         SystemSettingsThrustControlGet(&thrustType);
@@ -244,7 +250,8 @@ static void receiverTask(__attribute__((unused)) void *parameters)
             }
             /* Read signal quality from the group used for the throttle */
             (void)updateRcvrStatus(&activity_fsm,
-                                   settings.ChannelGroups.Throttle);
+                                   settings.ChannelGroups.Throttle,
+                                   rssiValue);
         }
         if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
             resetRcvrActivity(&activity_fsm);
@@ -267,6 +274,7 @@ static void receiverTask(__attribute__((unused)) void *parameters)
         }
 
         bool valid_input_detected = true;
+        bool valid_rssi_input     = false;
 
         // Read channel values in us
         for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
@@ -285,16 +293,27 @@ static void receiverTask(__attribute__((unused)) void *parameters)
             if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
                 valid_input_detected = false;
             } else {
+                int16_t neutralValue = ManualControlSettingsChannelNeutralToArray(settings.ChannelNeutral)[n];
+
+                if (n == MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI) {
+                    // Rssi neutral is ignored and set to min, for 0 - 100% output range
+                    neutralValue     = ManualControlSettingsChannelMinToArray(settings.ChannelMin)[n];
+                    valid_rssi_input = true;
+                }
                 scaledChannel[n] = scaleChannel(cmd.Channel[n],
                                                 ManualControlSettingsChannelMaxToArray(settings.ChannelMax)[n],
                                                 ManualControlSettingsChannelMinToArray(settings.ChannelMin)[n],
-                                                ManualControlSettingsChannelNeutralToArray(settings.ChannelNeutral)[n]);
+                                                neutralValue);
             }
         }
 
+        if (valid_rssi_input) {
+            rssiValue = (int8_t)(scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI] * 100);
+        }
         /* Read signal quality from the group used for the throttle */
         (void)updateRcvrStatus(&activity_fsm,
-                               settings.ChannelGroups.Throttle);
+                               settings.ChannelGroups.Throttle,
+                               rssiValue);
 
         // Sanity Check Throttle and Yaw
         if (settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
@@ -383,6 +402,10 @@ static void receiverTask(__attribute__((unused)) void *parameters)
             valid_input_detected &= validInputRange(settings.ChannelMin.Accessory3,
                                                     settings.ChannelMax.Accessory3, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY3]);
         }
+        if (settings.ChannelGroups.Rssi != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+            valid_input_detected &= validInputRange(settings.ChannelMin.Rssi,
+                                                    settings.ChannelMax.Rssi, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI]);
+        }
 
         // Implement hysteresis loop on connection status
         if (valid_input_detected && (++connected_count > 10)) {
@@ -569,6 +592,18 @@ static void receiverTask(__attribute__((unused)) void *parameters)
                     AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
                 }
             }
+            // Set Rssi input
+            if (settings.ChannelGroups.Rssi != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+                accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI];
+#ifdef USE_INPUT_LPF
+                // Allow some Rssi filtering without deadband
+                applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_RSSI, &settings.ResponseTime, 0.0f, dT);
+#endif
+
+                if (AccessoryDesiredInstSet(4, &accessory) != 0) {
+                    AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
+                }
+            }
         }
 
         // Update cmd object
@@ -625,6 +660,9 @@ static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8
 static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm)
 {
     bool activity_updated = false;
+    ManualControlSettingsData settings;
+
+    ManualControlSettingsGet(&settings);
 
     /* Compare the current value to the previous sampled value */
     for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
@@ -637,6 +675,13 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm
             delta = prev - curr;
         }
 
+        // Ignore activity from this Group/Channel because already used/set for Rssi input
+        // Without that, the ReceiverActivity will be saturated just with Rssi value activity.
+        if ((ManualControlSettingsChannelGroupsToArray(settings.ChannelGroups)[MANUALCONTROLSETTINGS_CHANNELGROUPS_RSSI] == fsm->group) &&
+            (ManualControlSettingsChannelNumberToArray(settings.ChannelNumber)[MANUALCONTROLSETTINGS_CHANNELNUMBER_RSSI] == channel)) {
+            delta = 0;
+        }
+
         if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
             /* Mark this channel as active */
             ReceiverActivityActiveGroupOptions group;
@@ -748,16 +793,16 @@ group_completed:
     return activity_updated;
 }
 
-/* Read signal quality from the specified group */
+/* Read signal quality from the specified group or use Rssi input value if any */
 static bool updateRcvrStatus(
     struct rcvr_activity_fsm *fsm,
-    ManualControlSettingsChannelGroupsOptions group)
+    ManualControlSettingsChannelGroupsOptions group, int8_t rssiValue)
 {
     extern uint32_t pios_rcvr_group_map[];
     bool activity_updated = false;
     int8_t quality;
 
-    quality = PIOS_RCVR_GetQuality(pios_rcvr_group_map[group]);
+    quality = (rssiValue > 0) ? rssiValue : PIOS_RCVR_GetQuality(pios_rcvr_group_map[group]);
 
     /* If no driver is detected or any other error then return */
     if (quality < 0) {

flight/pios/common/pios_sbus.c

@@ -125,6 +125,7 @@ static void PIOS_SBus_ResetChannels(struct pios_sbus_state *state)
 {
     for (int i = 0; i < PIOS_SBUS_NUM_INPUTS; i++) {
         state->channel_data[i] = PIOS_RCVR_TIMEOUT;
+        state->quality = 0.0f;
     }
 }
 
@@ -302,6 +303,7 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
             }
 #ifndef SBUS_GOOD_FRAME_COUNT
             /* Present quality as a weighted average of good frames */
+            // TODO: Refactor quality computation, give 4% (quality_trend / SBUS_FL_WEIGHTED_AVE) at minimum
             state->quality = ((state->quality * (SBUS_FL_WEIGHTED_AVE - 1)) +
                               quality_trend) / SBUS_FL_WEIGHTED_AVE;
 #endif /* SBUS_GOOD_FRAME_COUNT */

ground/gcs/src/plugins/config/configinputwidget.cpp

@@ -83,7 +83,8 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
     accessoryDesiredObj0(NULL),
     accessoryDesiredObj1(NULL),
     accessoryDesiredObj2(NULL),
-    accessoryDesiredObj3(NULL)
+    accessoryDesiredObj3(NULL),
+    rssiDesiredObj4(NULL)
 {
     manualCommandObj      = ManualControlCommand::GetInstance(getObjectManager());
     manualSettingsObj     = ManualControlSettings::GetInstance(getObjectManager());
@@ -94,6 +95,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
     accessoryDesiredObj1  = AccessoryDesired::GetInstance(getObjectManager(), 1);
     accessoryDesiredObj2  = AccessoryDesired::GetInstance(getObjectManager(), 2);
     accessoryDesiredObj3  = AccessoryDesired::GetInstance(getObjectManager(), 3);
+    rssiDesiredObj4 = AccessoryDesired::GetInstance(getObjectManager(), 4);
     actuatorSettingsObj   = ActuatorSettings::GetInstance(getObjectManager());
     systemSettingsObj     = SystemSettings::GetInstance(getObjectManager());
 
@@ -138,7 +140,12 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
         // Slider position based on real time Rcinput (allow monitoring)
         addWidgetBinding("ManualControlCommand", "Channel", inputChannelForm->ui->channelNeutral, index);
         // Neutral value stored on board (SpinBox)
-        addWidgetBinding("ManualControlSettings", "ChannelNeutral", inputChannelForm->ui->neutralValue, index);
+        // Rssi neutral is always set to min
+        if (index == ManualControlSettings::CHANNELGROUPS_RSSI) {
+            addWidgetBinding("ManualControlSettings", "ChannelMin", inputChannelForm->ui->neutralValue, index);
+        } else {
+            addWidgetBinding("ManualControlSettings", "ChannelNeutral", inputChannelForm->ui->neutralValue, index);
+        }
         addWidgetBinding("ManualControlSettings", "ChannelMax", inputChannelForm->ui->channelMax, index);
         addWidgetBinding("ManualControlSettings", "ChannelMin", inputChannelForm->ui->channelMin, index);
         addWidgetBinding("ManualControlSettings", "ChannelMax", inputChannelForm->ui->channelMax, index);
@@ -153,7 +160,10 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
         inputChannelForm->ui->channelRev->setVisible(reversable);
 
         // Input filter response time fields supported for some channels only
+        // Set Neutral field to readonly for Rssi
         switch (index) {
+        case ManualControlSettings::CHANNELGROUPS_RSSI:
+            inputChannelForm->ui->neutralValue->setReadOnly(true);
         case ManualControlSettings::CHANNELGROUPS_ROLL:
         case ManualControlSettings::CHANNELGROUPS_PITCH:
         case ManualControlSettings::CHANNELGROUPS_YAW:
@@ -832,7 +842,7 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
     {
         setTxMovement(nothing);
         manualSettingsData = manualSettingsObj->getData();
-        for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) {
+        for (uint i = 0; i < ManualControlSettings::CHANNELNUMBER_RSSI; ++i) {
             // Preserve the inverted status
             if (manualSettingsData.ChannelMin[i] <= manualSettingsData.ChannelMax[i]) {
                 manualSettingsData.ChannelMin[i] = manualSettingsData.ChannelNeutral[i];
@@ -937,7 +947,8 @@ void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step)
     case wizardIdentifyCenter:
         manualCommandData  = manualCommandObj->getData();
         manualSettingsData = manualSettingsObj->getData();
-        for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; ++i) {
+        // Ignore last Rssi channel
+        for (unsigned int i = 0; i < ManualControlSettings::CHANNELNUMBER_RSSI; ++i) {
             // Set Accessory neutral to middle range
             if (i >= ManualControlSettings::CHANNELNUMBER_ACCESSORY0) {
                 manualSettingsData.ChannelNeutral[i] = manualSettingsData.ChannelMin[i] + ((manualSettingsData.ChannelMax[i] - manualSettingsData.ChannelMin[i]) / 2);
@@ -1195,7 +1206,7 @@ void ConfigInputWidget::identifyLimits()
     bool newFlightModeValue = false;
 
     manualCommandData = manualCommandObj->getData();
-    for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) {
+    for (uint i = 0; i < ManualControlSettings::CHANNELNUMBER_RSSI; ++i) {
         inputValue = manualCommandData.Channel[i];
         // Check if channel is already detected and prevent glitches
         if ((manualSettingsData.ChannelNumber[i] != CHANNEL_NUMBER_NONE) &&
@@ -1934,7 +1945,7 @@ void ConfigInputWidget::updateConfigAlarmStatus()
 void ConfigInputWidget::updateCalibration()
 {
     manualCommandData = manualCommandObj->getData();
-    for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) {
+    for (uint i = 0; i < ManualControlSettings::CHANNELNUMBER_RSSI; ++i) {
         if ((!reverse[i] && manualSettingsData.ChannelMin[i] > manualCommandData.Channel[i]) ||
             (reverse[i] && manualSettingsData.ChannelMin[i] < manualCommandData.Channel[i])) {
             manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i];
@@ -1984,8 +1995,8 @@ void ConfigInputWidget::simpleCalibration(bool enable)
         flightModeSettingsData = flightModeSettingsObj->getData();
         flightModeSettingsData.Arming = FlightModeSettings::ARMING_ALWAYSDISARMED;
         flightModeSettingsObj->setData(flightModeSettingsData);
-
+        // Ignore last Rssi channel
-        for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) {
+        for (unsigned int i = 0; i < ManualControlSettings::CHANNELNUMBER_RSSI; i++) {
             reverse[i] = manualSettingsData.ChannelMax[i] < manualSettingsData.ChannelMin[i];
             manualSettingsData.ChannelMin[i]     = manualCommandData.Channel[i];
             manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i];
@@ -2023,7 +2034,7 @@ void ConfigInputWidget::simpleCalibration(bool enable)
             forceOneFlightMode();
         }
 
-        for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) {
+        for (unsigned int i = 0; i < ManualControlSettings::CHANNELNUMBER_RSSI; i++) {
             if ((i == ManualControlSettings::CHANNELNUMBER_FLIGHTMODE) || (i == ManualControlSettings::CHANNELNUMBER_THROTTLE)) {
                 adjustSpecialNeutrals();
                 checkThrottleRange();
@@ -2096,8 +2107,8 @@ bool ConfigInputWidget::shouldObjectBeSaved(UAVObject *object)
 void ConfigInputWidget::resetChannelSettings()
 {
     manualSettingsData = manualSettingsObj->getData();
-    // Clear all channel data : Channel Type (PPM,PWM..) and Number
+    // Clear all channel data : Channel Type (PPM,PWM..) and Number, except for Rssi
-    for (unsigned int channel = 0; channel < ManualControlSettings::CHANNELNUMBER_NUMELEM; channel++) {
+    for (unsigned int channel = 0; channel < ManualControlSettings::CHANNELNUMBER_RSSI; channel++) {
         manualSettingsData.ChannelGroups[channel] = ManualControlSettings::CHANNELGROUPS_NONE;
         manualSettingsData.ChannelNumber[channel] = CHANNEL_NUMBER_NONE;
         UAVObjectUpdaterHelper updateHelper;

ground/gcs/src/plugins/config/configinputwidget.h

@@ -130,6 +130,7 @@ private:
     AccessoryDesired *accessoryDesiredObj1;
     AccessoryDesired *accessoryDesiredObj2;
     AccessoryDesired *accessoryDesiredObj3;
+    AccessoryDesired *rssiDesiredObj4;
 
     ManualControlSettings *manualSettingsObj;
     ManualControlSettings::DataFields manualSettingsData;

shared/uavobjectdefinition/manualcontrolcommand.xml

@@ -8,7 +8,7 @@
         <field name="Yaw" units="%" type="float" elements="1"/>
         <field name="Collective" units="%" type="float" elements="1"/>
         <field name="Thrust" units="%" type="float" elements="1"/>
-	<field name="Channel" units="us" type="uint16" elements="10"/>
+	<field name="Channel" units="us" type="uint16" elements="11"/>
 	<field name="FlightModeSwitchPosition" units="" type="uint8" elements="1" defaultvalue="0"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>

shared/uavobjectdefinition/manualcontrolsettings.xml

@@ -1,34 +1,32 @@
 <xml>
     <object name="ManualControlSettings" singleinstance="true" settings="true" category="Control">
-        <description>Settings to indicate how to decode receiver input by @ref ManualControlModule.</description>
+	<description>Settings to indicate how to decode receiver input by @ref ManualControlModule.</description>
-        <field name="ChannelGroups" units="Channel Group" type="enum" 
+	<field name="ChannelGroups" units="Channel Group" type="enum" 
-	       elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3"
+	       elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3,Rssi"
-           options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),DSM (RcvrPort),S.Bus,EX.Bus,HoTT,SRXL,IBus,GCS,OPLink,OpenLRS,None" defaultvalue="None"/>
+	       options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),DSM (RcvrPort),S.Bus,EX.Bus,HoTT,SRXL,IBus,GCS,OPLink,OpenLRS,None" defaultvalue="None"/>
 	<field name="ChannelNumber" units="channel" type="uint8" defaultvalue="0"
-	       elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3"/>
+	       elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3,Rssi"/>
 	<field name="ChannelMin" units="us" type="int16" defaultvalue="1000"
-	       elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3"/>
+	       elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3,Rssi"/>
 	<field name="ChannelNeutral" units="us" type="int16" defaultvalue="1500"
-	       elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3"/>
+	       elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3,Rssi"/>
 	<field name="ChannelMax" units="us" type="int16" defaultvalue="2000"
-	       elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3"/>
+	       elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2,Accessory3,Rssi"/>
 	<field name="ResponseTime" units="ms" type="uint16" defaultvalue="0"
-	       elementnames="Roll,Pitch,Yaw,Collective,Accessory0,Accessory1,Accessory2,Accessory3"/>
+	       elementnames="Roll,Pitch,Yaw,Collective,Accessory0,Accessory1,Accessory2,Accessory3,Rssi"/>
 
-        <field name="Deadband" units="%" type="uint8" elements="1" defaultvalue="2"/>
+	<field name="Deadband" units="%" type="uint8" elements="1" defaultvalue="2"/>
 	<!-- Note the following deadband is used in AssistedControl.  Use of deadband with AssistedControl is not optional and will have a hardcoded minimum. -->
-        <field name="DeadbandAssistedControl" units="%" type="uint8" elements="1" defaultvalue="8" description="Stick deadband used for AssistedControl"/>
+	<field name="DeadbandAssistedControl" units="%" type="uint8" elements="1" defaultvalue="8" description="Stick deadband used for AssistedControl"/>
-
 
 	<!-- Note these options values should be identical to those defined in FlightMode -->
-        <field name="FlightModeNumber" units="" type="uint8" elements="1" defaultvalue="3"/>
+	<field name="FlightModeNumber" units="" type="uint8" elements="1" defaultvalue="3"/>
 
-        <field name="FailsafeFlightModeSwitchPosition" units="" type="int8" elements="1" defaultvalue="-1"/>
+	<field name="FailsafeFlightModeSwitchPosition" units="" type="int8" elements="1" defaultvalue="-1"/>
-        <field name="FailsafeChannel" units="%" type="float" elementnames="Throttle,Roll,Pitch,Yaw,Collective,Accessory0,Accessory1,Accessory2,Accessory3" defaultvalue="-1,0,0,0,0,0,0,0,0" />
+	<field name="FailsafeChannel" units="%" type="float" elementnames="Throttle,Roll,Pitch,Yaw,Collective,Accessory0,Accessory1,Accessory2,Accessory3" defaultvalue="-1,0,0,0,0,0,0,0,0" />
-
+	<access gcs="readwrite" flight="readwrite"/>
-        <access gcs="readwrite" flight="readwrite"/>
+	<telemetrygcs acked="true" updatemode="onchange" period="0"/>
-        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+	<telemetryflight acked="true" updatemode="onchange" period="0"/>
-        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+	<logging updatemode="manual" period="0"/>
-        <logging updatemode="manual" period="0"/>
     </object>
 </xml>