rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-26 15:54:15 +01:00
Code Issues Releases Activity

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

Browse Source 
LP-407 Add Rssi channel input
This commit is contained in:
Alessio Morale 2016-09-21 21:24:43 +02:00
commit 5dbdaaa904
6 changed files with 96 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
flight/modules/Receiver/receiver.c
View File

@ -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) {

2
flight/pios/common/pios_sbus.c
View File

@ -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 */

31
ground/gcs/src/plugins/config/configinputwidget.cpp
View File

@ -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)
// 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);
for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) {
// Ignore last Rssi channel
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
for (unsigned int channel = 0; channel < ManualControlSettings::CHANNELNUMBER_NUMELEM; channel++) {
// Clear all channel data : Channel Type (PPM,PWM..) and Number, except for Rssi
for (unsigned int channel = 0; channel < ManualControlSettings::CHANNELNUMBER_RSSI; channel++) {
manualSettingsData.ChannelGroups[channel] = ManualControlSettings::CHANNELGROUPS_NONE;
manualSettingsData.ChannelNumber[channel] = CHANNEL_NUMBER_NONE;
UAVObjectUpdaterHelper updateHelper;

1
ground/gcs/src/plugins/config/configinputwidget.h
View File

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

2
shared/uavobjectdefinition/manualcontrolcommand.xml
View File

@ -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"/>

14
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -2,30 +2,28 @@
<object name="ManualControlSettings" singleinstance="true" settings="true" category="Control">
<description>Settings to indicate how to decode receiver input by @ref ManualControlModule.</description>
<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"/>
<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"/>
<!-- 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"/>
<!-- Note these options values should be identical to those defined in FlightMode -->
<field name="FlightModeNumber" units="" type="uint8" elements="1" defaultvalue="3"/>
<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" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>