rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into bfdev-configurable-spi-phase-1

This commit is contained in:
jflyper 2017-06-29 09:28:36 +09:00 committed by GitHub
parent d45ef7805b 9028db79c8
commit 78bf9da3a6
60 changed files with 1402 additions and 692 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/main/blackbox/blackbox.c
View File

@ -992,16 +992,16 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time);
}
blackboxWriteUnsignedVB(GPS_numSat);
blackboxWriteSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]);
blackboxWriteSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]);
blackboxWriteUnsignedVB(GPS_altitude);
blackboxWriteUnsignedVB(GPS_speed);
blackboxWriteUnsignedVB(GPS_ground_course);
blackboxWriteUnsignedVB(gpsSol.numSat);
blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]);
blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]);
blackboxWriteUnsignedVB(gpsSol.llh.alt);
blackboxWriteUnsignedVB(gpsSol.groundSpeed);
blackboxWriteUnsignedVB(gpsSol.groundCourse);
gpsHistory.GPS_numSat = GPS_numSat;
gpsHistory.GPS_coord[0] = GPS_coord[0];
gpsHistory.GPS_coord[1] = GPS_coord[1];
gpsHistory.GPS_numSat = gpsSol.numSat;
gpsHistory.GPS_coord[LAT] = gpsSol.llh.lat;
gpsHistory.GPS_coord[LON] = gpsSol.llh.lon;
}
#endif
@ -1474,8 +1474,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
if (blackboxShouldLogGpsHomeFrame()) {
writeGPSHomeFrame();
writeGPSFrame(currentTimeUs);
} else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
|| GPS_coord[1] != gpsHistory.GPS_coord[1]) {
} else if (gpsSol.numSat != gpsHistory.GPS_numSat
|| gpsSol.llh.lat != gpsHistory.GPS_coord[LAT]
|| gpsSol.llh.lon != gpsHistory.GPS_coord[LON]) {
//We could check for velocity changes as well but I doubt it changes independent of position
writeGPSFrame(currentTimeUs);
}

10
src/main/cms/cms.c
View File

@ -315,10 +315,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
#ifdef OSD
case OME_VISIBLE:
if (IS_PRINTVALUE(p) && p->data) {
uint32_t address = (uint32_t)p->data;
uint16_t *val;
val = (uint16_t *)address;
uint16_t *val = (uint16_t *)p->data;
if (VISIBLE(*val)) {
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
@ -757,10 +754,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
#ifdef OSD
case OME_VISIBLE:
if (p->data) {
uint32_t address = (uint32_t)p->data;
uint16_t *val;
val = (uint16_t *)address;
uint16_t *val = (uint16_t *)p->data;
if (key == KEY_RIGHT)
*val |= VISIBLE_FLAG;

5
src/main/config/parameter_group_ids.h
View File

@ -107,8 +107,9 @@
#define PG_SONAR_CONFIG 516
#define PG_ESC_SENSOR_CONFIG 517
#define PG_I2C_CONFIG 518
#define PG_SPI_PIN_CONFIG 519
#define PG_BETAFLIGHT_END 519
#define PG_DASHBOARD_CONFIG 519
#define PG_SPI_PIN_CONFIG 520
#define PG_BETAFLIGHT_END 520
// OSD configuration (subject to change)

7
src/main/drivers/bus.h
View File

@ -19,14 +19,21 @@
#include "platform.h"
#include "drivers/bus_i2c.h"
#include "drivers/io_types.h"
typedef union busDevice_u {
struct deviceSpi_s {
SPI_TypeDef *instance;
IO_t csnPin;
} spi;
struct deviceI2C_s {
I2CDevice device;
uint8_t address;
} i2c;
} busDevice_t;
#ifdef TARGET_BUS_INIT
void targetBusInit(void);
#endif

8
src/main/drivers/bus_i2c.h
View File

@ -45,6 +45,14 @@ typedef enum I2CDevice {
#define I2CDEV_COUNT 4
#endif
// Macros to convert between CLI bus number and I2CDevice.
#define I2C_CFG_TO_DEV(x) ((x) - 1)
#define I2C_DEV_TO_CFG(x) ((x) + 1)
// I2C device address range in 8-bit address mode
#define I2C_ADDR8_MIN 8
#define I2C_ADDR8_MAX 119
typedef struct i2cConfig_s {
ioTag_t ioTagScl[I2CDEV_COUNT];
ioTag_t ioTagSda[I2CDEV_COUNT];

13
src/main/drivers/display.c
View File

@ -17,6 +17,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
@ -66,13 +67,23 @@ bool displayIsGrabbed(const displayPort_t *instance)
return (instance && instance->grabCount > 0);
}
void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y)
{
instance->posX = x;
instance->posY = y;
}
int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s)
{
return instance->vTable->write(instance, x, y, s);
instance->posX = x + strlen(s);
instance->posY = y;
return instance->vTable->writeString(instance, x, y, s);
}
int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c)
{
instance->posX = x + 1;
instance->posY = y;
return instance->vTable->writeChar(instance, x, y, c);
}

6
src/main/drivers/display.h
View File

@ -20,8 +20,11 @@
struct displayPortVTable_s;
typedef struct displayPort_s {
const struct displayPortVTable_s *vTable;
void *device;
uint8_t rows;
uint8_t cols;
uint8_t posX;
uint8_t posY;
// CMS state
bool cleared;
@ -35,7 +38,7 @@ typedef struct displayPortVTable_s {
int (*clearScreen)(displayPort_t *displayPort);
int (*drawScreen)(displayPort_t *displayPort);
int (*screenSize)(const displayPort_t *displayPort);
int (*write)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text);
int (*writeString)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text);
int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c);
bool (*isTransferInProgress)(const displayPort_t *displayPort);
int (*heartbeat)(displayPort_t *displayPort);
@ -58,6 +61,7 @@ bool displayIsGrabbed(const displayPort_t *instance);
void displayClearScreen(displayPort_t *instance);
void displayDrawScreen(displayPort_t *instance);
int displayScreenSize(const displayPort_t *instance);
void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y);
int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s);
int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c);
bool displayIsTransferInProgress(const displayPort_t *instance);

124
src/main/drivers/display_ug2864hsweg01.c
View File

@ -176,78 +176,76 @@ static const uint8_t multiWiiFont[][5] = { // Refer to "Times New Roman" Font Da
{ 0x7A, 0x7E, 0x7E, 0x7E, 0x7A }, // (131) - 0x00C8 Vertical Bargraph - 6 (full)
};
#define OLED_address 0x3C // OLED at address 0x3C in 7bit
static bool i2c_OLED_send_cmd(uint8_t command)
static bool i2c_OLED_send_cmd(busDevice_t *bus, uint8_t command)
{
return i2cWrite(OLED_I2C_INSTANCE, OLED_address, 0x80, command);
return i2cWrite(bus->i2c.device, bus->i2c.address, 0x80, command);
}
static bool i2c_OLED_send_byte(uint8_t val)
static bool i2c_OLED_send_byte(busDevice_t *bus, uint8_t val)
{
return i2cWrite(OLED_I2C_INSTANCE, OLED_address, 0x40, val);
return i2cWrite(bus->i2c.device, bus->i2c.address, 0x40, val);
}
void i2c_OLED_clear_display(void)
void i2c_OLED_clear_display(busDevice_t *bus)
{
i2c_OLED_send_cmd(0xa6); // Set Normal Display
i2c_OLED_send_cmd(0xae); // Display OFF
i2c_OLED_send_cmd(0x20); // Set Memory Addressing Mode
i2c_OLED_send_cmd(0x00); // Set Memory Addressing Mode to Horizontal addressing mode
i2c_OLED_send_cmd(0xb0); // set page address to 0
i2c_OLED_send_cmd(0x40); // Display start line register to 0
i2c_OLED_send_cmd(0); // Set low col address to 0
i2c_OLED_send_cmd(0x10); // Set high col address to 0
i2c_OLED_send_cmd(bus, 0xa6); // Set Normal Display
i2c_OLED_send_cmd(bus, 0xae); // Display OFF
i2c_OLED_send_cmd(bus, 0x20); // Set Memory Addressing Mode
i2c_OLED_send_cmd(bus, 0x00); // Set Memory Addressing Mode to Horizontal addressing mode
i2c_OLED_send_cmd(bus, 0xb0); // set page address to 0
i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0
i2c_OLED_send_cmd(bus, 0); // Set low col address to 0
i2c_OLED_send_cmd(bus, 0x10); // Set high col address to 0
for(uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture
i2c_OLED_send_byte(0x00); // clear
i2c_OLED_send_byte(bus, 0x00); // clear
}
i2c_OLED_send_cmd(0x81); // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction
i2c_OLED_send_cmd(200); // Here you can set the brightness 1 = dull, 255 is very bright
i2c_OLED_send_cmd(0xaf); // display on
i2c_OLED_send_cmd(bus, 0x81); // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction
i2c_OLED_send_cmd(bus, 200); // Here you can set the brightness 1 = dull, 255 is very bright
i2c_OLED_send_cmd(bus, 0xaf); // display on
}
void i2c_OLED_clear_display_quick(void)
void i2c_OLED_clear_display_quick(busDevice_t *bus)
{
i2c_OLED_send_cmd(0xb0); // set page address to 0
i2c_OLED_send_cmd(0x40); // Display start line register to 0
i2c_OLED_send_cmd(0); // Set low col address to 0
i2c_OLED_send_cmd(0x10); // Set high col address to 0
i2c_OLED_send_cmd(bus, 0xb0); // set page address to 0
i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0
i2c_OLED_send_cmd(bus, 0); // Set low col address to 0
i2c_OLED_send_cmd(bus, 0x10); // Set high col address to 0
for(uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture
i2c_OLED_send_byte(0x00); // clear
i2c_OLED_send_byte(bus, 0x00); // clear
}
}
void i2c_OLED_set_xy(uint8_t col, uint8_t row)
void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row)
{
i2c_OLED_send_cmd(0xb0 + row); //set page address
i2c_OLED_send_cmd(0x00 + ((CHARACTER_WIDTH_TOTAL * col) & 0x0f)); //set low col address
i2c_OLED_send_cmd(0x10 + (((CHARACTER_WIDTH_TOTAL * col) >> 4) & 0x0f)); //set high col address
i2c_OLED_send_cmd(bus, 0xb0 + row); //set page address
i2c_OLED_send_cmd(bus, 0x00 + ((CHARACTER_WIDTH_TOTAL * col) & 0x0f)); //set low col address
i2c_OLED_send_cmd(bus, 0x10 + (((CHARACTER_WIDTH_TOTAL * col) >> 4) & 0x0f)); //set high col address
}
void i2c_OLED_set_line(uint8_t row)
void i2c_OLED_set_line(busDevice_t *bus, uint8_t row)
{
i2c_OLED_send_cmd(0xb0 + row); //set page address
i2c_OLED_send_cmd(0); //set low col address
i2c_OLED_send_cmd(0x10); //set high col address
i2c_OLED_send_cmd(bus, 0xb0 + row); //set page address
i2c_OLED_send_cmd(bus, 0); //set low col address
i2c_OLED_send_cmd(bus, 0x10); //set high col address
}
void i2c_OLED_send_char(unsigned char ascii)
void i2c_OLED_send_char(busDevice_t *bus, unsigned char ascii)
{
unsigned char i;
uint8_t buffer;
for (i = 0; i < 5; i++) {
buffer = multiWiiFont[ascii - 32][i];
buffer ^= CHAR_FORMAT; // apply
i2c_OLED_send_byte(buffer);
i2c_OLED_send_byte(bus, buffer);
}
i2c_OLED_send_byte(CHAR_FORMAT); // the gap
i2c_OLED_send_byte(bus, CHAR_FORMAT); // the gap
}
void i2c_OLED_send_string(const char *string)
void i2c_OLED_send_string(busDevice_t *bus, const char *string)
{
// Sends a string of chars until null terminator
while (*string) {
i2c_OLED_send_char(*string);
i2c_OLED_send_char(bus, *string);
string++;
}
}
@ -255,38 +253,38 @@ void i2c_OLED_send_string(const char *string)
/**
* according to http://www.adafruit.com/datasheets/UG-2864HSWEG01.pdf Chapter 4.4 Page 15
*/
bool ug2864hsweg01InitI2C(void)
bool ug2864hsweg01InitI2C(busDevice_t *bus)
{
// Set display OFF
if (!i2c_OLED_send_cmd(0xAE)) {
if (!i2c_OLED_send_cmd(bus, 0xAE)) {
return false;
}
i2c_OLED_send_cmd(0xD4); // Set Display Clock Divide Ratio / OSC Frequency
i2c_OLED_send_cmd(0x80); // Display Clock Divide Ratio / OSC Frequency
i2c_OLED_send_cmd(0xA8); // Set Multiplex Ratio
i2c_OLED_send_cmd(0x3F); // Multiplex Ratio for 128x64 (64-1)
i2c_OLED_send_cmd(0xD3); // Set Display Offset
i2c_OLED_send_cmd(0x00); // Display Offset
i2c_OLED_send_cmd(0x40); // Set Display Start Line
i2c_OLED_send_cmd(0x8D); // Set Charge Pump
i2c_OLED_send_cmd(0x14); // Charge Pump (0x10 External, 0x14 Internal DC/DC)
i2c_OLED_send_cmd(0xA1); // Set Segment Re-Map
i2c_OLED_send_cmd(0xC8); // Set Com Output Scan Direction
i2c_OLED_send_cmd(0xDA); // Set COM Hardware Configuration
i2c_OLED_send_cmd(0x12); // COM Hardware Configuration
i2c_OLED_send_cmd(0x81); // Set Contrast
i2c_OLED_send_cmd(0xCF); // Contrast
i2c_OLED_send_cmd(0xD9); // Set Pre-Charge Period
i2c_OLED_send_cmd(0xF1); // Set Pre-Charge Period (0x22 External, 0xF1 Internal)
i2c_OLED_send_cmd(0xDB); // Set VCOMH Deselect Level
i2c_OLED_send_cmd(0x40); // VCOMH Deselect Level
i2c_OLED_send_cmd(0xA4); // Set all pixels OFF
i2c_OLED_send_cmd(0xA6); // Set display not inverted
i2c_OLED_send_cmd(0xAF); // Set display On
i2c_OLED_send_cmd(bus, 0xD4); // Set Display Clock Divide Ratio / OSC Frequency
i2c_OLED_send_cmd(bus, 0x80); // Display Clock Divide Ratio / OSC Frequency
i2c_OLED_send_cmd(bus, 0xA8); // Set Multiplex Ratio
i2c_OLED_send_cmd(bus, 0x3F); // Multiplex Ratio for 128x64 (64-1)
i2c_OLED_send_cmd(bus, 0xD3); // Set Display Offset
i2c_OLED_send_cmd(bus, 0x00); // Display Offset
i2c_OLED_send_cmd(bus, 0x40); // Set Display Start Line
i2c_OLED_send_cmd(bus, 0x8D); // Set Charge Pump
i2c_OLED_send_cmd(bus, 0x14); // Charge Pump (0x10 External, 0x14 Internal DC/DC)
i2c_OLED_send_cmd(bus, 0xA1); // Set Segment Re-Map
i2c_OLED_send_cmd(bus, 0xC8); // Set Com Output Scan Direction
i2c_OLED_send_cmd(bus, 0xDA); // Set COM Hardware Configuration
i2c_OLED_send_cmd(bus, 0x12); // COM Hardware Configuration
i2c_OLED_send_cmd(bus, 0x81); // Set Contrast
i2c_OLED_send_cmd(bus, 0xCF); // Contrast
i2c_OLED_send_cmd(bus, 0xD9); // Set Pre-Charge Period
i2c_OLED_send_cmd(bus, 0xF1); // Set Pre-Charge Period (0x22 External, 0xF1 Internal)
i2c_OLED_send_cmd(bus, 0xDB); // Set VCOMH Deselect Level
i2c_OLED_send_cmd(bus, 0x40); // VCOMH Deselect Level
i2c_OLED_send_cmd(bus, 0xA4); // Set all pixels OFF
i2c_OLED_send_cmd(bus, 0xA6); // Set display not inverted
i2c_OLED_send_cmd(bus, 0xAF); // Set display On
i2c_OLED_clear_display();
i2c_OLED_clear_display(bus);
return true;
}

17
src/main/drivers/display_ug2864hsweg01.h
View File

@ -17,6 +17,8 @@
#pragma once
#include "drivers/bus.h"
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
@ -34,12 +36,11 @@
#define VERTICAL_BARGRAPH_ZERO_CHARACTER (128 + 32)
#define VERTICAL_BARGRAPH_CHARACTER_COUNT 7
bool ug2864hsweg01InitI2C(void);
void i2c_OLED_set_xy(uint8_t col, uint8_t row);
void i2c_OLED_set_line(uint8_t row);
void i2c_OLED_send_char(unsigned char ascii);
void i2c_OLED_send_string(const char *string);
void i2c_OLED_clear_display(void);
void i2c_OLED_clear_display_quick(void);
bool ug2864hsweg01InitI2C(busDevice_t *bus);
void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row);
void i2c_OLED_set_line(busDevice_t *bus, uint8_t row);
void i2c_OLED_send_char(busDevice_t *bus, unsigned char ascii);
void i2c_OLED_send_string(busDevice_t *bus, const char *string);
void i2c_OLED_clear_display(busDevice_t *bus);
void i2c_OLED_clear_display_quick(busDevice_t *bus);

2
src/main/drivers/max7456_symbols.h
View File

@ -21,6 +21,8 @@
#pragma once
#define SYM_END_OF_FONT 0xFF
// Character Symbols
#define SYM_BLANK 0x20

107
src/main/drivers/pwm_output.c
View File

@ -28,9 +28,6 @@
#include "timer.h"
#include "drivers/pwm_output.h"
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
static pwmWriteFunc *pwmWrite;
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmCompleteWriteFunc *pwmCompleteWrite = NULL;
@ -99,16 +96,19 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
#endif
}
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, uint8_t inversion)
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
{
#if defined(USE_HAL_DRIVER)
TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
if(Handle == NULL) return;
#endif
configTimeBase(timerHardware->tim, period, mhz);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value,
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
configTimeBase(timerHardware->tim, period, hz);
pwmOCConfig(timerHardware->tim,
timerHardware->channel,
value,
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
);
#if defined(USE_HAL_DRIVER)
if(timerHardware->output & TIMER_OUTPUT_N_CHANNEL)
@ -122,7 +122,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
#endif
port->ccr = timerChCCR(timerHardware);
port->period = period;
port->tim = timerHardware->tim;
*port->ccr = 0;
@ -134,29 +134,10 @@ static void pwmWriteUnused(uint8_t index, float value)
UNUSED(value);
}
static void pwmWriteBrushed(uint8_t index, float value)
{
*motors[index].ccr = lrintf((value - 1000) * motors[index].period / 1000);
}
static void pwmWriteStandard(uint8_t index, float value)
{
*motors[index].ccr = lrintf(value);
}
static void pwmWriteOneShot125(uint8_t index, float value)
{
*motors[index].ccr = lrintf(value * ONESHOT125_TIMER_MHZ/8.0f);
}
static void pwmWriteOneShot42(uint8_t index, float value)
{
*motors[index].ccr = lrintf(value * ONESHOT42_TIMER_MHZ/24.0f);
}
static void pwmWriteMultiShot(uint8_t index, float value)
{
*motors[index].ccr = lrintf(((value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
/* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
*motors[index].ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
}
#ifdef USE_DSHOT
@ -246,32 +227,32 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
{
memset(motors, 0, sizeof(motors));
uint32_t timerMhzCounter = 0;
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
float sMin = 0;
float sLen = 0;
switch (motorConfig->motorPwmProtocol) {
default:
case PWM_TYPE_ONESHOT125:
timerMhzCounter = ONESHOT125_TIMER_MHZ;
pwmWrite = &pwmWriteOneShot125;
sMin = 125e-6f;
sLen = 125e-6f;
break;
case PWM_TYPE_ONESHOT42:
timerMhzCounter = ONESHOT42_TIMER_MHZ;
pwmWrite = &pwmWriteOneShot42;
sMin = 42e-6f;
sLen = 42e-6f;
break;
case PWM_TYPE_MULTISHOT:
timerMhzCounter = MULTISHOT_TIMER_MHZ;
pwmWrite = &pwmWriteMultiShot;
sMin = 5e-6f;
sLen = 20e-6f;
break;
case PWM_TYPE_BRUSHED:
timerMhzCounter = PWM_BRUSHED_TIMER_MHZ;
pwmWrite = &pwmWriteBrushed;
sMin = 0;
useUnsyncedPwm = true;
idlePulse = 0;
break;
case PWM_TYPE_STANDARD:
timerMhzCounter = PWM_TIMER_MHZ;
pwmWrite = &pwmWriteStandard;
sMin = 1e-3f;
sLen = 1e-3f;
useUnsyncedPwm = true;
idlePulse = 0;
break;
@ -295,6 +276,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
}
if (!isDshot) {
pwmWrite = &pwmWriteStandard;
pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
}
@ -314,7 +296,9 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
#ifdef USE_DSHOT
if (isDshot) {
pwmDshotMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol,
pwmDshotMotorHardwareConfig(timerHardware,
motorIndex,
motorConfig->motorPwmProtocol,
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
motors[motorIndex].enabled = true;
continue;
@ -328,12 +312,24 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
#endif
if (useUnsyncedPwm) {
const uint32_t hz = timerMhzCounter * 1000000;
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse, motorConfig->motorPwmInversion);
} else {
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion);
}
/* standard PWM outputs */
const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen));
const uint32_t clock = timerClock(timerHardware->tim);
/* used to find the desired timer frequency for max resolution */
const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */
const uint32_t hz = clock / prescaler;
const unsigned period = hz / pwmRateHz;
/*
if brushed then it is the entire length of the period.
TODO: this can be moved back to periodMin and periodLen
once mixer outputs a 0..1 float value.
*/
motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
pwmOutConfig(&motors[motorIndex], timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
bool timerAlreadyUsed = false;
for (int i = 0; i < motorIndex; i++) {
@ -364,16 +360,16 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
{
switch (pwmProtocolType) {
case(PWM_TYPE_PROSHOT1000):
return MOTOR_PROSHOT1000_MHZ * 1000000;
return MOTOR_PROSHOT1000_HZ;
case(PWM_TYPE_DSHOT1200):
return MOTOR_DSHOT1200_MHZ * 1000000;
return MOTOR_DSHOT1200_HZ;
case(PWM_TYPE_DSHOT600):
return MOTOR_DSHOT600_MHZ * 1000000;
return MOTOR_DSHOT600_HZ;
case(PWM_TYPE_DSHOT300):
return MOTOR_DSHOT300_MHZ * 1000000;
return MOTOR_DSHOT300_HZ;
default:
case(PWM_TYPE_DSHOT150):
return MOTOR_DSHOT150_MHZ * 1000000;
return MOTOR_DSHOT150_HZ;
}
}
@ -460,8 +456,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
/* flag failure and disable ability to arm */
break;
}
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_HZ, PWM_TIMER_HZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
servos[servoIndex].enabled = true;
}
}
@ -474,7 +469,7 @@ void pwmWriteBeeper(bool onoffBeep)
if(!beeperPwm.io)
return;
if(onoffBeep == true) {
*beeperPwm.ccr = (1000000/freqBeep)/2;
*beeperPwm.ccr = (PWM_TIMER_HZ / freqBeep) / 2;
beeperPwm.enabled = true;
} else {
*beeperPwm.ccr = 0;
@ -500,7 +495,7 @@ void beeperPwmInit(IO_t io, uint16_t frequency)
IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
#endif
freqBeep = frequency;
pwmOutConfig(&beeperPwm, timer, PWM_TIMER_MHZ, 1000000/freqBeep, (1000000/freqBeep)/2,0);
pwmOutConfig(&beeperPwm, timer, PWM_TIMER_HZ, PWM_TIMER_HZ / freqBeep, (PWM_TIMER_HZ / freqBeep) / 2, 0);
}
*beeperPwm.ccr = 0;
beeperPwm.enabled = false;

35
src/main/drivers/pwm_output.h
View File

@ -20,7 +20,9 @@
#include "drivers/io_types.h"
#include "timer.h"
#ifndef MAX_SUPPORTED_MOTORS
#define MAX_SUPPORTED_MOTORS 12
#endif
#if defined(USE_QUAD_MIXER_ONLY)
#define MAX_SUPPORTED_SERVOS 1
@ -75,45 +77,29 @@ typedef enum {
PWM_TYPE_MAX
} motorPwmProtocolTypes_e;
#define PWM_TIMER_MHZ 1
#define PWM_TIMER_HZ MHZ_TO_HZ(1)
#ifdef USE_DSHOT
#define MAX_DMA_TIMERS 8
#define MOTOR_DSHOT1200_MHZ 24
#define MOTOR_DSHOT600_MHZ 12
#define MOTOR_DSHOT300_MHZ 6
#define MOTOR_DSHOT150_MHZ 3
#define MOTOR_DSHOT1200_HZ MHZ_TO_HZ(24)
#define MOTOR_DSHOT600_HZ MHZ_TO_HZ(12)
#define MOTOR_DSHOT300_HZ MHZ_TO_HZ(6)
#define MOTOR_DSHOT150_HZ MHZ_TO_HZ(3)
#define MOTOR_BIT_0 7
#define MOTOR_BIT_1 14
#define MOTOR_BITLENGTH 19
#define MOTOR_PROSHOT1000_MHZ 24
#define MOTOR_PROSHOT1000_HZ MHZ_TO_HZ(24)
#define PROSHOT_BASE_SYMBOL 24 // 1uS
#define PROSHOT_BIT_WIDTH 3
#define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS
#endif
#if defined(STM32F40_41xxx) // must be multiples of timer clock
#define ONESHOT125_TIMER_MHZ 12
#define ONESHOT42_TIMER_MHZ 21
#define MULTISHOT_TIMER_MHZ 84
#define PWM_BRUSHED_TIMER_MHZ 21
#elif defined(STM32F7) // must be multiples of timer clock
#define ONESHOT125_TIMER_MHZ 9
#define ONESHOT42_TIMER_MHZ 27
#define MULTISHOT_TIMER_MHZ 54
#define PWM_BRUSHED_TIMER_MHZ 27
#else
#define ONESHOT125_TIMER_MHZ 8
#define ONESHOT42_TIMER_MHZ 24
#define MULTISHOT_TIMER_MHZ 72
#define PWM_BRUSHED_TIMER_MHZ 24
#endif
#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
#define PROSHOT_DMA_BUFFER_SIZE 6/* resolution + frame reset (2us) */
#define PROSHOT_DMA_BUFFER_SIZE 6 /* resolution + frame reset (2us) */
typedef struct {
TIM_TypeDef *timer;
@ -149,9 +135,10 @@ typedef struct {
volatile timCCR_t *ccr;
TIM_TypeDef *tim;
bool forceOverflow;
uint16_t period;
bool enabled;
IO_t io;
float pulseScale;
float pulseOffset;
} pwmOutputPort_t;
typedef struct motorDevConfig_s {

26
src/main/drivers/rx_pwm.c
View File

@ -391,7 +391,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig)
IOConfigGPIO(io, IOCFG_AF_PP);
#endif
timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ);
timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_HZ);
timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback);
timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback);
timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
@ -408,7 +408,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig)
#define UNUSED_PPM_TIMER_REFERENCE 0
#define FIRST_PWM_PORT 0
void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol)
void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer)
{
pwmOutputPort_t *motors = pwmGetMotors();
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) {
@ -416,26 +416,12 @@ void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol)
continue;
}
switch (pwmProtocol)
{
case PWM_TYPE_ONESHOT125:
ppmCountDivisor = ONESHOT125_TIMER_MHZ;
break;
case PWM_TYPE_ONESHOT42:
ppmCountDivisor = ONESHOT42_TIMER_MHZ;
break;
case PWM_TYPE_MULTISHOT:
ppmCountDivisor = MULTISHOT_TIMER_MHZ;
break;
case PWM_TYPE_BRUSHED:
ppmCountDivisor = PWM_BRUSHED_TIMER_MHZ;
break;
}
ppmCountDivisor = timerClock(pwmTimer) / (pwmTimer->PSC + 1);
return;
}
}
void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
void ppmRxInit(const ppmConfig_t *ppmConfig)
{
ppmResetDevice();
@ -447,7 +433,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
return;
}
ppmAvoidPWMTimerClash(timer->tim, pwmProtocol);
ppmAvoidPWMTimerClash(timer->tim);
port->mode = INPUT_MODE_PPM;
port->timerHardware = timer;
@ -462,7 +448,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
IOConfigGPIO(io, IOCFG_AF_PP);
#endif
timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ);
timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_HZ);
timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback);
timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback);
timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);

2
src/main/drivers/rx_pwm.h
View File

@ -36,7 +36,7 @@ typedef struct pwmConfig_s {
inputFilteringMode_e inputFilteringMode;
} pwmConfig_t;
void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol);
void ppmRxInit(const ppmConfig_t *ppmConfig);
void pwmRxInit(const pwmConfig_t *pwmConfig);
uint16_t pwmRead(uint8_t channel);

12
src/main/drivers/serial_escserial.c
View File

@ -184,8 +184,7 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
}
} while (isTimerPeriodTooLarge(timerPeriod));
uint8_t mhz = clock / 1000000;
timerConfigure(timerHardwarePtr, timerPeriod, mhz);
timerConfigure(timerHardwarePtr, timerPeriod, clock);
timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerBL);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
}
@ -193,9 +192,8 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options)
{
// start bit is usually a FALLING signal
uint8_t mhz = SystemCoreClock / 2000000;
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, 0xFFFF, mhz);
timerConfigure(timerHardwarePtr, 0xFFFF, SystemCoreClock / 2);
escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
@ -203,9 +201,9 @@ static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
{
uint32_t timerPeriod=34;
uint32_t timerPeriod = 34;
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, timerPeriod, 1);
timerConfigure(timerHardwarePtr, timerPeriod, MHZ_TO_HZ(1));
timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
}
@ -228,7 +226,7 @@ static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint
{
// start bit is usually a FALLING signal
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, 0xFFFF, 1);
timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1));
escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);

6
src/main/drivers/serial_softserial.c
View File

@ -204,7 +204,7 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr, uint32_t baud)
{
uint32_t baseClock = SystemCoreClock / timerClockDivisor(timerHardwarePtr->tim);
uint32_t baseClock = timerClock(timerHardwarePtr->tim);
uint32_t clock = baseClock;
uint32_t timerPeriod;
@ -220,9 +220,7 @@ static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr
}
} while (isTimerPeriodTooLarge(timerPeriod));
uint8_t mhz = baseClock / 1000000;
timerConfigure(timerHardwarePtr, timerPeriod, mhz);
timerConfigure(timerHardwarePtr, timerPeriod, baseClock);
}
static void resetBuffers(softSerial_t *softSerial)

29
src/main/drivers/timer.c
View File

@ -229,16 +229,16 @@ void timerNVICConfigure(uint8_t irq)
NVIC_Init(&NVIC_InitStructure);
}
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Period = (period - 1) & 0xffff; // AKA TIMx_ARR
TIM_TimeBaseStructure.TIM_Period = (period - 1) & 0xFFFF; // AKA TIMx_ARR
// "The counter clock frequency (CK_CNT) is equal to f CK_PSC / (PSC[15:0] + 1)." - STM32F10x Reference Manual 14.4.11
// Thus for 1Mhz: 72000000 / 1000000 = 72, 72 - 1 = 71 = TIM_Prescaler
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(tim) / ((uint32_t)mhz * 1000000)) - 1;
TIM_TimeBaseStructure.TIM_Prescaler = (timerClock(tim) / hz) - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
@ -246,9 +246,9 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
}
// old interface for PWM inputs. It should be replaced
void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz)
void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint32_t hz)
{
configTimeBase(timerHardwarePtr->tim, period, mhz);
configTimeBase(timerHardwarePtr->tim, period, hz);
TIM_Cmd(timerHardwarePtr->tim, ENABLE);
uint8_t irq = timerInputIrq(timerHardwarePtr->tim);
@ -849,14 +849,19 @@ uint16_t timerDmaSource(uint8_t channel)
uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz)
{
// protection here for desired MHZ > timerClock???
if ((uint32_t)(mhz * 1000000) > timerClock(tim)) {
return 0;
}
return (uint16_t)(round((timerClock(tim) / (mhz * 1000000)) - 1));
return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz));
}
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz)
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz)
{
return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hertz));
return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hz));
}
uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz)
{
// protection here for desired hertz > SystemCoreClock???
if (hz > timerClock(tim)) {
return 0;
}
return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1;
}

9
src/main/drivers/timer.h
View File

@ -134,6 +134,8 @@ typedef enum {
#define HARDWARE_TIMER_DEFINITION_COUNT 14
#endif
#define MHZ_TO_HZ(x) (x * 1000000)
extern const timerHardware_t timerHardware[];
extern const timerDef_t timerDefinitions[];
@ -155,7 +157,7 @@ typedef enum {
TYPE_TIMER
} channelType_t;
void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint8_t mhz); // This interface should be replaced.
void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint32_t hz); // This interface should be replaced.
void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterSamples);
void timerChConfigICDual(const timerHardware_t* timHw, bool polarityRising, unsigned inputFilterSamples);
@ -183,7 +185,7 @@ void timerForceOverflow(TIM_TypeDef *tim);
uint8_t timerClockDivisor(TIM_TypeDef *tim);
uint32_t timerClock(TIM_TypeDef *tim);
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz); // TODO - just for migration
rccPeriphTag_t timerRCC(TIM_TypeDef *tim);
uint8_t timerInputIrq(TIM_TypeDef *tim);
@ -200,5 +202,6 @@ void timerOCPreloadConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t preload);
volatile timCCR_t *timerCCR(TIM_TypeDef *tim, uint8_t channel);
uint16_t timerDmaSource(uint8_t channel);
uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz);
uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz);
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz);
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz);

27
src/main/drivers/timer_hal.c
View File

@ -241,7 +241,7 @@ TIM_HandleTypeDef* timerFindTimerHandle(TIM_TypeDef *tim)
return &timerHandle[timerIndex].Handle;
}
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
{
uint8_t timerIndex = lookupTimerIndex(tim);
if (timerIndex >= USED_TIMER_COUNT) {
@ -256,7 +256,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
timerHandle[timerIndex].Handle.Instance = tim;
timerHandle[timerIndex].Handle.Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
timerHandle[timerIndex].Handle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(tim) / ((uint32_t)mhz * 1000000)) - 1;
timerHandle[timerIndex].Handle.Init.Prescaler = (timerClock(tim) / hz) - 1;
timerHandle[timerIndex].Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
timerHandle[timerIndex].Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
@ -287,14 +287,14 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
}
// old interface for PWM inputs. It should be replaced
void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz)
void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint32_t hz)
{
uint8_t timerIndex = lookupTimerIndex(timerHardwarePtr->tim);
if (timerIndex >= USED_TIMER_COUNT) {
return;
}
configTimeBase(timerHardwarePtr->tim, period, mhz);
configTimeBase(timerHardwarePtr->tim, period, hz);
HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle);
uint8_t irq = timerInputIrq(timerHardwarePtr->tim);
@ -898,14 +898,19 @@ uint16_t timerDmaSource(uint8_t channel)
uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz)
{
// protection here for desired MHZ > SystemCoreClock???
if (mhz * 1000000 > (SystemCoreClock / timerClockDivisor(tim))) {
return 0;
}
return (uint16_t)(round((SystemCoreClock / timerClockDivisor(tim) / (mhz * 1000000)) - 1));
return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz));
}
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz)
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz)
{
return ((uint16_t)((SystemCoreClock / timerClockDivisor(tim) / (prescaler + 1)) / hertz));
return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hz));
}
uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz)
{
// protection here for desired hertz > SystemCoreClock???
if (hz > timerClock(tim)) {
return 0;
}
return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1;
}

2
src/main/fc/fc_core.c
View File

@ -231,7 +231,7 @@ void mwArm(void)
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5)
beeper(BEEPER_ARMING_GPS_FIX);
else
beeper(BEEPER_ARMING);

6
src/main/fc/fc_init.c
View File

@ -384,7 +384,7 @@ void init(void)
if (0) {}
#if defined(USE_PPM)
else if (feature(FEATURE_RX_PPM)) {
ppmRxInit(ppmConfig(), motorConfig()->dev.motorPwmProtocol);
ppmRxInit(ppmConfig());
}
#endif
#if defined(USE_PWM)
@ -504,6 +504,10 @@ void init(void)
// gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit()
pidInit(currentPidProfile);
#ifdef USE_SERVOS
servosFilterInit();
#endif
imuInit();
mspFcInit();

34
src/main/fc/fc_msp.c
View File

@ -1113,12 +1113,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_RAW_GPS:
sbufWriteU8(dst, STATE(GPS_FIX));
sbufWriteU8(dst, GPS_numSat);
sbufWriteU32(dst, GPS_coord[LAT]);
sbufWriteU32(dst, GPS_coord[LON]);
sbufWriteU16(dst, GPS_altitude);
sbufWriteU16(dst, GPS_speed);
sbufWriteU16(dst, GPS_ground_course);
sbufWriteU8(dst, gpsSol.numSat);
sbufWriteU32(dst, gpsSol.llh.lat);
sbufWriteU32(dst, gpsSol.llh.lon);
sbufWriteU16(dst, gpsSol.llh.alt);
sbufWriteU16(dst, gpsSol.groundSpeed);
sbufWriteU16(dst, gpsSol.groundCourse);
break;
case MSP_COMP_GPS:
@ -1129,12 +1129,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_GPSSVINFO:
sbufWriteU8(dst, GPS_numCh);
for (int i = 0; i < GPS_numCh; i++) {
sbufWriteU8(dst, GPS_svinfo_chn[i]);
sbufWriteU8(dst, GPS_svinfo_svid[i]);
sbufWriteU8(dst, GPS_svinfo_quality[i]);
sbufWriteU8(dst, GPS_svinfo_cno[i]);
}
for (int i = 0; i < GPS_numCh; i++) {
sbufWriteU8(dst, GPS_svinfo_chn[i]);
sbufWriteU8(dst, GPS_svinfo_svid[i]);
sbufWriteU8(dst, GPS_svinfo_quality[i]);
sbufWriteU8(dst, GPS_svinfo_cno[i]);
}
break;
#endif
@ -1838,11 +1838,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
} else {
DISABLE_STATE(GPS_FIX);
}
GPS_numSat = sbufReadU8(src);
GPS_coord[LAT] = sbufReadU32(src);
GPS_coord[LON] = sbufReadU32(src);
GPS_altitude = sbufReadU16(src);
GPS_speed = sbufReadU16(src);
gpsSol.numSat = sbufReadU8(src);
gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src);
gpsSol.llh.alt = sbufReadU16(src);
gpsSol.groundSpeed = sbufReadU16(src);
GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
break;

5
src/main/fc/settings.c
View File

@ -53,6 +53,7 @@
#include "flight/pid.h"
#include "flight/servos.h"
#include "io/dashboard.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
@ -720,6 +721,10 @@ const clivalue_t valueTable[] = {
{ "esc_sensor_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, halfDuplex) },
#endif
{ "led_inversion", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, ((1 << STATUS_LED_NUMBER) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) },
#ifdef USE_DASHBOARD
{ "dashboard_i2c_bus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2CDEV_COUNT }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, device) },
{ "dashboard_i2c_addr", VAR_UINT8 | MASTER_VALUE, .config.minmax = { I2C_ADDR8_MIN, I2C_ADDR8_MAX }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, address) },
#endif
};
const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);

4
src/main/flight/imu.c
View File

@ -419,9 +419,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
useMag = true;
}
#if defined(GPS)
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) {
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - GPS_ground_course);
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - gpsSol.groundCourse);
useYaw = true;
}
#endif

14
src/main/flight/mixer.c
View File

@ -339,8 +339,13 @@ bool mixerIsOutputSaturated(int axis, float errorRate)
// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
// DSHOT scaling is done to the actual dshot range
void initEscEndpoints(void) {
switch(motorConfig()->dev.motorPwmProtocol) {
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
case PWM_TYPE_PROSHOT1000:
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
disarmMotorOutput = DSHOT_DISARM_COMMAND;
if (feature(FEATURE_3D))
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
@ -349,14 +354,17 @@ void initEscEndpoints(void) {
motorOutputHigh = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
} else
break;
#endif
{
default:
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
motorOutputLow = motorConfig()->minthrottle;
motorOutputHigh = motorConfig()->maxthrottle;
deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
break;
}
rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck);

47
src/main/flight/navigation.c
View File

@ -254,7 +254,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
} else {
@ -270,7 +270,7 @@ void onGpsNewData(void)
uint16_t speed;
if (!(STATE(GPS_FIX) && GPS_numSat >= 5)) {
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
return;
}
@ -284,7 +284,7 @@ void onGpsNewData(void)
#if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
for (axis = 0; axis < 2; axis++) {
GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
// How close we are to a degree line ? its the first three digits from the fractions of degree
@ -296,8 +296,13 @@ void onGpsNewData(void)
GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode...
if (fraction3[axis] > 1 && fraction3[axis] < 999)
GPS_coord[axis] = GPS_filtered[axis];
if (fraction3[axis] > 1 && fraction3[axis] < 999) {
if (axis == LAT) {
gpsSol.llh.lat = GPS_filtered[LAT];
} else {
gpsSol.llh.lon = GPS_filtered[LON];
}
}
}
}
#endif
@ -320,8 +325,8 @@ void onGpsNewData(void)
// we are navigating
// gps nav calculations, these are common for nav and poshold
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
switch (nav_mode) {
case NAV_MODE_POSHOLD:
@ -359,10 +364,10 @@ void onGpsNewData(void)
void GPS_reset_home_position(void)
{
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
GPS_home[LAT] = GPS_coord[LAT];
GPS_home[LON] = GPS_coord[LON];
GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
GPS_home[LAT] = gpsSol.llh.lat;
GPS_home[LON] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading
// Set ground altitude
ENABLE_STATE(GPS_FIX_HOME);
@ -429,10 +434,10 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
GPS_WP[LON] = *lon;
GPS_calc_longitude_scaling(*lat);
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
nav_bearing = target_bearing;
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
original_target_bearing = target_bearing;
waypoint_speed_gov = navigationConfig()->nav_speed_min;
}
@ -484,13 +489,11 @@ static void GPS_calc_velocity(void)
static int16_t speed_old[2] = { 0, 0 };
static int32_t last_coord[2] = { 0, 0 };
static uint8_t init = 0;
// y_GPS_speed positive = Up
// x_GPS_speed positive = Right
if (init) {
float tmp = 1.0f / dTnav;
actual_speed[GPS_X] = (float)(GPS_coord[LON] - last_coord[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last_coord[LAT]) * tmp;
actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp;
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
@ -500,8 +503,8 @@ static void GPS_calc_velocity(void)
}
init = 1;
last_coord[LON] = GPS_coord[LON];
last_coord[LAT] = GPS_coord[LAT];
last_coord[LON] = gpsSol.llh.lon;
last_coord[LAT] = gpsSol.llh.lat;
}
////////////////////////////////////////////////////////////////////////////////////
@ -666,7 +669,7 @@ void updateGpsWaypointsAndMode(void)
bool resetNavNow = false;
static bool gpsReadyBeepDone = false;
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
//
// process HOME mode
@ -701,8 +704,8 @@ void updateGpsWaypointsAndMode(void)
// Transition to HOLD mode
ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_hold[LAT] = gpsSol.llh.lat;
GPS_hold[LON] = gpsSol.llh.lon;
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
resetNavNow = true;

22
src/main/flight/servos.c
View File

@ -499,23 +499,25 @@ bool isMixerUsingServos(void)
return useServo;
}
static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
void servosFilterInit(void)
{
if (servoConfig()->servo_lowpass_freq) {
for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
}
}
}
static void filterServos(void)
{
static int16_t servoIdx;
static bool servoFilterIsSet;
static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
#if defined(MIXER_DEBUG)
uint32_t startTime = micros();
#endif
if (servoConfig()->servo_lowpass_freq) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
if (!servoFilterIsSet) {
biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
servoFilterIsSet = true;
}
for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
// Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max);

1
src/main/flight/servos.h
View File

@ -132,3 +132,4 @@ void loadCustomServoMixer(void);
int servoDirection(int servoIndex, int fromChannel);
void servoConfigureOutput(void);
void servosInit(void);
void servosFilterInit(void);

4
src/main/io/beeper.c
View File

@ -295,12 +295,12 @@ void beeperConfirmationBeeps(uint8_t beepCount)
void beeperGpsStatus(void)
{
// if GPS fix then beep out number of satellites
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
uint8_t i = 0;
do {
beep_multiBeeps[i++] = 5;
beep_multiBeeps[i++] = 10;
} while (i < MAX_MULTI_BEEPS && GPS_numSat > i / 2);
} while (i < MAX_MULTI_BEEPS && gpsSol.numSat > i / 2);
beep_multiBeeps[i-1] = 50; // extend last pause
beep_multiBeeps[i] = BEEPER_COMMAND_STOP;

277
src/main/io/dashboard.c
View File

@ -31,6 +31,7 @@
#include "build/build_config.h"
#include "drivers/bus.h"
#include "drivers/display.h"
#include "drivers/display_ug2864hsweg01.h"
#include "drivers/time.h"
@ -71,11 +72,20 @@
#include "sensors/sensors.h"
PG_REGISTER_WITH_RESET_TEMPLATE(dashboardConfig_t, dashboardConfig, PG_DASHBOARD_CONFIG, 0);
PG_RESET_TEMPLATE(dashboardConfig_t, dashboardConfig,
.device = I2C_DEV_TO_CFG(DASHBOARD_I2C_INSTANCE),
.address = DASHBOARD_I2C_ADDRESS,
);
#define MICROSECONDS_IN_A_SECOND (1000 * 1000)
#define DISPLAY_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5)
#define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5)
static busDevice_t *bus;
static uint32_t nextDisplayUpdateAt = 0;
static bool dashboardPresent = false;
@ -119,15 +129,17 @@ typedef struct pageState_s {
static pageState_t pageState;
void resetDisplay(void) {
dashboardPresent = ug2864hsweg01InitI2C();
static void resetDisplay(void)
{
dashboardPresent = ug2864hsweg01InitI2C(bus);
}
void LCDprint(uint8_t i) {
i2c_OLED_send_char(i);
void LCDprint(uint8_t i)
{
i2c_OLED_send_char(bus, i);
}
void padLineBuffer(void)
static void padLineBuffer(void)
{
uint8_t length = strlen(lineBuffer);
while (length < sizeof(lineBuffer) - 1) {
@ -136,7 +148,8 @@ void padLineBuffer(void)
lineBuffer[length] = 0;
}
void padHalfLineBuffer(void)
#ifdef GPS
static void padHalfLineBuffer(void)
{
uint8_t halfLineIndex = sizeof(lineBuffer) / 2;
uint8_t length = strlen(lineBuffer);
@ -145,9 +158,11 @@ void padHalfLineBuffer(void)
}
lineBuffer[length] = 0;
}
#endif
// LCDbar(n,v) : draw a bar graph - n number of chars for width, v value in % to display
void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) {
static void drawHorizonalPercentageBar(uint8_t width,uint8_t percent)
{
uint8_t i, j;
if (percent > 100)
@ -166,94 +181,91 @@ void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) {
}
#if 0
void fillScreenWithCharacters()
static void fillScreenWithCharacters()
{
for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) {
for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) {
i2c_OLED_set_xy(column, row);
i2c_OLED_send_char('A' + column);
i2c_OLED_set_xy(bus, column, row);
i2c_OLED_send_char(bus, 'A' + column);
}
}
}
#endif
void updateTicker(void)
static void updateTicker(void)
{
static uint8_t tickerIndex = 0;
i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
i2c_OLED_send_char(tickerCharacters[tickerIndex]);
i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
i2c_OLED_send_char(bus, tickerCharacters[tickerIndex]);
tickerIndex++;
tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
}
void updateRxStatus(void)
static void updateRxStatus(void)
{
i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
char rxStatus = '!';
if (rxIsReceivingSignal()) {
rxStatus = 'r';
} if (rxAreFlightChannelsValid()) {
rxStatus = 'R';
}
i2c_OLED_send_char(rxStatus);
i2c_OLED_send_char(bus, rxStatus);
}
void updateFailsafeStatus(void)
static void updateFailsafeStatus(void)
{
char failsafeIndicator = '?';
switch (failsafePhase()) {
case FAILSAFE_IDLE:
failsafeIndicator = '-';
break;
case FAILSAFE_RX_LOSS_DETECTED:
failsafeIndicator = 'R';
break;
case FAILSAFE_LANDING:
failsafeIndicator = 'l';
break;
case FAILSAFE_LANDED:
failsafeIndicator = 'L';
break;
case FAILSAFE_RX_LOSS_MONITORING:
failsafeIndicator = 'M';
break;
case FAILSAFE_RX_LOSS_RECOVERED:
failsafeIndicator = 'r';
break;
case FAILSAFE_IDLE:
failsafeIndicator = '-';
break;
case FAILSAFE_RX_LOSS_DETECTED:
failsafeIndicator = 'R';
break;
case FAILSAFE_LANDING:
failsafeIndicator = 'l';
break;
case FAILSAFE_LANDED:
failsafeIndicator = 'L';
break;
case FAILSAFE_RX_LOSS_MONITORING:
failsafeIndicator = 'M';
break;
case FAILSAFE_RX_LOSS_RECOVERED:
failsafeIndicator = 'r';
break;
}
i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
i2c_OLED_send_char(failsafeIndicator);
i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
i2c_OLED_send_char(bus, failsafeIndicator);
}
void showTitle()
static void showTitle()
{
i2c_OLED_set_line(0);
i2c_OLED_send_string(pageState.page->title);
i2c_OLED_set_line(bus, 0);
i2c_OLED_send_string(bus, pageState.page->title);
}
void handlePageChange(void)
static void handlePageChange(void)
{
i2c_OLED_clear_display_quick();
i2c_OLED_clear_display_quick(bus);
showTitle();
}
void drawRxChannel(uint8_t channelIndex, uint8_t width)
static void drawRxChannel(uint8_t channelIndex, uint8_t width)
{
uint32_t percentage;
LCDprint(rcChannelLetters[channelIndex]);
percentage = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
const uint32_t percentage = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
drawHorizonalPercentageBar(width - 1, percentage);
}
#define RX_CHANNELS_PER_PAGE_COUNT 14
void showRxPage(void)
static void showRxPage(void)
{
for (uint8_t channelIndex = 0; channelIndex < rxRuntimeConfig.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
i2c_OLED_set_line((channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
for (int channelIndex = 0; channelIndex < rxRuntimeConfig.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
i2c_OLED_set_line(bus, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
drawRxChannel(channelIndex, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
@ -269,29 +281,29 @@ void showRxPage(void)
}
}
void showWelcomePage(void)
static void showWelcomePage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision);
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(targetName);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, targetName);
}
void showArmedPage(void)
static void showArmedPage(void)
{
}
void showProfilePage(void)
static void showProfilePage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
tfp_sprintf(lineBuffer, "Profile: %d", getCurrentPidProfileIndex());
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"};
const pidProfile_t *pidProfile = currentPidProfile;
@ -303,14 +315,14 @@ void showProfilePage(void)
pidProfile->pid[axis].D
);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
}
const uint8_t currentRateProfileIndex = getCurrentControlRateProfileIndex();
tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex);
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
const controlRateConfig_t *controlRateConfig = controlRateProfiles(currentRateProfileIndex);
tfp_sprintf(lineBuffer, "RCE: %d, RCR: %d",
@ -318,8 +330,8 @@ void showProfilePage(void)
controlRateConfig->rcRate8
);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "RR:%d PR:%d YR:%d",
controlRateConfig->rates[FD_ROLL],
@ -327,15 +339,15 @@ void showProfilePage(void)
controlRateConfig->rates[FD_YAW]
);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
}
#define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0]))
#define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
#ifdef GPS
void showGpsPage() {
static void showGpsPage()
{
if (!feature(FEATURE_GPS)) {
pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
return;
@ -351,93 +363,93 @@ void showGpsPage() {
gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT;
}
i2c_OLED_set_xy(0, rowIndex);
i2c_OLED_send_char(tickerCharacters[gpsTicker]);
i2c_OLED_set_xy(bus, 0, rowIndex);
i2c_OLED_send_char(bus, tickerCharacters[gpsTicker]);
i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
i2c_OLED_set_xy(bus, MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
uint32_t index;
for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
i2c_OLED_send_char(bus, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
}
char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", GPS_numSat, fixChar);
tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "La/Lo: %d/%d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER);
tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "Spd: %d", GPS_speed);
tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "GC: %d", GPS_ground_course);
tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse);
padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors, gpsData.timeouts);
padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "Dt: %d", gpsData.lastMessage - gpsData.lastLastMessage);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts);
padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
#ifdef GPS_PH_DEBUG
tfp_sprintf(lineBuffer, "Angles: P:%d R:%d", GPS_angle[PITCH], GPS_angle[ROLL]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
#endif
#if 0
tfp_sprintf(lineBuffer, "%d %d %d %d", debug[0], debug[1], debug[2], debug[3]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
#endif
}
#endif
void showBatteryPage(void)
static void showBatteryPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getBatteryVoltage() / 10, getBatteryVoltage() % 10, getBatteryCellCount());
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
uint8_t batteryPercentage = calculateBatteryPercentageRemaining();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_set_line(bus, rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
}
@ -446,50 +458,50 @@ void showBatteryPage(void)
int32_t amperage = getAmperage();
tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, getMAhDrawn());
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
uint8_t capacityPercentage = calculateBatteryPercentageRemaining();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_set_line(bus, rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
}
}
void showSensorsPage(void)
static void showSensorsPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
static const char *format = "%s %5d %5d %5d";
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(" X Y Z");
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, " X Y Z");
if (sensors(SENSOR_ACC)) {
tfp_sprintf(lineBuffer, format, "ACC", acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
}
if (sensors(SENSOR_GYRO)) {
tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyro.gyroADCf[X]), lrintf(gyro.gyroADCf[Y]), lrintf(gyro.gyroADCf[Z]));
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
}
#ifdef MAG
if (sensors(SENSOR_MAG)) {
tfp_sprintf(lineBuffer, format, "MAG", mag.magADC[X], mag.magADC[Y], mag.magADC[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
}
#endif
tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
/*
uint8_t length;
@ -502,8 +514,8 @@ void showSensorsPage(void)
}
ftoa(EstG.A[Y], lineBuffer + length);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
ftoa(EstG.A[Z], lineBuffer);
length = strlen(lineBuffer);
@ -513,20 +525,20 @@ void showSensorsPage(void)
}
ftoa(smallAngle, lineBuffer + length);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
*/
}
#ifndef SKIP_TASK_STATISTICS
void showTasksPage(void)
static void showTasksPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
static const char *format = "%2d%6d%5d%4d%4d";
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string("Task max avg mx% av%");
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, "Task max avg mx% av%");
cfTaskInfo_t taskInfo;
for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) {
getTaskInfo(taskId, &taskInfo);
@ -536,8 +548,8 @@ void showTasksPage(void)
const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 10000;
tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, maxLoad, averageLoad);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
if (rowIndex > SCREEN_CHARACTER_ROW_COUNT) {
break;
}
@ -548,13 +560,13 @@ void showTasksPage(void)
#ifdef ENABLE_DEBUG_DASHBOARD_PAGE
void showDebugPage(void)
static void showDebugPage(void)
{
for (int rowIndex = 0; rowIndex < 4; rowIndex++) {
tfp_sprintf(lineBuffer, "%d = %5d", rowIndex, debug[rowIndex]);
padLineBuffer();
i2c_OLED_set_line(rowIndex + PAGE_TITLE_LINE_COUNT);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex + PAGE_TITLE_LINE_COUNT);
i2c_OLED_send_string(bus, lineBuffer);
}
}
#endif
@ -664,11 +676,16 @@ void dashboardUpdate(timeUs_t currentTimeUs)
void dashboardInit(void)
{
static busDevice_t dashBoardBus;
dashBoardBus.i2c.device = I2C_CFG_TO_DEV(dashboardConfig()->device);
dashBoardBus.i2c.address = dashboardConfig()->address;
bus = &dashBoardBus;
delay(200);
resetDisplay();
delay(200);
displayPort = displayPortOledInit();
displayPort = displayPortOledInit(bus);
#if defined(CMS)
if (dashboardPresent) {
cmsDisplayPortRegister(displayPort);

17
src/main/io/dashboard.h
View File

@ -16,9 +16,26 @@
*/
#include "common/time.h"
#include "config/parameter_group.h"
#include "drivers/bus_i2c.h"
#define ENABLE_DEBUG_DASHBOARD_PAGE
#ifdef OLED_I2C_INSTANCE
#define DASHBOARD_I2C_INSTANCE OLED_I2C_INSTANCE
#else
#define DASHBOARD_I2C_INSTANCE I2CDEV_1
#endif
#define DASHBOARD_I2C_ADDRESS 0x3C // OLED at address 0x3C in 7bit
typedef struct dashboardConfig_s {
I2CDevice device;
uint8_t address;
} dashboardConfig_t;
PG_DECLARE(dashboardConfig_t, dashboardConfig);
typedef enum {
PAGE_WELCOME,
PAGE_ARMED,

4
src/main/io/displayport_max7456.c
View File

@ -95,7 +95,7 @@ static int screenSize(const displayPort_t *displayPort)
return maxScreenSize;
}
static int write(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
static int writeString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
{
UNUSED(displayPort);
max7456Write(x, y, s);
@ -143,7 +143,7 @@ static const displayPortVTable_t max7456VTable = {
.clearScreen = clearScreen,
.drawScreen = drawScreen,
.screenSize = screenSize,
.write = write,
.writeString = writeString,
.writeChar = writeChar,
.isTransferInProgress = isTransferInProgress,
.heartbeat = heartbeat,

6
src/main/io/displayport_msp.c
View File

@ -100,7 +100,7 @@ static int screenSize(const displayPort_t *displayPort)
return displayPort->rows * displayPort->cols;
}
static int write(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string)
static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string)
{
#define MSP_OSD_MAX_STRING_LENGTH 30 // FIXME move this
uint8_t buf[MSP_OSD_MAX_STRING_LENGTH + 4];
@ -125,7 +125,7 @@ static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8
buf[0] = c;
buf[1] = 0;
return write(displayPort, col, row, buf); //!!TODO - check if there is a direct MSP command to do this
return writeString(displayPort, col, row, buf); //!!TODO - check if there is a direct MSP command to do this
}
static bool isTransferInProgress(const displayPort_t *displayPort)
@ -152,7 +152,7 @@ static const displayPortVTable_t mspDisplayPortVTable = {
.clearScreen = clearScreen,
.drawScreen = drawScreen,
.screenSize = screenSize,
.write = write,
.writeString = writeString,
.writeChar = writeChar,
.isTransferInProgress = isTransferInProgress,
.heartbeat = heartbeat,

20
src/main/io/displayport_oled.c
View File

@ -41,8 +41,7 @@ static int oledRelease(displayPort_t *displayPort)
static int oledClearScreen(displayPort_t *displayPort)
{
UNUSED(displayPort);
i2c_OLED_clear_display_quick();
i2c_OLED_clear_display_quick(displayPort->device);
return 0;
}
@ -57,19 +56,17 @@ static int oledScreenSize(const displayPort_t *displayPort)
return displayPort->rows * displayPort->cols;
}
static int oledWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
static int oledWriteString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
{
UNUSED(displayPort);
i2c_OLED_set_xy(x, y);
i2c_OLED_send_string(s);
i2c_OLED_set_xy(displayPort->device, x, y);
i2c_OLED_send_string(displayPort->device, s);
return 0;
}
static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c)
{
UNUSED(displayPort);
i2c_OLED_set_xy(x, y);
i2c_OLED_send_char(c);
i2c_OLED_set_xy(displayPort->device, x, y);
i2c_OLED_send_char(displayPort->device, c);
return 0;
}
@ -102,7 +99,7 @@ static const displayPortVTable_t oledVTable = {
.clearScreen = oledClearScreen,
.drawScreen = oledDrawScreen,
.screenSize = oledScreenSize,
.write = oledWrite,
.writeString = oledWriteString,
.writeChar = oledWriteChar,
.isTransferInProgress = oledIsTransferInProgress,
.heartbeat = oledHeartbeat,
@ -110,8 +107,9 @@ static const displayPortVTable_t oledVTable = {
.txBytesFree = oledTxBytesFree
};
displayPort_t *displayPortOledInit(void)
displayPort_t *displayPortOledInit(void *device)
{
oledDisplayPort.device = device;
displayInit(&oledDisplayPort, &oledVTable);
oledDisplayPort.rows = SCREEN_CHARACTER_ROW_COUNT;
oledDisplayPort.cols = SCREEN_CHARACTER_COLUMN_COUNT;

2
src/main/io/displayport_oled.h
View File

@ -17,4 +17,4 @@
#pragma once
displayPort_t *displayPortOledInit(void);
displayPort_t *displayPortOledInit(void *device);

40
src/main/io/gps.c
View File

@ -71,18 +71,11 @@ static char *gpsPacketLogChar = gpsPacketLog;
// **********************
// GPS
// **********************
int32_t GPS_coord[2]; // LAT/LON
uint8_t GPS_numSat;
uint16_t GPS_hdop = 9999; // Compute GPS quality signal
gpsSolutionData_t gpsSol;
uint32_t GPS_packetCount = 0;
uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
uint8_t GPS_update = 0; // it's a binary toggle to distinct a GPS position update
uint16_t GPS_altitude; // altitude in 0.1m
uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10
uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number
uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID
@ -424,7 +417,7 @@ void gpsInitHardware(void)
static void updateGpsIndicator(timeUs_t currentTimeUs)
{
static uint32_t GPSLEDTime;
if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat >= 5)) {
GPSLEDTime = currentTimeUs + 150000;
LED1_TOGGLE;
}
@ -456,8 +449,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
}
gpsData.lastMessage = millis();
// TODO - move some / all of these into gpsData
GPS_numSat = 0;
gpsSol.numSat = 0;
DISABLE_STATE(GPS_FIX);
gpsSetState(GPS_INITIALIZING);
break;
@ -746,16 +738,16 @@ static bool gpsNewFrameNMEA(char c)
*gpsPacketLogChar = LOG_NMEA_GGA;
frameOK = 1;
if (STATE(GPS_FIX)) {
GPS_coord[LAT] = gps_Msg.latitude;
GPS_coord[LON] = gps_Msg.longitude;
GPS_numSat = gps_Msg.numSat;
GPS_altitude = gps_Msg.altitude;
gpsSol.llh.lat = gps_Msg.latitude;
gpsSol.llh.lon = gps_Msg.longitude;
gpsSol.numSat = gps_Msg.numSat;
gpsSol.llh.alt = gps_Msg.altitude;
}
break;
case FRAME_RMC:
*gpsPacketLogChar = LOG_NMEA_RMC;
GPS_speed = gps_Msg.speed;
GPS_ground_course = gps_Msg.ground_course;
gpsSol.groundSpeed = gps_Msg.speed;
gpsSol.groundCourse = gps_Msg.ground_course;
break;
} // end switch
} else {
@ -952,9 +944,9 @@ static bool UBLOX_parse_gps(void)
case MSG_POSLLH:
*gpsPacketLogChar = LOG_UBLOX_POSLLH;
//i2c_dataset.time = _buffer.posllh.time;
GPS_coord[LON] = _buffer.posllh.longitude;
GPS_coord[LAT] = _buffer.posllh.latitude;
GPS_altitude = _buffer.posllh.altitude_msl / 10 / 100; //alt in m
gpsSol.llh.lon = _buffer.posllh.longitude;
gpsSol.llh.lat = _buffer.posllh.latitude;
gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10 / 100; //alt in m
if (next_fix) {
ENABLE_STATE(GPS_FIX);
} else {
@ -973,14 +965,14 @@ static bool UBLOX_parse_gps(void)
next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
if (!next_fix)
DISABLE_STATE(GPS_FIX);
GPS_numSat = _buffer.solution.satellites;
GPS_hdop = _buffer.solution.position_DOP;
gpsSol.numSat = _buffer.solution.satellites;
gpsSol.hdop = _buffer.solution.position_DOP;
break;
case MSG_VELNED:
*gpsPacketLogChar = LOG_UBLOX_VELNED;
// speed_3d = _buffer.velned.speed_3d; // cm/s
GPS_speed = _buffer.velned.speed_2d; // cm/s
GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s
gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
_new_speed = true;
break;
case MSG_SVINFO:

24
src/main/io/gps.h
View File

@ -31,8 +31,6 @@ typedef enum {
GPS_UBLOX
} gpsProvider_e;
#define GPS_PROVIDER_MAX GPS_UBLOX
typedef enum {
SBAS_AUTO = 0,
SBAS_EGNOS,
@ -77,6 +75,21 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
int16_t mmmm;
} gpsCoordinateDDDMMmmmm_t;
/* LLH Location in NEU axis system */
typedef struct gpsLocation_s {
int32_t lat; // latitude * 1e+7
int32_t lon; // longitude * 1e+7
uint16_t alt; // altitude in 0.1m
} gpsLocation_t;
typedef struct gpsSolutionData_s {
uint8_t numSat;
gpsLocation_t llh;
uint16_t GPS_altitude; // altitude in 0.1m
uint16_t groundSpeed; // speed in 0.1m/s
uint16_t groundCourse; // degrees * 10
uint16_t hdop; // generic HDOP value (*100)
} gpsSolutionData_t;
typedef enum {
GPS_MESSAGE_STATE_IDLE = 0,
@ -102,16 +115,11 @@ typedef struct gpsData_s {
extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
extern gpsData_t gpsData;
extern int32_t GPS_coord[2]; // LAT/LON
extern gpsSolutionData_t gpsSol;
extern uint8_t GPS_numSat;
extern uint16_t GPS_hdop; // GPS signal quality
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
extern uint32_t GPS_packetCount;
extern uint32_t GPS_svInfoReceivedCount;
extern uint16_t GPS_altitude; // altitude in 0.1m
extern uint16_t GPS_speed; // speed in 0.1m/s
extern uint16_t GPS_ground_course; // degrees * 10
extern uint8_t GPS_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID

4
src/main/io/ledstrip.c
View File

@ -731,7 +731,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
static uint8_t gpsFlashCounter = 0;
if (gpsPauseCounter > 0) {
gpsPauseCounter--;
} else if (gpsFlashCounter >= GPS_numSat) {
} else if (gpsFlashCounter >= gpsSol.numSat) {
gpsFlashCounter = 0;
gpsPauseCounter = blinkPauseLength;
} else {
@ -743,7 +743,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
const hsvColor_t *gpsColor;
if (GPS_numSat == 0 || !sensors(SENSOR_GPS)) {
if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) {
gpsColor = getSC(LED_SCOLOR_GPSNOSATS);
} else {
bool colorOn = gpsPauseCounter == 0; // each interval starts with pause

32
src/main/io/osd.c
View File

@ -36,6 +36,7 @@
#include "blackbox/blackbox.h"
#include "blackbox/blackbox_io.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "build/version.h"
@ -88,13 +89,6 @@
#define VIDEO_BUFFER_CHARS_PAL 480
// Character coordinate
#define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
#define OSD_POS(x,y) ((x & 0x001F) | ((y & 0x001F) << OSD_POSITION_BITS))
#define OSD_X(x) (x & 0x001F)
#define OSD_Y(x) ((x >> OSD_POSITION_BITS) & 0x001F)
// Blink control
static bool blinkState = true;
@ -168,9 +162,9 @@ static char osdGetMetersToSelectedUnitSymbol()
{
switch (osdConfig()->units) {
case OSD_UNIT_IMPERIAL:
return 0xF;
return SYM_FT;
default:
return 0xC;
return SYM_M;
}
}
@ -277,12 +271,12 @@ static void osdDrawSingleElement(uint8_t item)
#ifdef GPS
case OSD_GPS_SATS:
buff[0] = 0x1f;
tfp_sprintf(buff + 1, "%d", GPS_numSat);
tfp_sprintf(buff + 1, "%d", gpsSol.numSat);
break;
case OSD_GPS_SPEED:
// FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K.
tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(GPS_speed));
tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed));
break;
case OSD_GPS_LAT:
@ -291,10 +285,10 @@ static void osdDrawSingleElement(uint8_t item)
int32_t val;
if (item == OSD_GPS_LAT) {
buff[0] = SYM_ARROW_EAST;
val = GPS_coord[LAT];
val = gpsSol.llh.lat;
} else {
buff[0] = SYM_ARROW_SOUTH;
val = GPS_coord[LON];
val = gpsSol.llh.lon;
}
char wholeDegreeString[5];
@ -761,10 +755,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
static void osdDrawLogo(int x, int y)
{
// display logo and help
char fontOffset = 160;
int fontOffset = 160;
for (int row = 0; row < 4; row++) {
for (int column = 0; column < 24; column++) {
if (fontOffset != 255) // FIXME magic number
if (fontOffset <= SYM_END_OF_FONT)
displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++);
}
}
@ -807,10 +801,8 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
void osdUpdateAlarms(void)
{
// This is overdone?
// uint16_t *itemPos = osdConfig()->item_pos;
int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100;
statRssi = rssi * 100 / 1024;
if (statRssi < osdConfig()->rssi_alarm)
SET_BLINK(OSD_RSSI_VALUE);
@ -880,7 +872,7 @@ static void osdUpdateStats(void)
{
int16_t value = 0;
#ifdef GPS
value = CM_S_TO_KM_H(GPS_speed);
value = CM_S_TO_KM_H(gpsSol.groundSpeed);
#endif
if (stats.max_speed < value)
stats.max_speed = value;
@ -1039,7 +1031,7 @@ static void osdShowArmed(void)
displayWrite(osdDisplayPort, 12, 7, "ARMED");
}
static void osdRefresh(timeUs_t currentTimeUs)
STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
{
static uint8_t lastSec = 0;
uint8_t sec;
@ -1058,6 +1050,8 @@ static void osdRefresh(timeUs_t currentTimeUs)
armState = ARMING_FLAG(ARMED);
}
statRssi = scaleRange(rssi, 0, 1024, 0, 100);
osdUpdateStats();
sec = currentTimeUs / 1000000;

8
src/main/io/osd.h
View File

@ -26,6 +26,14 @@
#define OSD_POS_MAX 0x3FF
#define OSD_POSCFG_MAX (VISIBLE_FLAG|0x3FF) // For CLI values
// Character coordinate
#define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
#define OSD_POSITION_XY_MASK ((1 << OSD_POSITION_BITS) - 1)
#define OSD_POS(x,y) ((x & OSD_POSITION_XY_MASK) | ((y & OSD_POSITION_XY_MASK) << OSD_POSITION_BITS))
#define OSD_X(x) (x & OSD_POSITION_XY_MASK)
#define OSD_Y(x) ((x >> OSD_POSITION_BITS) & OSD_POSITION_XY_MASK)
typedef enum {
OSD_RSSI_VALUE,
OSD_MAIN_BATT_VOLTAGE,

4
src/main/io/serial.c
View File

@ -38,8 +38,6 @@
#include "drivers/serial_softserial.h"
#endif
#define USE_SERIAL (defined(USE_UART) || defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
#ifdef SITL
#include "drivers/serial_tcp.h"
#endif
@ -332,7 +330,7 @@ serialPort_t *openSerialPort(
portMode_t mode,
portOptions_t options)
{
#if !(USE_SERIAL)
#if !(defined(USE_UART) || defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
UNUSED(rxCallback);
UNUSED(baudRate);
UNUSED(mode);

1
src/main/target/CC3D/target.c
View File

@ -22,6 +22,7 @@
#include "drivers/timer.h"
#include "drivers/timer_def.h"
#include "drivers/dma.h"
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {

3
src/main/target/CC3D/target.h
View File

@ -114,9 +114,10 @@
#undef USE_SERIALRX_SUMD // Graupner Hott protocol
#undef USE_SERIALRX_SUMH // Graupner legacy protocol
#undef USE_SERIALRX_XBUS // JR
#undef LED_STRIP
#endif
#undef LED_STRIP
//#undef LED_STRIP
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
// IO - from schematics

14
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -778,15 +778,15 @@ static uint8_t numOfSat = 0;
#ifdef GPS
bool writeGpsPositionPrameToBST(void)
{
if((lat != GPS_coord[LAT]) || (lon != GPS_coord[LON]) || (alt != GPS_altitude) || (numOfSat != GPS_numSat)) {
lat = GPS_coord[LAT];
lon = GPS_coord[LON];
alt = GPS_altitude;
numOfSat = GPS_numSat;
uint16_t speed = (GPS_speed * 9 / 25);
if((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != gpsSol.llh.alt) || (numOfSat != gpsSol.numSat)) {
lat = gpsSol.llh.lat;
lon = gpsSol.llh.lon;
alt = gpsSol.llh.alt;
numOfSat = gpsSol.numSat;
uint16_t speed = (gpsSol.groundSpeed * 9 / 25);
uint16_t gpsHeading = 0;
uint16_t altitude = 0;
gpsHeading = GPS_ground_course * 10;
gpsHeading = gpsSol.groundCourse * 10;
altitude = alt * 10 + 1000;
bstMasterStartBuffer(PUBLIC_ADDRESS);

2
src/main/target/KROOZX/config.c
View File

@ -30,8 +30,6 @@
#define VBAT_SCALE 113
#define OSD_POS(x,y) (x | (y << 5))
#ifdef TARGET_CONFIG
void targetConfiguration(void)
{

1
src/main/target/common_fc_pre.h
View File

@ -36,6 +36,7 @@
// Using RX DMA disables the use of receive callbacks
#define USE_UART1_RX_DMA
#define USE_UART1_TX_DMA
#define MAX_SUPPORTED_MOTORS 8
#endif
#ifdef STM32F3

16
src/main/telemetry/crsf.c
View File

@ -111,7 +111,7 @@ int32_t Latitude ( degree / 10`000`000 )
int32_t Longitude (degree / 10`000`000 )
uint16_t Groundspeed ( km/h / 10 )
uint16_t GPS heading ( degree / 100 )
uint16 Altitude ( meter ­ 1000m offset )
uint16 Altitude ( meter ­1000m offset )
uint8_t Satellites in use ( counter )
*/
void crsfFrameGps(sbuf_t *dst)
@ -119,14 +119,14 @@ void crsfFrameGps(sbuf_t *dst)
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
sbufWriteU32BigEndian(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees
sbufWriteU32BigEndian(dst, GPS_coord[LON]);
sbufWriteU16BigEndian(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s
sbufWriteU16BigEndian(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10
sbufWriteU32BigEndian(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
sbufWriteU32BigEndian(dst, gpsSol.llh.lon);
sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 5) / 10); // gpsSol.groundSpeed is in 0.1m/s
sbufWriteU16BigEndian(dst, gpsSol.groundCourse * 10); // gpsSol.groundCourse is degrees * 10
//Send real GPS altitude only if it's reliable (there's a GPS fix)
const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000;
const uint16_t altitude = (STATE(GPS_FIX) ? gpsSol.llh.alt : 0) + 1000;
sbufWriteU16BigEndian(dst, altitude);
sbufWriteU8(dst, GPS_numSat);
sbufWriteU8(dst, gpsSol.numSat);
}
/*
@ -204,7 +204,7 @@ void crsfFrameAttitude(sbuf_t *dst)
/*
0x21 Flight mode text based
Payload:
char[] Flight mode ( Null­terminated string )
char[] Flight mode ( Null terminated string )
*/
void crsfFrameFlightMode(sbuf_t *dst)
{

17
src/main/telemetry/frsky.c
View File

@ -183,7 +183,7 @@ static void sendBaro(void)
#ifdef GPS
static void sendGpsAltitude(void)
{
uint16_t altitude = GPS_altitude;
uint16_t altitude = gpsSol.llh.alt;
//Send real GPS altitude only if it's reliable (there's a GPS fix)
if (!STATE(GPS_FIX)) {
altitude = 0;
@ -230,9 +230,9 @@ static void sendTemperature1(void)
#ifdef GPS
static void sendSatalliteSignalQualityAsTemperature2(void)
{
uint16_t satellite = GPS_numSat;
if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL);
uint16_t satellite = gpsSol.numSat;
if (gpsSol.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL);
}
sendDataHead(ID_TEMPRATURE2);
@ -254,9 +254,9 @@ static void sendSpeed(void)
//Speed should be sent in knots (GPS speed is in cm/s)
sendDataHead(ID_GPS_SPEED_BP);
//convert to knots: 1cm/s = 0.0194384449 knots
serialize16(GPS_speed * 1944 / 100000);
serialize16(gpsSol.groundSpeed * 1944 / 100000);
sendDataHead(ID_GPS_SPEED_AP);
serialize16((GPS_speed * 1944 / 100) % 100);
serialize16((gpsSol.groundSpeed * 1944 / 100) % 100);
}
#endif
@ -337,11 +337,14 @@ static void sendFakeLatLong(void)
static void sendGPSLatLong(void)
{
static uint8_t gpsFixOccured = 0;
int32_t coord[2] = {0,0};
if (STATE(GPS_FIX) || gpsFixOccured == 1) {
// If we have ever had a fix, send the last known lat/long
gpsFixOccured = 1;
sendLatLong(GPS_coord);
coord[LAT] = gpsSol.llh.lat;
coord[LON] = gpsSol.llh.lon;
sendLatLong(coord);
} else {
// otherwise send fake lat/long in order to display compass value
sendFakeLatLong();

12
src/main/telemetry/hott.c
View File

@ -185,35 +185,35 @@ void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t
void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
{
hottGPSMessage->gps_satelites = GPS_numSat;
hottGPSMessage->gps_satelites = gpsSol.numSat;
if (!STATE(GPS_FIX)) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
return;
}
if (GPS_numSat >= 5) {
if (gpsSol.numSat >= 5) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
} else {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
}
addGPSCoordinates(hottGPSMessage, GPS_coord[LAT], GPS_coord[LON]);
addGPSCoordinates(hottGPSMessage, gpsSol.llh.lat, gpsSol.llh.lon);
// GPS Speed is returned in cm/s (from io/gps.c) and must be sent in km/h (Hott requirement)
const uint16_t speed = (GPS_speed * 36) / 1000;
const uint16_t speed = (gpsSol.groundSpeed * 36) / 1000;
hottGPSMessage->gps_speed_L = speed & 0x00FF;
hottGPSMessage->gps_speed_H = speed >> 8;
hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;
uint16_t altitude = GPS_altitude;
uint16_t altitude = gpsSol.llh.alt;
if (!STATE(GPS_FIX)) {
altitude = getEstimatedAltitude() / 100;
}
const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // GPS_altitude in m ; offset = 500 -> O m
const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // gpsSol.llh.alt in m ; offset = 500 -> O m
hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
hottGPSMessage->altitude_H = hottGpsAltitude >> 8;

14
src/main/telemetry/ltm.c
View File

@ -133,23 +133,23 @@ static void ltm_gframe(void)
if (!STATE(GPS_FIX))
gps_fix_type = 1;
else if (GPS_numSat < 5)
else if (gpsSol.numSat < 5)
gps_fix_type = 2;
else
gps_fix_type = 3;
ltm_initialise_packet('G');
ltm_serialise_32(GPS_coord[LAT]);
ltm_serialise_32(GPS_coord[LON]);
ltm_serialise_8((uint8_t)(GPS_speed / 100));
ltm_serialise_32(gpsSol.llh.lat);
ltm_serialise_32(gpsSol.llh.lon);
ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100));
#if defined(BARO) || defined(SONAR)
ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : GPS_altitude * 100;
ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100;
#else
ltm_alt = GPS_altitude * 100;
ltm_alt = gpsSol.llh.alt * 100;
#endif
ltm_serialise_32(ltm_alt);
ltm_serialise_8((GPS_numSat << 2) | gps_fix_type);
ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type);
ltm_finalise();
#endif
}

30
src/main/telemetry/mavlink.c
View File

@ -286,7 +286,7 @@ void mavlinkSendPosition(void)
gpsFixType = 1;
}
else {
if (GPS_numSat < 5) {
if (gpsSol.numSat < 5) {
gpsFixType = 2;
}
else {
@ -300,21 +300,21 @@ void mavlinkSendPosition(void)
// fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
gpsFixType,
// lat Latitude in 1E7 degrees
GPS_coord[LAT],
gpsSol.llh.lat,
// lon Longitude in 1E7 degrees
GPS_coord[LON],
gpsSol.llh.lon,
// alt Altitude in 1E3 meters (millimeters) above MSL
GPS_altitude * 1000,
gpsSol.llh.alt * 1000,
// eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
65535,
// epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
65535,
// vel GPS ground speed (m/s * 100). If unknown, set to: 65535
GPS_speed,
gpsSol.groundSpeed,
// cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
GPS_ground_course * 10,
gpsSol.groundCourse * 10,
// satellites_visible Number of satellites visible. If unknown, set to 255
GPS_numSat);
gpsSol.numSat);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
@ -323,16 +323,16 @@ void mavlinkSendPosition(void)
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
micros(),
// lat Latitude in 1E7 degrees
GPS_coord[LAT],
gpsSol.llh.lat,
// lon Longitude in 1E7 degrees
GPS_coord[LON],
gpsSol.llh.lon,
// alt Altitude in 1E3 meters (millimeters) above MSL
GPS_altitude * 1000,
gpsSol.llh.alt * 1000,
// relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
#if defined(BARO) || defined(SONAR)
(sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : GPS_altitude * 1000,
(sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : gpsSol.llh.alt * 1000,
#else
GPS_altitude * 1000,
gpsSol.llh.alt * 1000,
#endif
// Ground X Speed (Latitude), expressed as m/s * 100
0,
@ -391,7 +391,7 @@ void mavlinkSendHUDAndHeartbeat(void)
#if defined(GPS)
// use ground speed if source available
if (sensors(SENSOR_GPS)) {
mavGroundSpeed = GPS_speed / 100.0f;
mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
}
#endif
@ -404,13 +404,13 @@ void mavlinkSendHUDAndHeartbeat(void)
#if defined(GPS)
else if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude;
mavAltitude = gpsSol.llh.alt;
}
#endif
#elif defined(GPS)
if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude;
mavAltitude = gpsSol.llh.alt;
}
#endif

14
src/main/telemetry/smartport.c
View File

@ -602,7 +602,7 @@ void handleSmartPortTelemetry(void)
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
//convert to knots: 1cm/s = 0.0194384449 knots
//Speed should be sent in knots/1000 (GPS speed is in cm/s)
uint32_t tmpui = GPS_speed * 1944 / 100;
uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
smartPortSendPackage(id, tmpui);
smartPortHasRequest = 0;
}
@ -649,14 +649,14 @@ void handleSmartPortTelemetry(void)
// the MSB of the sent uint32_t helps FrSky keep track
// the even/odd bit of our counter helps us keep track
if (smartPortIdCnt & 1) {
tmpui = abs(GPS_coord[LON]); // now we have unsigned value and one bit to spare
tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare
tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
if (GPS_coord[LON] < 0) tmpui |= 0x40000000;
if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
}
else {
tmpui = abs(GPS_coord[LAT]); // now we have unsigned value and one bit to spare
tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare
tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
if (GPS_coord[LAT] < 0) tmpui |= 0x40000000;
if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
}
smartPortSendPackage(id, tmpui);
smartPortHasRequest = 0;
@ -735,7 +735,7 @@ void handleSmartPortTelemetry(void)
if (sensors(SENSOR_GPS)) {
#ifdef GPS
// provide GPS lock status
smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);
smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + gpsSol.numSat);
smartPortHasRequest = 0;
#endif
} else if (feature(FEATURE_GPS)) {
@ -776,7 +776,7 @@ void handleSmartPortTelemetry(void)
#ifdef GPS
case FSSP_DATAID_GPS_ALT :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
smartPortHasRequest = 0;
}
break;

15
src/test/Makefile
View File

@ -107,6 +107,17 @@ maths_unittest_SRC := \
$(USER_DIR)/common/maths.c
osd_unittest_SRC := \
$(USER_DIR)/io/osd.c \
$(USER_DIR)/common/typeconversion.c \
$(USER_DIR)/drivers/display.c \
$(USER_DIR)/common/maths.c \
$(USER_DIR)/common/printf.c
osd_unittest_DEFINES := \
OSD
parameter_groups_unittest_SRC := \
$(USER_DIR)/config/parameter_group.c
@ -359,7 +370,7 @@ $(OBJECT_DIR)/$1/%.c.o: $(USER_DIR)/%.c
@echo "compiling $$<" "$(STDOUT)"
$(V1) mkdir -p $$(dir $$@)
$(V1) $(CC) $(C_FLAGS) $(TEST_CFLAGS) \
$(foreach def,$1_DEFINES,-D $(def)) \
$(foreach def,$($1_DEFINES),-D $(def)) \
-c $$< -o $$@
@ -367,7 +378,7 @@ $(OBJECT_DIR)/$1/$1.o: $(TEST_DIR)/$1.cc
@echo "compiling $$<" "$(STDOUT)"
$(V1) mkdir -p $$(dir $$@)
$(V1) $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) \
$(foreach def,$1_DEFINES,-D $(def)) \
$(foreach def,$($1_DEFINES),-D $(def)) \
-c $$< -o $$@

96
src/test/unit/cms_unittest.cc
View File

@ -28,7 +28,6 @@
extern "C" {
#include "platform.h"
#include "target.h"
#include "drivers/display.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
void cmsMenuOpen(void);
@ -38,102 +37,9 @@ extern "C" {
}
#include "unittest_macros.h"
#include "unittest_displayport.h"
#include "gtest/gtest.h"
static displayPort_t testDisplayPort;
static int displayPortTestGrab(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int displayPortTestRelease(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int displayPortTestClearScreen(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int displayPortTestDrawScreen(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int displayPortTestScreenSize(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int displayPortTestWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
{
UNUSED(displayPort);
UNUSED(x);
UNUSED(y);
UNUSED(s);
return 0;
}
static int displayPortTestWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c)
{
UNUSED(displayPort);
UNUSED(x);
UNUSED(y);
UNUSED(c);
return 0;
}
static bool displayPortTestIsTransferInProgress(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int displayPortTestHeartbeat(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static void displayPortTestResync(displayPort_t *displayPort)
{
UNUSED(displayPort);
}
static uint32_t displayPortTestTxBytesFree(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static const displayPortVTable_t testDisplayPortVTable = {
.grab = displayPortTestGrab,
.release = displayPortTestRelease,
.clearScreen = displayPortTestClearScreen,
.drawScreen = displayPortTestDrawScreen,
.screenSize = displayPortTestScreenSize,
.write = displayPortTestWrite,
.writeChar = displayPortTestWriteChar,
.isTransferInProgress = displayPortTestIsTransferInProgress,
.heartbeat = displayPortTestHeartbeat,
.resync = displayPortTestResync,
.txBytesFree = displayPortTestTxBytesFree
};
displayPort_t *displayPortTestInit(void)
{
displayInit(&testDisplayPort, &testDisplayPortVTable);
testDisplayPort.rows = 10;
testDisplayPort.cols = 40;
return &testDisplayPort;
}
TEST(CMSUnittest, TestCmsDisplayPortRegister)
{
cmsInit();

24
src/test/unit/flight_imu_unittest.cc
View File

@ -35,22 +35,24 @@ extern "C" {
#include "drivers/compass/compass.h"
#include "drivers/sensor.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "fc/runtime_config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "rx/rx.h"
#include "fc/runtime_config.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "io/gps.h"
#include "rx/rx.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
void imuComputeRotationMatrix(void);
void imuUpdateEulerAngles(void);
@ -206,9 +208,7 @@ gyro_t gyro;
acc_t acc;
mag_t mag;
uint8_t GPS_numSat;
uint16_t GPS_speed;
uint16_t GPS_ground_course;
gpsSolutionData_t gpsSol;
uint8_t debugMode;
int16_t debug[DEBUG16_VALUE_COUNT];

19
src/test/unit/ledstrip_unittest.cc
View File

@ -24,27 +24,28 @@
extern "C" {
#include "build/build_config.h"
#include "common/color.h"
#include "common/axis.h"
#include "common/color.h"
#include "config/parameter_group_ids.h"
#include "sensors/battery.h"
#include "drivers/io.h"
#include "rx/rx.h"
#include "drivers/light_ws2811strip.h"
#include "drivers/timer.h"
#include "drivers/timer_def.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "drivers/light_ws2811strip.h"
#include "drivers/timer.h"
#include "drivers/timer_def.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "rx/rx.h"
#include "sensors/battery.h"
#include "target.h"
}
@ -298,7 +299,7 @@ float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint32_t rcModeActivationMask;
uint16_t rssi = 0;
uint8_t GPS_numSat = 0;
gpsSolutionData_t gpsSol;
batteryState_e getBatteryState(void) {
return BATTERY_OK;

577
src/test/unit/osd_unittest.cc Normal file
View File

@ -0,0 +1,577 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
extern "C" {
#include "platform.h"
#include "build/debug.h"
#include "blackbox/blackbox.h"
#include "config/parameter_group_ids.h"
#include "drivers/max7456_symbols.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "io/gps.h"
#include "io/osd.h"
#include "sensors/battery.h"
#include "rx/rx.h"
void osdRefresh(timeUs_t currentTimeUs);
uint8_t stateFlags;
uint8_t armingFlags;
uint16_t flightModeFlags;
uint16_t rssi;
attitudeEulerAngles_t attitude;
pidProfile_t *currentPidProfile;
int16_t debug[DEBUG16_VALUE_COUNT];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome;
uint16_t GPS_directionToHome;
int32_t GPS_coord[2];
gpsSolutionData_t gpsSol;
PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
timeUs_t simulationTime = 0;
batteryState_e simulationBatteryState;
uint8_t simulationBatteryCellCount;
uint16_t simulationBatteryVoltage;
int32_t simulationAltitude;
int32_t simulationVerticalSpeed;
}
/* #define DEBUG_OSD */
#include "unittest_macros.h"
#include "unittest_displayport.h"
#include "gtest/gtest.h"
void setDefualtSimulationState()
{
rssi = 1024;
simulationBatteryState = BATTERY_OK;
simulationBatteryCellCount = 4;
simulationBatteryVoltage = 168;
simulationAltitude = 0;
simulationVerticalSpeed = 0;
}
/*
* Performs a test of the OSD actions on arming.
* (reused throughout the test suite)
*/
void doTestArm(bool testEmpty = true)
{
// given
// craft has been armed
ENABLE_ARMING_FLAG(ARMED);
// when
// sufficient OSD updates have been called
osdRefresh(simulationTime);
// then
// arming alert displayed
displayPortTestBufferSubstring(12, 7, "ARMED");
// given
// armed alert times out (0.5 seconds)
simulationTime += 0.5e6;
// when
// sufficient OSD updates have been called
osdRefresh(simulationTime);
// then
// arming alert disappears
#ifdef DEBUG_OSD
displayPortTestPrint();
#endif
if (testEmpty) {
displayPortTestBufferIsEmpty();
}
}
/*
* Performs a test of the OSD actions on disarming.
* (reused throughout the test suite)
*/
void doTestDisarm()
{
// given
// craft is disarmed after having been armed
DISABLE_ARMING_FLAG(ARMED);
// when
// sufficient OSD updates have been called
osdRefresh(simulationTime);
// then
// post flight statistics displayed
displayPortTestBufferSubstring(2, 2, " --- STATS ---");
}
/*
* Tests initialisation of the OSD and the power on splash screen.
*/
TEST(OsdTest, TestInit)
{
// given
// display port is initialised
displayPortTestInit();
// and
// default state values are set
setDefualtSimulationState();
// and
// this battery configuration (used for battery voltage elements)
batteryConfigMutable()->vbatmincellvoltage = 33;
batteryConfigMutable()->vbatmaxcellvoltage = 43;
// when
// OSD is initialised
osdInit(&testDisplayPort);
// then
// display buffer should contain splash screen
displayPortTestBufferSubstring(7, 8, "MENU: THR MID");
displayPortTestBufferSubstring(11, 9, "+ YAW LEFT");
displayPortTestBufferSubstring(11, 10, "+ PITCH UP");
// when
// splash screen timeout has elapsed
simulationTime += 4e6;
osdUpdate(simulationTime);
// then
// display buffer should be empty
#ifdef DEBUG_OSD
displayPortTestPrint();
#endif
displayPortTestBufferIsEmpty();
}
/*
* Tests visibility of the ARMED notification after arming.
*/
TEST(OsdTest, TestArm)
{
doTestArm();
}
/*
* Tests display and timeout of the post flight statistics screen after disarming.
*/
TEST(OsdTest, TestDisarm)
{
doTestDisarm();
// given
// post flight stats times out (60 seconds)
simulationTime += 60e6;
// when
// sufficient OSD updates have been called
osdRefresh(simulationTime);
// then
// post flight stats screen disappears
#ifdef DEBUG_OSD
displayPortTestPrint();
#endif
displayPortTestBufferIsEmpty();
}
/*
* Tests disarming and immediately rearming clears post flight stats and shows ARMED notification.
*/
TEST(OsdTest, TestDisarmWithImmediateRearm)
{
doTestArm();
doTestDisarm();
doTestArm();
}
/*
* Tests dismissing the statistics screen with pitch stick after disarming.
*/
TEST(OsdTest, TestDisarmWithDismissStats)
{
// Craft is alread armed after previous test
doTestDisarm();
// given
// sticks have been moved
rcData[PITCH] = 1800;
// when
// sufficient OSD updates have been called
osdRefresh(simulationTime);
osdRefresh(simulationTime);
// then
// post flight stats screen disappears
#ifdef DEBUG_OSD
displayPortTestPrint();
#endif
displayPortTestBufferIsEmpty();
rcData[PITCH] = 1500;
}
/*
* Tests the calculation of statistics with imperial unit output.
*/
TEST(OsdTest, TestStatsImperial)
{
// given
// this set of enabled post flight statistics
osdConfigMutable()->enabled_stats[OSD_STAT_MAX_SPEED] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_MIN_BATTERY] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_MIN_RSSI] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_MAX_CURRENT] = false;
osdConfigMutable()->enabled_stats[OSD_STAT_USED_MAH] = false;
osdConfigMutable()->enabled_stats[OSD_STAT_MAX_ALTITUDE] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_BLACKBOX] = false;
osdConfigMutable()->enabled_stats[OSD_STAT_END_BATTERY] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_FLYTIME] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_ARMEDTIME] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_MAX_DISTANCE] = true;
osdConfigMutable()->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = false;
// and
// using imperial unit system
osdConfigMutable()->units = OSD_UNIT_IMPERIAL;
// and
// a GPS fix is present
stateFlags |= GPS_FIX | GPS_FIX_HOME;
// when
// the craft is armed
doTestArm();
// and
// these conditions occur during flight
rssi = 1024;
gpsSol.groundSpeed = 500;
GPS_distanceToHome = 20;
simulationBatteryVoltage = 158;
simulationAltitude = 100;
simulationTime += 1e6;
osdRefresh(simulationTime);
rssi = 512;
gpsSol.groundSpeed = 800;
GPS_distanceToHome = 50;
simulationBatteryVoltage = 147;
simulationAltitude = 150;
simulationTime += 1e6;
osdRefresh(simulationTime);
rssi = 256;
gpsSol.groundSpeed = 200;
GPS_distanceToHome = 100;
simulationBatteryVoltage = 152;
simulationAltitude = 200;
simulationTime += 1e6;
osdRefresh(simulationTime);
// and
// the craft is disarmed
doTestDisarm();
// then
// statistics screen should display the following
displayPortTestBufferSubstring(2, 3, "ARMED TIME : 00:04");
displayPortTestBufferSubstring(2, 4, "FLY TIME : 00:07");
displayPortTestBufferSubstring(2, 5, "MAX SPEED : 28");
displayPortTestBufferSubstring(2, 6, "MAX DISTANCE : 328%c", SYM_FT);
displayPortTestBufferSubstring(2, 7, "MIN BATTERY : 14.7%c", SYM_VOLT);
displayPortTestBufferSubstring(2, 8, "END BATTERY : 15.2%c", SYM_VOLT);
displayPortTestBufferSubstring(2, 9, "MIN RSSI : 25%%");
displayPortTestBufferSubstring(2, 10, "MAX ALTITUDE : 6.5%c", SYM_FT);
}
/*
* Tests the calculation of statistics with metric unit output.
* (essentially an abridged version of the previous test
*/
TEST(OsdTest, TestStatsMetric)
{
// given
// using metric unit system
osdConfigMutable()->units = OSD_UNIT_METRIC;
// and
// default state values are set
setDefualtSimulationState();
// when
// the craft is armed
doTestArm();
// and
// these conditions occur during flight (simplified to less assignments than previous test)
rssi = 256;
gpsSol.groundSpeed = 800;
GPS_distanceToHome = 100;
simulationBatteryVoltage = 147;
simulationAltitude = 200;
simulationTime += 1e6;
osdRefresh(simulationTime);
osdRefresh(simulationTime);
simulationBatteryVoltage = 152;
simulationTime += 1e6;
osdRefresh(simulationTime);
// and
// the craft is disarmed
doTestDisarm();
// then
// statistics screen should display the following
displayPortTestBufferSubstring(2, 3, "ARMED TIME : 00:02");
displayPortTestBufferSubstring(2, 4, "FLY TIME : 00:09");
displayPortTestBufferSubstring(2, 5, "MAX SPEED : 28");
displayPortTestBufferSubstring(2, 6, "MAX DISTANCE : 100%c", SYM_M);
displayPortTestBufferSubstring(2, 7, "MIN BATTERY : 14.7%c", SYM_VOLT);
displayPortTestBufferSubstring(2, 8, "END BATTERY : 15.2%c", SYM_VOLT);
displayPortTestBufferSubstring(2, 9, "MIN RSSI : 25%%");
displayPortTestBufferSubstring(2, 10, "MAX ALTITUDE : 2.0%c", SYM_M);
}
/*
* Tests activation of alarms and element flashing.
*/
TEST(OsdTest, TestAlarms)
{
// given
// default state is set
setDefualtSimulationState();
// and
// the following OSD elements are visible
osdConfigMutable()->item_pos[OSD_RSSI_VALUE] = OSD_POS(8, 1) | VISIBLE_FLAG;
osdConfigMutable()->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1) | VISIBLE_FLAG;
osdConfigMutable()->item_pos[OSD_FLYTIME] = OSD_POS(1, 1) | VISIBLE_FLAG;
osdConfigMutable()->item_pos[OSD_ALTITUDE] = OSD_POS(23, 7) | VISIBLE_FLAG;
// and
// this set of alarm values
osdConfigMutable()->rssi_alarm = 20;
osdConfigMutable()->cap_alarm = 2200;
osdConfigMutable()->time_alarm = 1; // in minutes
osdConfigMutable()->alt_alarm = 100; // meters
// and
// using the metric unit system
osdConfigMutable()->units = OSD_UNIT_METRIC;
// when
// the craft is armed
doTestArm(false);
// then
// no elements should flash as all values are out of alarm range
for (int i = 0; i < 30; i++) {
// Check for visibility every 100ms, elements should always be visible
simulationTime += 0.1e6;
osdRefresh(simulationTime);
#ifdef DEBUG_OSD
printf("%d\n", i);
#endif
displayPortTestBufferSubstring(8, 1, "%c99", SYM_RSSI);
displayPortTestBufferSubstring(12, 1, "%c16.8%c", SYM_BATT_FULL, SYM_VOLT);
displayPortTestBufferSubstring(1, 1, "%c00:", SYM_FLY_M); // only test the minute part of the timer
displayPortTestBufferSubstring(23, 7, " 0.0%c", SYM_M);
}
// when
// all values are out of range
rssi = 128;
simulationBatteryState = BATTERY_CRITICAL;
simulationBatteryVoltage = 135;
simulationAltitude = 12000;
// Fly timer is incremented on periodic calls to osdRefresh, can't simply just increment the simulated system clock
for (int i = 0; i < 60; i++) {
simulationTime += 1e6;
osdRefresh(simulationTime);
}
// then
// elements showing values in alarm range should flash
for (int i = 0; i < 15; i++) {
// Blinking should happen at 5Hz
simulationTime += 0.2e6;
osdRefresh(simulationTime);
#ifdef DEBUG_OSD
printf("%d\n", i);
displayPortTestPrint();
#endif
if (i % 2 == 0) {
displayPortTestBufferSubstring(8, 1, "%c12", SYM_RSSI);
displayPortTestBufferSubstring(12, 1, "%c13.5%c", SYM_MAIN_BATT, SYM_VOLT);
displayPortTestBufferSubstring(1, 1, "%c01:", SYM_FLY_M); // only test the minute part of the timer
displayPortTestBufferSubstring(23, 7, " 120.0%c", SYM_M);
} else {
displayPortTestBufferIsEmpty();
}
}
}
/*
* Tests the RSSI OSD element.
*/
TEST(OsdTest, TestElementRssi)
{
// given
osdConfigMutable()->item_pos[OSD_RSSI_VALUE] = OSD_POS(8, 1) | VISIBLE_FLAG;
osdConfigMutable()->rssi_alarm = 0;
// when
rssi = 1024;
displayClearScreen(&testDisplayPort);
osdRefresh(simulationTime);
// then
displayPortTestBufferSubstring(8, 1, "%c99", SYM_RSSI);
// when
rssi = 0;
displayClearScreen(&testDisplayPort);
osdRefresh(simulationTime);
// then
displayPortTestBufferSubstring(8, 1, "%c0", SYM_RSSI);
// when
rssi = 512;
displayClearScreen(&testDisplayPort);
osdRefresh(simulationTime);
// then
displayPortTestBufferSubstring(8, 1, "%c50", SYM_RSSI);
}
// STUBS
extern "C" {
bool sensors(uint32_t mask) {
UNUSED(mask);
return true;
}
bool IS_RC_MODE_ACTIVE(boxId_e boxId) {
UNUSED(boxId);
return false;
}
uint32_t micros() {
return simulationTime;
}
bool isBeeperOn() {
return false;
}
bool isAirmodeActive() {
return false;
}
uint8_t getCurrentPidProfileIndex() {
return 0;
}
uint8_t getCurrentControlRateProfileIndex() {
return 0;
}
batteryState_e getBatteryState() {
return simulationBatteryState;
}
uint8_t getBatteryCellCount() {
return simulationBatteryCellCount;
}
uint16_t getBatteryVoltage() {
return simulationBatteryVoltage;
}
int32_t getAmperage() {
return 0;
}
int32_t getMAhDrawn() {
return 0;
}
int32_t getEstimatedAltitude() {
return simulationAltitude;
}
int32_t getEstimatedVario() {
return simulationVerticalSpeed;
}
unsigned int blackboxGetLogNumber() {
return 0;
}
bool isSerialTransmitBufferEmpty(const serialPort_t *instance) {
UNUSED(instance);
return false;
}
void serialWrite(serialPort_t *instance, uint8_t ch) {
UNUSED(instance);
UNUSED(ch);
}
bool cmsDisplayPortRegister(displayPort_t *pDisplay) {
UNUSED(pDisplay);
return false;
}
}

24
src/test/unit/telemetry_crsf_unittest.cc
View File

@ -82,12 +82,7 @@ uint16_t Groundspeed ( km/h / 10 )
uint16_t GPS heading ( degree / 100 )
uint16 Altitude ( meter ­ 1000m offset )
uint8_t Satellites in use ( counter )
uint8_t GPS_numSat;
int32_t GPS_coord[2];
uint16_t GPS_distanceToHome; // distance to home point in meters
uint16_t GPS_altitude; // altitude in m
uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10
*/
#define FRAME_HEADER_FOOTER_LEN 4
@ -114,13 +109,13 @@ TEST(TelemetryCrsfTest, TestGPS)
EXPECT_EQ(0, satelliteCount);
EXPECT_EQ(crfsCrc(frame, frameLen), frame[18]);
GPS_coord[LAT] = 56 * GPS_DEGREES_DIVIDER;
GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER;
gpsSol.llh.lat = 56 * GPS_DEGREES_DIVIDER;
gpsSol.llh.lon = 163 * GPS_DEGREES_DIVIDER;
ENABLE_STATE(GPS_FIX);
GPS_altitude = 2345; // altitude in m
GPS_speed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587
GPS_numSat = 9;
GPS_ground_course = 1479; // degrees * 10
gpsSol.llh.alt = 2345; // altitude in m
gpsSol.groundSpeed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587
gpsSol.numSat = 9;
gpsSol.groundCourse = 1479; // degrees * 10
frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS);
lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6];
EXPECT_EQ(560000000, lattitude);
@ -279,12 +274,8 @@ uint8_t useHottAlarmSoundPeriod (void) { return 0; }
attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
uint8_t GPS_numSat;
int32_t GPS_coord[2];
uint16_t GPS_distanceToHome; // distance to home point in meters
uint16_t GPS_altitude; // altitude in m
uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10
gpsSolutionData_t gpsSol;
void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
@ -327,4 +318,3 @@ uint8_t calculateBatteryPercentageRemaining(void) {
}
}

5
src/test/unit/telemetry_hott_unittest.cc
View File

@ -171,11 +171,8 @@ uint16_t batteryWarningVoltage;
uint8_t useHottAlarmSoundPeriod (void) { return 0; }
uint8_t GPS_numSat;
int32_t GPS_coord[2];
uint16_t GPS_speed; // speed in 0.1m/s
gpsSolutionData_t gpsSol;
uint16_t GPS_distanceToHome; // distance to home point in meters
uint16_t GPS_altitude; // altitude in 0.1m
int16_t GPS_directionToHome; // direction to home or hol point in degrees

165
src/test/unit/unittest_displayport.h Normal file
View File

@ -0,0 +1,165 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <string.h>
extern "C" {
#include "drivers/display.h"
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
void displayPortTestBufferSubstring(int x, int y, const char * expectedFormat, ...) __attribute__ ((format (printf, 3, 4)));
#define UNITTEST_DISPLAYPORT_ROWS 16
#define UNITTEST_DISPLAYPORT_COLS 30
#define UNITTEST_DISPLAYPORT_BUFFER_LEN (UNITTEST_DISPLAYPORT_ROWS * UNITTEST_DISPLAYPORT_COLS)
char testDisplayPortBuffer[UNITTEST_DISPLAYPORT_BUFFER_LEN];
static displayPort_t testDisplayPort;
static int displayPortTestGrab(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int displayPortTestRelease(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int displayPortTestClearScreen(displayPort_t *displayPort)
{
UNUSED(displayPort);
memset(testDisplayPortBuffer, ' ', UNITTEST_DISPLAYPORT_BUFFER_LEN);
return 0;
}
static int displayPortTestDrawScreen(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int displayPortTestScreenSize(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int displayPortTestWriteString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
{
UNUSED(displayPort);
for (unsigned int i = 0; i < strlen(s); i++) {
testDisplayPortBuffer[(y * UNITTEST_DISPLAYPORT_COLS) + x + i] = s[i];
}
return 0;
}
static int displayPortTestWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c)
{
UNUSED(displayPort);
testDisplayPortBuffer[(y * UNITTEST_DISPLAYPORT_COLS) + x] = c;
return 0;
}
static bool displayPortTestIsTransferInProgress(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int displayPortTestHeartbeat(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static void displayPortTestResync(displayPort_t *displayPort)
{
UNUSED(displayPort);
}
static uint32_t displayPortTestTxBytesFree(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static const displayPortVTable_t testDisplayPortVTable = {
.grab = displayPortTestGrab,
.release = displayPortTestRelease,
.clearScreen = displayPortTestClearScreen,
.drawScreen = displayPortTestDrawScreen,
.screenSize = displayPortTestScreenSize,
.writeString = displayPortTestWriteString,
.writeChar = displayPortTestWriteChar,
.isTransferInProgress = displayPortTestIsTransferInProgress,
.heartbeat = displayPortTestHeartbeat,
.resync = displayPortTestResync,
.txBytesFree = displayPortTestTxBytesFree
};
displayPort_t *displayPortTestInit(void)
{
displayInit(&testDisplayPort, &testDisplayPortVTable);
testDisplayPort.rows = UNITTEST_DISPLAYPORT_ROWS;
testDisplayPort.cols = UNITTEST_DISPLAYPORT_COLS;
return &testDisplayPort;
}
void displayPortTestPrint(void)
{
for (int i = 0; i < UNITTEST_DISPLAYPORT_BUFFER_LEN; i++) {
if (i > 0 && i % UNITTEST_DISPLAYPORT_COLS == 0) {
printf("\n");
}
printf("%c", testDisplayPortBuffer[i]);
}
printf("\n\n");
}
void displayPortTestBufferIsEmpty()
{
for (size_t i = 0; i < UNITTEST_DISPLAYPORT_BUFFER_LEN; i++) {
EXPECT_EQ(' ', testDisplayPortBuffer[i]);
}
}
void displayPortTestBufferSubstring(int x, int y, const char * expectedFormat, ...)
{
char expected[UNITTEST_DISPLAYPORT_BUFFER_LEN];
va_list args;
va_start(args, expectedFormat);
vsnprintf(expected, UNITTEST_DISPLAYPORT_BUFFER_LEN, expectedFormat, args);
va_end(args);
#ifdef DEBUG_OSD
displayPortTestPrint();
#endif
for (size_t i = 0; i < strlen(expected); i++) {
EXPECT_EQ(expected[i], testDisplayPortBuffer[(y * testDisplayPort.cols) + x + i]);
}
}