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 */
#define EXBUS_HEADER_LENGTH 4
#define EXBUS_CRC_LENGTH 2
#define EXBUS_MAX_CHANNELS 16
#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_HEADER_LENGTH 4
#define EXBUS_CRC_LENGTH 2
#define EXBUS_MAX_CHANNELS 16
#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_SYNC_CHANNEL 0x3E
#define EXBUS_SYNC_TELEMETRY 0x3D
#define EXBUS_BYTE_REQ 0x01
#define EXBUS_BYTE_NOREQ 0x03
#define EXBUS_SYNC_CHANNEL 0x3E
#define EXBUS_SYNC_TELEMETRY 0x3D
#define EXBUS_BYTE_REQ 0x01
#define EXBUS_BYTE_NOREQ 0x03
#define EXBUS_DATA_CHANNEL 0x31
#define EXBUS_DATA_TELEMETRY 0x3A
#define EXBUS_DATA_JETIBOX 0x3B
#define EXBUS_DATA_CHANNEL 0x31
#define EXBUS_DATA_TELEMETRY 0x3A
#define EXBUS_DATA_JETIBOX 0x3B
#define EXBUS_LOW_BAUD_RATE 125000
#define EXBUS_HIGH_BAUD_RATE 250000
#define EXBUS_BAUD_RATE_LIMIT 64
#define EXBUS_FRAME_TIMEOUT 4
#define EXBUS_FAILSAFE_TIMEOUT 64
#define EXBUS_LOW_BAUD_RATE 125000
#define EXBUS_HIGH_BAUD_RATE 250000
#define EXBUS_BAUD_RATE_LIMIT 64
#define EXBUS_FRAME_TIMEOUT 4
#define EXBUS_FAILSAFE_TIMEOUT 64
/* With an Ex.Bus frame rate of 10ms (100Hz) averaging over 20 samples
* gives about a 200ms response.
@ -78,7 +78,7 @@ static uint8_t PIOS_EXBUS_Quality_Get(uint32_t rcvr_id);
/* Local Variables */
const struct pios_rcvr_driver pios_exbus_rcvr_driver = {
.read = PIOS_EXBUS_Get,
.read = PIOS_EXBUS_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;
}
// Calculate quality trend using weighted average of good frames
state->quality = ((state->quality * (EXBUS_FL_WEIGHTED_AVERAGE - 1)) +
quality_trend) / EXBUS_FL_WEIGHTED_AVERAGE;
state->quality = ((state->quality * (EXBUS_FL_WEIGHTED_AVERAGE - 1)) +
quality_trend) / EXBUS_FL_WEIGHTED_AVERAGE;
/* get ready for the next frame */
state->frame_found = false;
}

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

@ -90,6 +90,11 @@
#define HOTT_FRAME_TIMEOUT 4
#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 */
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,
bool *need_yield);
static void PIOS_HOTT_Supervisor(uint32_t hott_id);
static uint8_t PIOS_HOTT_Quality_Get(uint32_t rcvr_id);
/* Local Variables */
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 {
@ -118,6 +125,7 @@ struct pios_hott_state {
uint8_t tx_connected;
uint8_t byte_count;
uint8_t frame_length;
float quality;
};
struct pios_hott_dev {
@ -162,6 +170,7 @@ static void PIOS_HOTT_ResetState(struct pios_hott_state *state)
state->failsafe_timer = 0;
state->frame_found = 0;
state->tx_connected = 0;
state->quality = 0.0f;
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;
}
if (state->byte_count == state->frame_length) {
uint8_t quality_trend = 0;
/* full frame received - process and wait for new one */
if (!PIOS_HOTT_UnrollChannels(hott_dev)) {
/* data looking good */
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 */
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];
}
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
* two functions: frame syncing and failsafe triggering.
@ -410,6 +438,7 @@ static void PIOS_HOTT_Supervisor(uint32_t hott_id)
PIOS_HOTT_ResetChannels(state);
state->failsafe_timer = 0;
state->tx_connected = 0;
state->quality = 0.0f;
}
}