rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-1803 Initial commit

Browse Source
This commit is contained in:
Steve Evans 2015-04-02 19:09:58 +01:00
parent a60baf281f
commit a0f180d393
9 changed files with 99 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -37,6 +37,7 @@
#include <manualcontrolsettings.h>
#include <manualcontrolcommand.h>
#include <receiveractivity.h>
#include <receiverstatus.h>
#include <flightstatus.h>
#include <flighttelemetrystats.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
@ -99,6 +100,7 @@ struct rcvr_activity_fsm {
ManualControlSettingsChannelGroupsOptions group;
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count;
uint8_t quality;
};
static struct rcvr_activity_fsm activity_fsm;
@ -148,6 +150,18 @@ int32_t ReceiverInitialize()
}
MODULE_INITCALL(ReceiverInitialize, ReceiverStart);
static void updateRcvrStatus(uint32_t rcvr_id)
{
extern uint32_t pios_rcvr_group_map[];
uint8_t quality = PIOS_RCVR_GetQuality(pios_rcvr_group_map[rcvr_id]);
#if 1
ReceiverStatusQualitySet(&quality);
#else
quality = quality * 100 / 255;
#endif
}
/**
* Module task
*/
@ -243,6 +257,13 @@ static void receiverTask(__attribute__((unused)) void *parameters)
}
}
/* Update the receiver status, specifically signal quality, for the
* receiver used for flight mode control.
*
* SCEDEBUG Fixed to use S.Bus for now
*/
updateRcvrStatus(MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS);
// Check settings, if error raise alarm
if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE

28
flight/pios/common/pios_rcvr.c
View File

@ -113,6 +113,34 @@ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel)
return rcvr_dev->driver->read(rcvr_dev->lower_id, channel);
}
/**
* @brief Reads input quality from the appropriate driver
* @param[in] rcvr_id driver to read from
* @returns Unitless input value
* @retval PIOS_RCVR_NODRIVER driver was not initialized
*/
uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id)
{
if (rcvr_id == 0) {
return PIOS_RCVR_NODRIVER;
}
struct pios_rcvr_dev *rcvr_dev = (struct pios_rcvr_dev *)rcvr_id;
if (!PIOS_RCVR_validate(rcvr_dev)) {
/* Undefined RCVR port for this board (see pios_board.c) */
PIOS_Assert(0);
}
if (!rcvr_dev->driver->get_quality)
{
/* If no quality is available assume max */
return 255;
}
return rcvr_dev->driver->get_quality(rcvr_dev->lower_id);
}
/**
* @brief Get a semaphore that signals when a new sample is available.
* @param[in] rcvr_id driver to read from

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

@ -32,6 +32,8 @@
#ifdef PIOS_INCLUDE_SBUS
#include <uavobjectmanager.h>
#include <sbusstatus.h>
#include "pios_sbus_priv.h"
/* Forward Declarations */
@ -42,11 +44,12 @@ static uint16_t PIOS_SBus_RxInCallback(uint32_t context,
uint16_t *headroom,
bool *need_yield);
static void PIOS_SBus_Supervisor(uint32_t sbus_id);
static uint8_t PIOS_SBus_Quality_Get(uint32_t rcvr_id);
/* Local Variables */
const struct pios_rcvr_driver pios_sbus_rcvr_driver = {
.read = PIOS_SBus_Get,
.get_quality = PIOS_SBus_Quality_Get
};
enum pios_sbus_dev_magic {
@ -60,8 +63,14 @@ struct pios_sbus_state {
uint8_t failsafe_timer;
uint8_t frame_found;
uint8_t byte_count;
uint8_t quality;
};
/* With an S.Bus frame rate of 7ms (130Hz) averaging over 26 samples
* gives about a 200ms response.
*/
#define SBUS_FL_WEIGHTED_AVE 26
struct pios_sbus_dev {
enum pios_sbus_dev_magic magic;
const struct pios_sbus_cfg *cfg;
@ -120,6 +129,7 @@ static void PIOS_SBus_ResetState(struct pios_sbus_state *state)
state->receive_timer = 0;
state->failsafe_timer = 0;
state->frame_found = 0;
state->quality = 0;
PIOS_SBus_ResetChannels(state);
}
@ -251,18 +261,29 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
state->byte_count++;
} else {
if (b == SBUS_EOF_BYTE || (b & SBUS_R7008SB_EOF_COUNTER_MASK) == 0) {
uint8_t quality_trend;
/* full frame received */
uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3];
if (flags & SBUS_FLAG_FL) {
/* frame lost, do not update */
} else if (flags & SBUS_FLAG_FS) {
/* failsafe flag active */
PIOS_SBus_ResetChannels(state);
/* Quality trend is towards 0% */
quality_trend = 0;
} else {
/* data looking good */
PIOS_SBus_UnrollChannels(state);
state->failsafe_timer = 0;
}
/* Quality trend is towards 100% */
quality_trend = 100;
if (flags & SBUS_FLAG_FS) {
/* failsafe flag active */
PIOS_SBus_ResetChannels(state);
} else {
/* data looking good */
PIOS_SBus_UnrollChannels(state);
state->failsafe_timer = 0;
}
}
/* Present quality as a weighted average of good frames */
state->quality = ((state->quality * (SBUS_FL_WEIGHTED_AVE - 1)) +
quality_trend) / SBUS_FL_WEIGHTED_AVE;
} else {
/* discard whole frame */
}
@ -341,6 +362,19 @@ static void PIOS_SBus_Supervisor(uint32_t sbus_id)
}
}
static uint8_t PIOS_SBus_Quality_Get(uint32_t sbus_id)
{
struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id;
bool valid = PIOS_SBus_Validate(sbus_dev);
PIOS_Assert(valid);
struct pios_sbus_state *state = &(sbus_dev->state);
return state->quality;
}
#endif /* PIOS_INCLUDE_SBUS */
/**

2
flight/pios/inc/pios_rcvr.h
View File

@ -35,10 +35,12 @@ struct pios_rcvr_driver {
void (*init)(uint32_t id);
int32_t (*read)(uint32_t id, uint8_t channel);
xSemaphoreHandle (*get_semaphore)(uint32_t id, uint8_t channel);
uint8_t (*get_quality)(uint32_t id);
};
/* Public Functions */
extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel);
extern uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id);
extern xSemaphoreHandle PIOS_RCVR_GetSemaphore(uint32_t rcvr_id, uint8_t channel);
/*! Define error codes for PIOS_RCVR_Get */

1
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -122,6 +122,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/gpssettings.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
SRC += $(OPUAVSYNTHDIR)/receiverstatus.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/ratedesired.c
SRC += $(OPUAVSYNTHDIR)/txpidsettings.c

1
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
View File

@ -103,6 +103,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings

1
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -103,6 +103,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings

1
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -103,6 +103,7 @@ UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += altitudeholdsettings

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -112,6 +112,7 @@ HEADERS += \
$$UAVOBJECT_SYNTHETICS/hwsettings.h \
$$UAVOBJECT_SYNTHETICS/gcsreceiver.h \
$$UAVOBJECT_SYNTHETICS/receiveractivity.h \
$$UAVOBJECT_SYNTHETICS/receiverstatus.h \
$$UAVOBJECT_SYNTHETICS/attitudesettings.h \
$$UAVOBJECT_SYNTHETICS/txpidsettings.h \
$$UAVOBJECT_SYNTHETICS/cameradesired.h \
@ -219,6 +220,7 @@ SOURCES += \
$$UAVOBJECT_SYNTHETICS/hwsettings.cpp \
$$UAVOBJECT_SYNTHETICS/gcsreceiver.cpp \
$$UAVOBJECT_SYNTHETICS/receiveractivity.cpp \
$$UAVOBJECT_SYNTHETICS/receiverstatus.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesettings.cpp \
$$UAVOBJECT_SYNTHETICS/txpidsettings.cpp \
$$UAVOBJECT_SYNTHETICS/cameradesired.cpp \