rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-16 08:29:15 +01:00
Code Issues Releases Activity

LP-104 LP-196 Signal quality implementation for Graupner HoTT. Uncrustification of flight code.

Browse Source
This commit is contained in:
Fredrik Arvidsson 2016-01-25 00:42:54 +01:00
parent ffa671da78
commit 9e53b35fdc
2 changed files with 50 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
flight/pios/common/pios_exbus.c
View File

@ -39,26 +39,26 @@
*/ */
/* EXBUS frame size and contents definitions */ /* EXBUS frame size and contents definitions */
#define EXBUS_HEADER_LENGTH 4 #define EXBUS_HEADER_LENGTH 4
#define EXBUS_CRC_LENGTH 2 #define EXBUS_CRC_LENGTH 2
#define EXBUS_MAX_CHANNELS 16 #define EXBUS_MAX_CHANNELS 16
#define EXBUS_OVERHEAD_LENGTH (EXBUS_HEADER_LENGTH + EXBUS_CRC_LENGTH) #define EXBUS_OVERHEAD_LENGTH (EXBUS_HEADER_LENGTH + EXBUS_CRC_LENGTH)
#define EXBUS_MAX_FRAME_LENGTH (EXBUS_MAX_CHANNELS * 2 + 10 + EXBUS_OVERHEAD_LENGTH) #define EXBUS_MAX_FRAME_LENGTH (EXBUS_MAX_CHANNELS * 2 + 10 + EXBUS_OVERHEAD_LENGTH)
#define EXBUS_SYNC_CHANNEL 0x3E #define EXBUS_SYNC_CHANNEL 0x3E
#define EXBUS_SYNC_TELEMETRY 0x3D #define EXBUS_SYNC_TELEMETRY 0x3D
#define EXBUS_BYTE_REQ 0x01 #define EXBUS_BYTE_REQ 0x01
#define EXBUS_BYTE_NOREQ 0x03 #define EXBUS_BYTE_NOREQ 0x03
#define EXBUS_DATA_CHANNEL 0x31 #define EXBUS_DATA_CHANNEL 0x31
#define EXBUS_DATA_TELEMETRY 0x3A #define EXBUS_DATA_TELEMETRY 0x3A
#define EXBUS_DATA_JETIBOX 0x3B #define EXBUS_DATA_JETIBOX 0x3B
#define EXBUS_LOW_BAUD_RATE 125000 #define EXBUS_LOW_BAUD_RATE 125000
#define EXBUS_HIGH_BAUD_RATE 250000 #define EXBUS_HIGH_BAUD_RATE 250000
#define EXBUS_BAUD_RATE_LIMIT 64 #define EXBUS_BAUD_RATE_LIMIT 64
#define EXBUS_FRAME_TIMEOUT 4 #define EXBUS_FRAME_TIMEOUT 4
#define EXBUS_FAILSAFE_TIMEOUT 64 #define EXBUS_FAILSAFE_TIMEOUT 64
/* With an Ex.Bus frame rate of 10ms (100Hz) averaging over 20 samples /* With an Ex.Bus frame rate of 10ms (100Hz) averaging over 20 samples
* gives about a 200ms response. * gives about a 200ms response.
@ -78,7 +78,7 @@ static uint8_t PIOS_EXBUS_Quality_Get(uint32_t rcvr_id);
/* Local Variables */ /* Local Variables */
const struct pios_rcvr_driver pios_exbus_rcvr_driver = { const struct pios_rcvr_driver pios_exbus_rcvr_driver = {
.read = PIOS_EXBUS_Get, .read = PIOS_EXBUS_Get,
.get_quality = PIOS_EXBUS_Quality_Get, .get_quality = PIOS_EXBUS_Quality_Get,
}; };
@ -278,8 +278,8 @@ static void PIOS_EXBUS_UpdateState(struct pios_exbus_dev *exbus_dev, uint8_t byt
quality_trend = 100; quality_trend = 100;
} }
// Calculate quality trend using weighted average of good frames // Calculate quality trend using weighted average of good frames
state->quality = ((state->quality * (EXBUS_FL_WEIGHTED_AVERAGE - 1)) + state->quality = ((state->quality * (EXBUS_FL_WEIGHTED_AVERAGE - 1)) +
quality_trend) / EXBUS_FL_WEIGHTED_AVERAGE; quality_trend) / EXBUS_FL_WEIGHTED_AVERAGE;
/* get ready for the next frame */ /* get ready for the next frame */
state->frame_found = false; state->frame_found = false;
} }

31
flight/pios/common/pios_hott.c
View File

@ -90,6 +90,11 @@
#define HOTT_FRAME_TIMEOUT 4 #define HOTT_FRAME_TIMEOUT 4
#define HOTT_FAILSAFE_TIMEOUT 64 #define HOTT_FAILSAFE_TIMEOUT 64
/* With an Ex.Bus frame rate of 11/22ms (90/45Hz) averaging over 15 samples
* gives about a 165/330ms response.
*/
#define HOTT_FL_WEIGHTED_AVERAGE 20
/* Forward Declarations */ /* Forward Declarations */
static int32_t PIOS_HOTT_Get(uint32_t rcvr_id, uint8_t channel); static int32_t PIOS_HOTT_Get(uint32_t rcvr_id, uint8_t channel);
@ -99,10 +104,12 @@ static uint16_t PIOS_HOTT_RxInCallback(uint32_t context,
uint16_t *headroom, uint16_t *headroom,
bool *need_yield); bool *need_yield);
static void PIOS_HOTT_Supervisor(uint32_t hott_id); static void PIOS_HOTT_Supervisor(uint32_t hott_id);
static uint8_t PIOS_HOTT_Quality_Get(uint32_t rcvr_id);
/* Local Variables */ /* Local Variables */
const struct pios_rcvr_driver pios_hott_rcvr_driver = { const struct pios_rcvr_driver pios_hott_rcvr_driver = {
.read = PIOS_HOTT_Get, .read = PIOS_HOTT_Get,
.get_quality = PIOS_HOTT_Quality_Get,
}; };
enum pios_hott_dev_magic { enum pios_hott_dev_magic {
@ -118,6 +125,7 @@ struct pios_hott_state {
uint8_t tx_connected; uint8_t tx_connected;
uint8_t byte_count; uint8_t byte_count;
uint8_t frame_length; uint8_t frame_length;
float quality;
}; };
struct pios_hott_dev { struct pios_hott_dev {
@ -162,6 +170,7 @@ static void PIOS_HOTT_ResetState(struct pios_hott_state *state)
state->failsafe_timer = 0; state->failsafe_timer = 0;
state->frame_found = 0; state->frame_found = 0;
state->tx_connected = 0; state->tx_connected = 0;
state->quality = 0.0f;
PIOS_HOTT_ResetChannels(state); PIOS_HOTT_ResetChannels(state);
} }
@ -275,11 +284,17 @@ static void PIOS_HOTT_UpdateState(struct pios_hott_dev *hott_dev, uint8_t byte)
state->frame_length = HOTT_OVERHEAD_LENGTH + 2 * byte; state->frame_length = HOTT_OVERHEAD_LENGTH + 2 * byte;
} }
if (state->byte_count == state->frame_length) { if (state->byte_count == state->frame_length) {
uint8_t quality_trend = 0;
/* full frame received - process and wait for new one */ /* full frame received - process and wait for new one */
if (!PIOS_HOTT_UnrollChannels(hott_dev)) { if (!PIOS_HOTT_UnrollChannels(hott_dev)) {
/* data looking good */ /* data looking good */
state->failsafe_timer = 0; state->failsafe_timer = 0;
quality_trend = 100;
} }
// Calculate quality trend using weighted average of good frames
state->quality = ((state->quality * (HOTT_FL_WEIGHTED_AVERAGE - 1)) +
quality_trend) / HOTT_FL_WEIGHTED_AVERAGE;
/* prepare for the next frame */ /* prepare for the next frame */
state->frame_found = 0; state->frame_found = 0;
} }
@ -375,6 +390,19 @@ static int32_t PIOS_HOTT_Get(uint32_t rcvr_id, uint8_t channel)
return hott_dev->state.channel_data[channel]; return hott_dev->state.channel_data[channel];
} }
static uint8_t PIOS_HOTT_Quality_Get(uint32_t hott_id)
{
struct pios_hott_dev *hott_dev = (struct pios_hott_dev *)hott_id;
bool valid = PIOS_HOTT_Validate(hott_dev);
PIOS_Assert(valid);
struct pios_hott_state *state = &(hott_dev->state);
return (uint8_t)(state->quality + 0.5f);
}
/** /**
* Input data supervisor is called periodically and provides * Input data supervisor is called periodically and provides
* two functions: frame syncing and failsafe triggering. * two functions: frame syncing and failsafe triggering.
@ -410,6 +438,7 @@ static void PIOS_HOTT_Supervisor(uint32_t hott_id)
PIOS_HOTT_ResetChannels(state); PIOS_HOTT_ResetChannels(state);
state->failsafe_timer = 0; state->failsafe_timer = 0;
state->tx_connected = 0; state->tx_connected = 0;
state->quality = 0.0f;
} }
} }