diff --git a/.travis.yml b/.travis.yml index eec253c3b..57aae71af 100644 --- a/.travis.yml +++ b/.travis.yml @@ -41,7 +41,7 @@ env: # - TARGET=REVO_OPBL # - TARGET=RMDO # - TARGET=SINGULARITY -# - TARGET=SIRINFPV + - TARGET=SIRINFPV - TARGET=SPARKY # - TARGET=SPARKY2 # - TARGET=SPARKY_OPBL @@ -53,7 +53,7 @@ env: # - TARGET=X_RACERSPI # - TARGET=ZCOREF3 # - TARGET=RCEXPLORERF3 - + # use new docker environment sudo: false diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index d902906c4..6996068a9 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -136,7 +136,7 @@ typedef struct master_s { #endif #ifdef OSD - osd_profile osdProfile; + osd_profile_t osdProfile; #endif profile_t profile[MAX_PROFILE_COUNT]; diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 647d0dc83..fa04b2619 100755 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -17,6 +17,9 @@ #include #include +#include +#include +#include #include "platform.h" @@ -24,35 +27,50 @@ #include "common/printf.h" -#include "bus_spi.h" -#include "light_led.h" -#include "io.h" -#include "system.h" -#include "nvic.h" -#include "dma.h" +#include "drivers/bus_spi.h" +#include "drivers/light_led.h" +#include "drivers/io.h" +#include "drivers/system.h" +#include "drivers/nvic.h" +#include "drivers/dma.h" #include "max7456.h" #include "max7456_symbols.h" -#define DISABLE_MAX7456 IOHi(max7456CsPin) -#define ENABLE_MAX7456 IOLo(max7456CsPin) - -uint16_t max_screen_size; -static MAX7456_CHAR_TYPE max7456_screen[VIDEO_BUFFER_CHARS_PAL + 5]; -#define SCREEN_BUFFER ((MAX7456_CHAR_TYPE*)&max7456_screen[3]) - -#ifdef MAX7456_DMA_CHANNEL_TX -volatile uint8_t dma_transaction_in_progress = 0; +// On shared SPI bus we want to change clock for OSD chip and restore for other devices +#ifdef MAX7456_SPI_CLK + #define ENABLE_MAX7456 {spiSetDivisor(MAX7456_SPI_INSTANCE, MAX7456_SPI_CLK);IOLo(max7456CsPin);} +#else + #define ENABLE_MAX7456 IOLo(max7456CsPin) #endif -static uint8_t video_signal_type = 0; -static uint8_t max7456_lock = 0; -static IO_t max7456CsPin = IO_NONE; +#ifdef MAX7456_RESTORE_CLK + #define DISABLE_MAX7456 {IOHi(max7456CsPin);spiSetDivisor(MAX7456_SPI_INSTANCE, MAX7456_RESTORE_CLK);} +#else + #define DISABLE_MAX7456 IOHi(max7456CsPin) +#endif +uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL; +// we write everything in SCREEN_BUFFER and then comapre +// SCREEN_BUFFER with SHADOW_BUFFER to upgrade only changed chars +// this solution is faster then redraw all screen -MAX7456_CHAR_TYPE* max7456_get_screen_buffer(void) { - return SCREEN_BUFFER; -} +static uint8_t SCREEN_BUFFER[VIDEO_BUFFER_CHARS_PAL + 40]; //for faster writes we use memcpy so we need some space to don't overwrite buffer +static uint8_t SHADOW_BUFFER[VIDEO_BUFFER_CHARS_PAL]; + +//max chars to update in one idle +#define MAX_CHARS2UPDATE 100 +#ifdef MAX7456_DMA_CHANNEL_TX +volatile bool dmaTxInProgress = false; +#endif // MAX7456_DMA_CHANNEL_TX + +static uint8_t spiBuffer[MAX_CHARS2UPDATE * 6]; + +static uint8_t videoSignalCfg = 0; +static uint8_t videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; //PAL by default +static bool max7456Lock = false; +static IO_t max7456CsPin = IO_NONE; +static bool fontIsLoading = false; static uint8_t max7456_send(uint8_t add, uint8_t data) { @@ -61,19 +79,20 @@ static uint8_t max7456_send(uint8_t add, uint8_t data) } #ifdef MAX7456_DMA_CHANNEL_TX -static void max7456_send_dma(void* tx_buffer, void* rx_buffer, uint16_t buffer_size) { +static void max7456_send_dma(void* tx_buffer, void* rx_buffer, uint16_t buffer_size) +{ DMA_InitTypeDef DMA_InitStructure; #ifdef MAX7456_DMA_CHANNEL_RX static uint16_t dummy[] = {0xffff}; -#else +#else // MAX7456_DMA_CHANNEL_RX UNUSED(rx_buffer); -#endif - while (dma_transaction_in_progress); // Wait for prev DMA transaction +#endif // MAX7456_DMA_CHANNEL_RX + while (dmaTxInProgress); // Wait for prev DMA transaction DMA_DeInit(MAX7456_DMA_CHANNEL_TX); #ifdef MAX7456_DMA_CHANNEL_RX DMA_DeInit(MAX7456_DMA_CHANNEL_RX); -#endif +#endif // MAX7456_DMA_CHANNEL_RX // Common to both channels DMA_StructInit(&DMA_InitStructure); @@ -90,24 +109,24 @@ static void max7456_send_dma(void* tx_buffer, void* rx_buffer, uint16_t buffer_s #ifdef STM32F4 DMA_InitStructure.DMA_Memory0BaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy); DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; -#else +#else // STM32F4 DMA_InitStructure.DMA_MemoryBaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy); DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; -#endif +#endif // STM32F4 DMA_InitStructure.DMA_MemoryInc = rx_buffer ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable; DMA_Init(MAX7456_DMA_CHANNEL_RX, &DMA_InitStructure); DMA_Cmd(MAX7456_DMA_CHANNEL_RX, ENABLE); -#endif +#endif // MAX7456_DMA_CHANNEL_RX // Tx channel #ifdef STM32F4 DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)tx_buffer; //max7456_screen; DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; -#else +#else // STM32F4 DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)tx_buffer; //max7456_screen; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; -#endif +#endif // STM32F4 DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_Init(MAX7456_DMA_CHANNEL_TX, &DMA_InitStructure); @@ -115,31 +134,37 @@ static void max7456_send_dma(void* tx_buffer, void* rx_buffer, uint16_t buffer_s #ifdef MAX7456_DMA_CHANNEL_RX DMA_ITConfig(MAX7456_DMA_CHANNEL_RX, DMA_IT_TC, ENABLE); -#else +#else // MAX7456_DMA_CHANNEL_RX DMA_ITConfig(MAX7456_DMA_CHANNEL_TX, DMA_IT_TC, ENABLE); -#endif +#endif // MAX7456_DMA_CHANNEL_RX // Enable SPI TX/RX request ENABLE_MAX7456; - dma_transaction_in_progress = 1; + dmaTxInProgress = true; SPI_I2S_DMACmd(MAX7456_SPI_INSTANCE, #ifdef MAX7456_DMA_CHANNEL_RX SPI_I2S_DMAReq_Rx | -#endif +#endif // MAX7456_DMA_CHANNEL_RX SPI_I2S_DMAReq_Tx, ENABLE); } -void max7456_dma_irq_handler(dmaChannelDescriptor_t* descriptor) { +void max7456_dma_irq_handler(dmaChannelDescriptor_t* descriptor) +{ if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { #ifdef MAX7456_DMA_CHANNEL_RX DMA_Cmd(MAX7456_DMA_CHANNEL_RX, DISABLE); -#else +#endif // MAX7456_DMA_CHANNEL_RX + // make sure spi dmd transfer is complete + while (SPI_I2S_GetFlagStatus (MAX7456_SPI_INSTANCE, SPI_I2S_FLAG_TXE) == RESET) {}; + while (SPI_I2S_GetFlagStatus (MAX7456_SPI_INSTANCE, SPI_I2S_FLAG_BSY) == SET) {}; + //Empty RX buffer. RX DMA takes care of it if enabled + //this should be done after transmission finish!!! while (SPI_I2S_GetFlagStatus(MAX7456_SPI_INSTANCE, SPI_I2S_FLAG_RXNE) == SET) { MAX7456_SPI_INSTANCE->DR; } -#endif + DMA_Cmd(MAX7456_DMA_CHANNEL_TX, DISABLE); DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); @@ -147,139 +172,231 @@ void max7456_dma_irq_handler(dmaChannelDescriptor_t* descriptor) { SPI_I2S_DMACmd(MAX7456_SPI_INSTANCE, #ifdef MAX7456_DMA_CHANNEL_RX SPI_I2S_DMAReq_Rx | -#endif +#endif // MAX7456_DMA_CHANNEL_RX SPI_I2S_DMAReq_Tx, DISABLE); DISABLE_MAX7456; - for (uint16_t x = 0; x < max_screen_size; x++) - max7456_screen[x + 3] = MAX7456_CHAR(' '); - dma_transaction_in_progress = 0; + dmaTxInProgress = false; + } + + if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_HTIF)) { + DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF); + } + if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF)) { + DMA_CLEAR_FLAG(descriptor, DMA_IT_TEIF); } } -#endif +#endif // MAX7456_DMA_CHANNEL_TX -void max7456_init(uint8_t video_system) +uint8_t max7456_get_rows_count(void) { - uint8_t max_screen_rows; + if (videoSignalReg & VIDEO_MODE_PAL) + return VIDEO_LINES_PAL; + + return VIDEO_LINES_NTSC; +} + +//because MAX7456 need some time to detect video system etc. we need to wait for a while to initialize it at startup +//and in case of restart we need to reinitialize chip +void max7456_init2(void) +{ + uint8_t maxScreenRows; uint8_t srdata = 0; uint16_t x; + static bool firstInit = true; -#ifdef MAX7456_SPI_CS_PIN - max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN)); -#endif - IOInit(max7456CsPin, OWNER_OSD, RESOURCE_SPI_CS, 0); - IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG); + //do not init MAX before camera power up correctly + if (millis() < 1500) + return; - //Minimum spi clock period for max7456 is 100ns (10Mhz) - spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD); - - delay(1000); - // force soft reset on Max7456 ENABLE_MAX7456; - max7456_send(VM0_REG, MAX7456_RESET); - delay(100); - srdata = max7456_send(0xA0, 0xFF); - if ((0x01 & srdata) == 0x01) { //PAL - video_signal_type = VIDEO_MODE_PAL; - } - else if ((0x02 & srdata) == 0x02) { //NTSC - video_signal_type = VIDEO_MODE_NTSC; - } - - // Override detected type: 0-AUTO, 1-PAL, 2-NTSC - switch(video_system) { + switch(videoSignalCfg) { case PAL: - video_signal_type = VIDEO_MODE_PAL; + videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; break; case NTSC: - video_signal_type = VIDEO_MODE_NTSC; + videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE; break; + default: + srdata = max7456_send(MAX7456ADD_STAT, 0x00); + if ((0x02 & srdata) == 0x02) + videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE; } - if (video_signal_type) { //PAL - max_screen_size = VIDEO_BUFFER_CHARS_PAL; - max_screen_rows = VIDEO_LINES_PAL; - } else { // NTSC - max_screen_size = VIDEO_BUFFER_CHARS_NTSC; - max_screen_rows = VIDEO_LINES_NTSC; + if (videoSignalReg & VIDEO_MODE_PAL) { //PAL + maxScreenSize = VIDEO_BUFFER_CHARS_PAL; + maxScreenRows = VIDEO_LINES_PAL; + } else { // NTSC + maxScreenSize = VIDEO_BUFFER_CHARS_NTSC; + maxScreenRows = VIDEO_LINES_NTSC; } // set all rows to same charactor black/white level - for(x = 0; x < max_screen_rows; x++) { + for(x = 0; x < maxScreenRows; x++) { max7456_send(MAX7456ADD_RB0 + x, BWBRIGHTNESS); } // make sure the Max7456 is enabled - max7456_send(VM0_REG, OSD_ENABLE | video_signal_type); - + max7456_send(VM0_REG, videoSignalReg); + max7456_send(DMM_REG, CLEAR_DISPLAY); DISABLE_MAX7456; - delay(100); - for (x = 0; x < max_screen_size; x++) - SCREEN_BUFFER[x] = MAX7456_CHAR(' '); - -#ifdef MAX7456_DMA_CHANNEL_TX - max7456_screen[0] = (uint16_t)(MAX7456ADD_DMAH | (0 << 8)); - max7456_screen[1] = (uint16_t)(MAX7456ADD_DMAL | (0 << 8)); - max7456_screen[2] = (uint16_t)(MAX7456ADD_DMM | (1 << 8)); - max7456_screen[max_screen_size + 3] = (uint16_t)(MAX7456ADD_DMDI | (0xFF << 8)); - max7456_screen[max_screen_size + 4] = (uint16_t)(MAX7456ADD_DMM | (0 << 8)); - - dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0); -#endif -} - -// Copy string from ram into screen buffer -void max7456_write_string(const char *string, int16_t address) { - MAX7456_CHAR_TYPE *dest; - - if (address >= 0) - dest = SCREEN_BUFFER + address; - else - dest = SCREEN_BUFFER + (max_screen_size + address); - - while(*string && dest < (SCREEN_BUFFER + max_screen_size)) { - *dest++ = MAX7456_CHAR(*string++); + //clear shadow to force redraw all screen in non-dma mode + memset(SHADOW_BUFFER, 0, maxScreenSize); + if (firstInit) { + max7456_refresh_all(); + firstInit = false; } } -void max7456_draw_screen(void) { - if (!max7456_lock) { -#ifdef MAX7456_DMA_CHANNEL_TX - max7456_send_dma(max7456_screen, NULL, max_screen_size * 2 + 10); -#else - uint16_t xx; - max7456_lock = 1; +//here we init only CS and try to init MAX for first time +void max7456_init(uint8_t videoSystem) +{ +#ifdef MAX7456_SPI_CS_PIN + max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN)); +#endif // MAX7456_SPI_CS_PIN + IOInit(max7456CsPin, OWNER_OSD, RESOURCE_SPI_CS, 0); + IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG); + spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD); + // force soft reset on Max7456 + ENABLE_MAX7456; + max7456_send(VM0_REG, MAX7456_RESET); + DISABLE_MAX7456; + videoSignalCfg = videoSystem; + +#ifdef MAX7456_DMA_CHANNEL_TX + dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0); +#endif // MAX7456_DMA_CHANNEL_TX + //real init will be made letter when driver idle detect +} + +//just fill with spaces with some tricks +void max7456_clear_screen(void) +{ + uint16_t x; + uint32_t *p = (uint32_t*)&SCREEN_BUFFER[0]; + for (x = 0; x < VIDEO_BUFFER_CHARS_PAL/4; x++) + p[x] = 0x20202020; +} + +uint8_t* max7456_get_screen_buffer(void) +{ + return SCREEN_BUFFER; +} + +void max7456_write_char(uint8_t x, uint8_t y, uint8_t c) +{ + SCREEN_BUFFER[y*30+x] = c; +} + +void max7456_write(uint8_t x, uint8_t y, char *buff) +{ + uint8_t i = 0; + for (i = 0; *(buff+i); i++) + if (x+i < 30) //do not write over screen + SCREEN_BUFFER[y*30+x+i] = *(buff+i); +} + +#ifdef MAX7456_DMA_CHANNEL_TX +uint8_t max7456_dma_in_progres(void) +{ + return dmaTxInProgress; +} +#endif // MAX7456_DMA_CHANNEL_TX + +void max7456_draw_screen(void) +{ + uint8_t check; + static uint16_t pos = 0; + int k = 0, buff_len=0; + + if (!max7456Lock && !fontIsLoading) { + //-----------------detect MAX7456 fail, or initialize it at startup when it is ready-------- + max7456Lock = true; + ENABLE_MAX7456; + check = max7456_send(VM0_REG | 0x80, 0x00); + DISABLE_MAX7456; + + if ( check != videoSignalReg ) + max7456_init2(); + + //------------ end of (re)init------------------------------------- + + for (k=0; k< MAX_CHARS2UPDATE; k++) { + if (SCREEN_BUFFER[pos] != SHADOW_BUFFER[pos]) { + spiBuffer[buff_len++] = MAX7456ADD_DMAH; + spiBuffer[buff_len++] = pos >> 8; + spiBuffer[buff_len++] = MAX7456ADD_DMAL; + spiBuffer[buff_len++] = pos & 0xff; + spiBuffer[buff_len++] = MAX7456ADD_DMDI; + spiBuffer[buff_len++] = SCREEN_BUFFER[pos]; + SHADOW_BUFFER[pos] = SCREEN_BUFFER[pos]; + k++; + } + + if (++pos >= maxScreenSize) { + pos = 0; + break; + } + } + + if (buff_len) { +#ifdef MAX7456_DMA_CHANNEL_TX + if (buff_len > 0) + max7456_send_dma(spiBuffer, NULL, buff_len); +#else + ENABLE_MAX7456; + for (k=0; k < buff_len; k++) + spiTransferByte(MAX7456_SPI_INSTANCE, spiBuffer[k]); + DISABLE_MAX7456; +#endif // MAX7456_DMA_CHANNEL_TX + } + max7456Lock = 0; + } +} + +// this funcktion refresh all and should not be used when copter is armed +void max7456_refresh_all(void) +{ + if (!max7456Lock) { +#ifdef MAX7456_DMA_CHANNEL_TX + while (dmaTxInProgress); +#endif + uint16_t xx; + max7456Lock = true; ENABLE_MAX7456; max7456_send(MAX7456ADD_DMAH, 0); max7456_send(MAX7456ADD_DMAL, 0); max7456_send(MAX7456ADD_DMM, 1); - for (xx = 0; xx < max_screen_size; ++xx) { + + for (xx = 0; xx < maxScreenSize; ++xx) { max7456_send(MAX7456ADD_DMDI, SCREEN_BUFFER[xx]); - SCREEN_BUFFER[xx] = MAX7456_CHAR(' '); + SHADOW_BUFFER[xx] = SCREEN_BUFFER[xx]; } + max7456_send(MAX7456ADD_DMDI, 0xFF); max7456_send(MAX7456ADD_DMM, 0); DISABLE_MAX7456; - max7456_lock = 0; -#endif + max7456Lock = 0; } } -void max7456_write_nvm(uint8_t char_address, uint8_t *font_data) { +void max7456_write_nvm(uint8_t char_address, uint8_t *font_data) +{ uint8_t x; #ifdef MAX7456_DMA_CHANNEL_TX - while (dma_transaction_in_progress); + while (dmaTxInProgress); #endif - while (max7456_lock); - max7456_lock = 1; - ENABLE_MAX7456; + while (max7456Lock); + max7456Lock = true; + ENABLE_MAX7456; // disable display - max7456_send(VM0_REG, video_signal_type); + fontIsLoading = true; + max7456_send(VM0_REG, 0); max7456_send(MAX7456ADD_CMAH, char_address); // set start address high @@ -288,21 +405,20 @@ void max7456_write_nvm(uint8_t char_address, uint8_t *font_data) { max7456_send(MAX7456ADD_CMDI, font_data[x]); #ifdef LED0_TOGGLE LED0_TOGGLE; -#else +#else // LED0_TOGGLE LED1_TOGGLE; -#endif +#endif // LED0_TOGGLE } // transfer 54 bytes from shadow ram to NVM max7456_send(MAX7456ADD_CMM, WRITE_NVR); // wait until bit 5 in the status register returns to 0 (12ms) - while ((max7456_send(MAX7456ADD_STAT, 0) & STATUS_REG_NVR_BUSY) != 0x00); + while ((max7456_send(MAX7456ADD_STAT, 0x00) & STATUS_REG_NVR_BUSY) != 0x00); - max7456_send(VM0_REG, video_signal_type | 0x0C); DISABLE_MAX7456; - max7456_lock = 0; + + max7456Lock = 0; } - -#endif +#endif // USE_MAX7456 diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h old mode 100644 new mode 100755 index 5ba902ba5..81445e09f --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -20,6 +20,7 @@ #ifndef WHITEBRIGHTNESS #define WHITEBRIGHTNESS 0x01 #endif + #ifndef BLACKBRIGHTNESS #define BLACKBRIGHTNESS 0x00 #endif @@ -35,19 +36,18 @@ #define VM1_REG 0x01 // video mode register 0 bits -#define VIDEO_BUFFER_DISABLE 0x01 -#define MAX7456_RESET 0x02 -#define VERTICAL_SYNC_NEXT_VSYNC 0x04 -#define OSD_ENABLE 0x08 -#define SYNC_MODE_AUTO 0x00 -#define SYNC_MODE_INTERNAL 0x30 -#define SYNC_MODE_EXTERNAL 0x20 -#define VIDEO_MODE_PAL 0x40 -#define VIDEO_MODE_NTSC 0x00 +#define VIDEO_BUFFER_DISABLE 0x01 +#define MAX7456_RESET 0x02 +#define VERTICAL_SYNC_NEXT_VSYNC 0x04 +#define OSD_ENABLE 0x08 +#define SYNC_MODE_AUTO 0x00 +#define SYNC_MODE_INTERNAL 0x30 +#define SYNC_MODE_EXTERNAL 0x20 +#define VIDEO_MODE_PAL 0x40 +#define VIDEO_MODE_NTSC 0x00 // video mode register 1 bits - // duty cycle is on_off #define BLINK_DUTY_CYCLE_50_50 0x00 #define BLINK_DUTY_CYCLE_33_66 0x01 @@ -61,8 +61,8 @@ #define BLINK_TIME_3 0x0C // background mode brightness (percent) -#define BACKGROUND_BRIGHTNESS_0 0x00 -#define BACKGROUND_BRIGHTNESS_7 0x01 +#define BACKGROUND_BRIGHTNESS_0 0x00 +#define BACKGROUND_BRIGHTNESS_7 0x01 #define BACKGROUND_BRIGHTNESS_14 0x02 #define BACKGROUND_BRIGHTNESS_21 0x03 #define BACKGROUND_BRIGHTNESS_28 0x04 @@ -73,10 +73,9 @@ #define BACKGROUND_MODE_GRAY 0x40 //MAX7456 commands -#define CLEAR_DISPLAY 0x04 -#define CLEAR_DISPLAY_VERT 0x06 -#define END_STRING 0xff - +#define CLEAR_DISPLAY 0x04 +#define CLEAR_DISPLAY_VERT 0x06 +#define END_STRING 0xff #define MAX7456ADD_VM0 0x00 //0b0011100// 00 // 00 ,0011100 #define MAX7456ADD_VM1 0x01 @@ -142,19 +141,19 @@ enum VIDEO_TYPES { AUTO = 0, PAL, NTSC }; -extern uint16_t max_screen_size; +extern uint16_t maxScreenSize; + +void max7456_init(uint8_t system); +void max7456_draw_screen(void); +void max7456_write_string(const char *string, int16_t address); +void max7456_write_nvm(uint8_t char_address, uint8_t *font_data); +uint8_t max7456_get_rows_count(void); +void max7456_write(uint8_t x, uint8_t y, char *buff); +void max7456_write_char(uint8_t x, uint8_t y, uint8_t c); +void max7456_clear_screen(void); +void max7456_refresh_all(void); +uint8_t* max7456_get_screen_buffer(void); #ifdef MAX7456_DMA_CHANNEL_TX - #define MAX7456_CHAR_TYPE uint16_t - #define MAX7456_CHAR(X) (MAX7456ADD_DMDI | ((X) << 8)) -#else - #define MAX7456_CHAR_TYPE char - #define MAX7456_CHAR(X) (X) -#endif - -void max7456_init(uint8_t system); -void max7456_draw_screen(void); -void max7456_write_string(const char *string, int16_t address); -void max7456_write_nvm(uint8_t char_address, uint8_t *font_data); -MAX7456_CHAR_TYPE* max7456_get_screen_buffer(void); - +uint8_t max7456_dma_in_progres(void); +#endif // MAX7456_DMA_CHANNEL_TX diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index ba798d1d9..a5db8f22d 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -36,29 +36,29 @@ // Direction arrows #define SYM_ARROW_SOUTH 0x60 -#define SYM_ARROW_2 0x61 -#define SYM_ARROW_3 0x62 -#define SYM_ARROW_4 0x63 -#define SYM_ARROW_EAST 0x64 -#define SYM_ARROW_6 0x65 -#define SYM_ARROW_7 0x66 -#define SYM_ARROW_8 0x67 +#define SYM_ARROW_2 0x61 +#define SYM_ARROW_3 0x62 +#define SYM_ARROW_4 0x63 +#define SYM_ARROW_EAST 0x64 +#define SYM_ARROW_6 0x65 +#define SYM_ARROW_7 0x66 +#define SYM_ARROW_8 0x67 #define SYM_ARROW_NORTH 0x68 -#define SYM_ARROW_10 0x69 -#define SYM_ARROW_11 0x6A -#define SYM_ARROW_12 0x6B -#define SYM_ARROW_WEST 0x6C -#define SYM_ARROW_14 0x6D -#define SYM_ARROW_15 0x6E -#define SYM_ARROW_16 0x6F +#define SYM_ARROW_10 0x69 +#define SYM_ARROW_11 0x6A +#define SYM_ARROW_12 0x6B +#define SYM_ARROW_WEST 0x6C +#define SYM_ARROW_14 0x6D +#define SYM_ARROW_15 0x6E +#define SYM_ARROW_16 0x6F // Heading Graphics -#define SYM_HEADING_N 0x18 -#define SYM_HEADING_S 0x19 -#define SYM_HEADING_E 0x1A -#define SYM_HEADING_W 0x1B -#define SYM_HEADING_DIVIDED_LINE 0x1C -#define SYM_HEADING_LINE 0x1D +#define SYM_HEADING_N 0x18 +#define SYM_HEADING_S 0x19 +#define SYM_HEADING_E 0x1A +#define SYM_HEADING_W 0x1B +#define SYM_HEADING_DIVIDED_LINE 0x1C +#define SYM_HEADING_LINE 0x1D // FRSKY HUB #define SYM_CELL0 0xF0 @@ -93,63 +93,60 @@ #define SYM_ALT 0xCC // GPS Mode and Autopilot -#define SYM_3DFIX 0xDF -#define SYM_HOLD 0xEF -#define SYM_G_HOME 0xFF -#define SYM_GHOME 0x9D -#define SYM_GHOME1 0x9E -#define SYM_GHOLD 0xCD -#define SYM_GHOLD1 0xCE -#define SYM_GMISSION 0xB5 +#define SYM_3DFIX 0xDF +#define SYM_HOLD 0xEF +#define SYM_G_HOME 0xFF +#define SYM_GHOME 0x9D +#define SYM_GHOME1 0x9E +#define SYM_GHOLD 0xCD +#define SYM_GHOLD1 0xCE +#define SYM_GMISSION 0xB5 #define SYM_GMISSION1 0xB6 -#define SYM_GLAND 0xB7 -#define SYM_GLAND1 0xB8 +#define SYM_GLAND 0xB7 +#define SYM_GLAND1 0xB8 // Gimbal active Mode -#define SYM_GIMBAL 0x16 +#define SYM_GIMBAL 0x16 #define SYM_GIMBAL1 0x17 - // Sensor´s Presence -#define SYM_ACC 0xA0 -#define SYM_MAG 0xA1 -#define SYM_BAR 0xA2 -#define SYM_GPS 0xA3 -#define SYM_MAN 0xC0 -#define SYM_MAN1 0xC1 -#define SYM_MAN2 0xC2 -#define SYM_CHECK 0xBE -#define SYM_BARO10 0xB7 -#define SYM_BARO11 0xB8 -#define SYM_MAG10 0xB5 -#define SYM_MAG11 0xB6 +#define SYM_ACC 0xA0 +#define SYM_MAG 0xA1 +#define SYM_BAR 0xA2 +#define SYM_GPS 0xA3 +#define SYM_MAN 0xC0 +#define SYM_MAN1 0xC1 +#define SYM_MAN2 0xC2 +#define SYM_CHECK 0xBE +#define SYM_BARO10 0xB7 +#define SYM_BARO11 0xB8 +#define SYM_MAG10 0xB5 +#define SYM_MAG11 0xB6 // AH Center screen Graphics -#define SYM_AH_CENTER_LINE 0x26 -#define SYM_AH_CENTER_LINE_RIGHT 0x27 -#define SYM_AH_CENTER 0x7E -#define SYM_AH_RIGHT 0x02 -#define SYM_AH_LEFT 0x03 -#define SYM_AH_DECORATION_UP 0xC9 -#define SYM_AH_DECORATION_DOWN 0xCF - +#define SYM_AH_CENTER_LINE 0x26 +#define SYM_AH_CENTER_LINE_RIGHT 0x27 +#define SYM_AH_CENTER 0x7E +#define SYM_AH_RIGHT 0x02 +#define SYM_AH_LEFT 0x03 +#define SYM_AH_DECORATION_UP 0xC9 +#define SYM_AH_DECORATION_DOWN 0xCF // AH Bars #define SYM_AH_BAR9_0 0x80 - // Temperature #define SYM_TEMP_F 0x0D #define SYM_TEMP_C 0x0E // Batt evolution -#define SYM_BATT_FULL 0x90 -#define SYM_BATT_5 0x91 -#define SYM_BATT_4 0x92 -#define SYM_BATT_3 0x93 -#define SYM_BATT_2 0x94 -#define SYM_BATT_1 0x95 -#define SYM_BATT_EMPTY 0x96 +#define SYM_BATT_FULL 0x90 +#define SYM_BATT_5 0x91 +#define SYM_BATT_4 0x92 +#define SYM_BATT_3 0x93 +#define SYM_BATT_2 0x94 +#define SYM_BATT_1 0x95 +#define SYM_BATT_EMPTY 0x96 // Vario #define SYM_VARIO 0x7F @@ -159,41 +156,41 @@ // Batt Icon´s #define SYM_MAIN_BATT 0x97 -#define SYM_VID_BAT 0xBF +#define SYM_VID_BAT 0xBF // Unit Icon´s (Metric) -#define SYM_MS 0x9F -#define SYM_KMH 0xA5 -#define SYM_ALTM 0xA7 -#define SYM_DISTHOME_M 0xBB -#define SYM_M 0x0C +#define SYM_MS 0x9F +#define SYM_KMH 0xA5 +#define SYM_ALTM 0xA7 +#define SYM_DISTHOME_M 0xBB +#define SYM_M 0x0C // Unit Icon´s (Imperial) -#define SYM_FTS 0x99 -#define SYM_MPH 0xA6 -#define SYM_ALTFT 0xA8 +#define SYM_FTS 0x99 +#define SYM_MPH 0xA6 +#define SYM_ALTFT 0xA8 #define SYM_DISTHOME_FT 0xB9 -#define SYM_FT 0x0F +#define SYM_FT 0x0F // Voltage and amperage -#define SYM_VOLT 0x06 -#define SYM_AMP 0x9A -#define SYM_MAH 0x07 -#define SYM_WATT 0x57 +#define SYM_VOLT 0x06 +#define SYM_AMP 0x9A +#define SYM_MAH 0x07 +#define SYM_WATT 0x57 // Flying Mode -#define SYM_ACRO 0xAE -#define SYM_ACROGY 0x98 -#define SYM_ACRO1 0xAF -#define SYM_STABLE 0xAC -#define SYM_STABLE1 0xAD -#define SYM_HORIZON 0xC4 -#define SYM_HORIZON1 0xC5 -#define SYM_PASS 0xAA -#define SYM_PASS1 0xAB -#define SYM_AIR 0xEA -#define SYM_AIR1 0xEB -#define SYM_PLUS 0x89 +#define SYM_ACRO 0xAE +#define SYM_ACROGY 0x98 +#define SYM_ACRO1 0xAF +#define SYM_STABLE 0xAC +#define SYM_STABLE1 0xAD +#define SYM_HORIZON 0xC4 +#define SYM_HORIZON1 0xC5 +#define SYM_PASS 0xAA +#define SYM_PASS1 0xAB +#define SYM_AIR 0xEA +#define SYM_AIR1 0xEB +#define SYM_PLUS 0x89 // Note, these change with scrolling enabled (scrolling is TODO) //#define SYM_AH_DECORATION_LEFT 0x13 @@ -201,14 +198,14 @@ #define SYM_AH_DECORATION 0x13 // Time -#define SYM_ON_M 0x9B +#define SYM_ON_M 0x9B #define SYM_FLY_M 0x9C -#define SYM_ON_H 0x70 +#define SYM_ON_H 0x70 #define SYM_FLY_H 0x71 // Throttle Position (%) -#define SYM_THR 0x04 -#define SYM_THR1 0x05 +#define SYM_THR 0x04 +#define SYM_THR1 0x05 // RSSI #define SYM_RSSI 0x01 @@ -223,4 +220,4 @@ #define SYM_MIN 0xB3 #define SYM_AVG 0xB4 -#endif +#endif // USE_MAX7456 diff --git a/src/main/io/osd.c b/src/main/io/osd.c old mode 100644 new mode 100755 index f165dbe39..feffc25bc --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -15,6 +15,11 @@ * along with Cleanflight. If not, see . */ +/* + Created by Marcin Baliniak + some functions based on MinimOSD + */ + #include #include #include @@ -103,6 +108,7 @@ #include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -117,753 +123,1582 @@ #include "common/printf.h" -#define MICROSECONDS_IN_A_SECOND (1000 * 1000) +#define IS_HI(X) (rcData[X] > 1750) +#define IS_LO(X) (rcData[X] < 1250) +#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) -#define OSD_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5) -#define OSD_LINE_LENGTH 30 +//key definiotion because API provide menu navigation over MSP/GUI app - not used NOW +#define KEY_ENTER 0 +#define KEY_UP 1 +#define KEY_DOWN 2 +#define KEY_LEFT 3 +#define KEY_RIGHT 4 +#define KEY_ESC 5 -#define STICKMIN 10 -#define STICKMAX 90 +//osd current screen - to reduce long lines ;-) +#define OSD_cfg masterConfig.osdProfile +#define curr_profile masterConfig.profile[masterConfig.current_profile_index] -/** Artificial Horizon limits **/ -#define AHIPITCHMAX 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees -#define AHIROLLMAX 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees -#define AHISIDEBARWIDTHPOSITION 7 -#define AHISIDEBARHEIGHTPOSITION 3 +uint16_t refreshTimeout = 0; -static uint32_t next_osd_update_at = 0; -static uint32_t armed_seconds = 0; -static uint32_t armed_at = 0; -static uint8_t armed = 0; +#define VISIBLE_FLAG 0x0800 +#define BLINK_FLAG 0x0400 +uint8_t blinkState = 1; -static uint8_t current_page = 0; -static uint8_t sticks[] = {0,0,0,0}; +#define OSD_POS(x,y) (x | (y << 5)) +#define OSD_X(x) (x & 0x001F) +#define OSD_Y(x) ((x >> 5) & 0x001F) +#define VISIBLE(x) (x & VISIBLE_FLAG) +#define BLINK(x) ((x & BLINK_FLAG) && blinkState) +#define BLINK_OFF(x) (x & ~BLINK_FLAG) -static uint8_t cursor_row = 255; -static uint8_t cursor_col = 0; -static uint8_t in_menu = 0; -static uint8_t activating_menu = 0; +extern uint8_t RSSI; // TODO: not used? -static char string_buffer[30]; +static uint16_t flyTime = 0; +uint8_t statRssi; -extern uint16_t rssi; +statistic_t stats; -enum { - MENU_VALUE_DECREASE = 0, - MENU_VALUE_INCREASE, +#define BUTTON_TIME 2 +#define BUTTON_PAUSE 5 +#define REFRESH_1S 12 + +#define LEFT_MENU_COLUMN 1 +#define RIGHT_MENU_COLUMN 23 +#define MAX_MENU_ITEMS (max7456_get_rows_count() - 2) + +uint8_t osdRows; + +//type of elements +typedef enum +{ + OME_Label, + OME_Back, + OME_OSD_Exit, + OME_Submenu, + OME_Bool, + OME_INT8, + OME_UINT8, + OME_UINT16, + OME_INT16, + OME_FLOAT, //only up to 255 value and cant be 2.55 or 25.5, just for PID's + //wlasciwosci elementow + OME_VISIBLE, + OME_POS, + OME_TAB, + OME_END, +} OSD_MenuElement; + +//local variable to detect arm/disarm and show statistic etc +uint8_t armState; +uint8_t featureBlackbox = 0; +uint8_t featureLedstrip = 0; + +#if defined(VTX) || defined(USE_RTC6705) +uint8_t featureVtx = 0, vtxBand, vtxChannel; +#endif // VTX || USE_RTC6705 + +// We are in menu flag +bool inMenu = false; + +typedef void (* OSDMenuFuncPtr)(void *data); + +void osdUpdate(uint32_t currentTime); +void osdOpenMenu(void); +void osdExitMenu(void * ptr); +void osdMenuBack(void); +void osdEditElement(void *ptr); +void osdEraseFlash(void *ptr); +void osdUpdateMaxRows(void); +void osdChangeScreen(void * ptr); +void osdDrawElements(void); +void osdDrawSingleElement(uint8_t item); + +typedef struct +{ + char *text; + OSD_MenuElement type; + OSDMenuFuncPtr func; + void *data; +} OSD_Entry; + +typedef struct +{ + uint8_t *val; + uint8_t min; + uint8_t max; + uint8_t step; +} OSD_UINT8_t; + +typedef struct +{ + int8_t *val; + int8_t min; + int8_t max; + int8_t step; +} OSD_INT8_t; + +typedef struct +{ + int16_t *val; + int16_t min; + int16_t max; + int16_t step; +} OSD_INT16_t; + +typedef struct +{ + uint16_t *val; + uint16_t min; + uint16_t max; + uint16_t step; +} OSD_UINT16_t; + +typedef struct +{ + uint8_t *val; + uint8_t min; + uint8_t max; + uint8_t step; + uint16_t multipler; +} OSD_FLOAT_t; + +typedef struct +{ + uint8_t *val; + uint8_t max; + const char * const *names; +} OSD_TAB_t; + +OSD_Entry *menuStack[10]; //tab to save menu stack +uint8_t menuStackHistory[10]; //current position in menu stack +uint8_t menuStackIdx = 0; + +OSD_Entry *currentMenu; +OSD_Entry *nextPage = NULL; + +int8_t currentMenuPos = 0; +uint8_t currentMenuIdx = 0; +uint16_t *currentElement = NULL; + +OSD_Entry menuAlarms[]; +OSD_Entry menuOsdLayout[]; +OSD_Entry menuOsdActiveElems[]; +OSD_Entry menuOsdElemsPositions[]; +OSD_Entry menuFeatures[]; +OSD_Entry menuBlackbox[]; +#ifdef LED_STRIP +OSD_Entry menuLedstrip[]; +#endif // LED_STRIP +#if defined(VTX) || defined(USE_RTC6705) +OSD_Entry menu_vtx[]; +#endif // VTX || USE_RTC6705 +OSD_Entry menuImu[]; +OSD_Entry menuPid[]; +OSD_Entry menuRc[]; +OSD_Entry menuRateExpo[]; +OSD_Entry menuMisc[]; + +OSD_Entry menuMain[] = +{ + {"----MAIN MENU----", OME_Label, NULL, NULL}, + {"SCREEN LAYOUT", OME_Submenu, osdChangeScreen, &menuOsdLayout[0]}, + {"ALARMS", OME_Submenu, osdChangeScreen, &menuAlarms[0]}, + {"CFG. IMU", OME_Submenu, osdChangeScreen, &menuImu[0]}, + {"FEATURES", OME_Submenu, osdChangeScreen, &menuFeatures[0]}, + {"SAVE & EXIT", OME_OSD_Exit, osdExitMenu, (void*)1}, + {"EXIT", OME_OSD_Exit, osdExitMenu, (void*)0}, + {NULL,OME_END, NULL, NULL} }; -#ifdef USE_RTC6705 +OSD_Entry menuFeatures[] = +{ + {"----- FEATURES -----", OME_Label, NULL, NULL}, + {"BLACKBOX", OME_Submenu, osdChangeScreen, &menuBlackbox[0]}, +#ifdef LED_STRIP + {"LED STRIP", OME_Submenu, osdChangeScreen, &menuLedstrip[0]}, +#endif // LED_STRIP +#if defined(VTX) || defined(USE_RTC6705) + {"VTX", OME_Submenu, osdChangeScreen, &menu_vtx[0]}, +#endif // VTX || USE_RTC6705 + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; -static const char *vtx_bands[] = { - "BOSCAM A", +OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; + +OSD_Entry menuBlackbox[] = +{ + {"--- BLACKBOX ---", OME_Label, NULL, NULL}, + {"ENABLED", OME_Bool, NULL, &featureBlackbox}, + {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom}, +#ifdef USE_FLASHFS + {"ERASE FLASH", OME_Submenu, osdEraseFlash, NULL}, +#endif // USE_FLASHFS + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; + +#ifdef LED_STRIP +//local variable to keep color value +uint8_t ledColor; + +static const char * const LED_COLOR_NAMES[] = { + " BLACK", + " WHITE", + " RED", + " ORANGE", + " YELLOW", + " LIME GREEN", + " GREEN", + " MINT GREEN", + " CYAN", + " LIGHT BLUE", + " BLUE", + "DARK VIOLET", + " MAGNETA", + " DEEP PINK" +}; + +//find first led with color flag and restore color index +//after saving all leds with flags color will have color set in OSD +void getLedColor(void) +{ + for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + + int fn = ledGetFunction(ledConfig); + + if (fn == LED_FUNCTION_COLOR) { + ledColor = ledGetColor(ledConfig); + break; + } + } +} + +//udate all leds with flag color +void applyLedColor(void * ptr) +{ + UNUSED(ptr); + for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { + ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if (ledGetFunction(ledConfig) == LED_FUNCTION_COLOR) + *ledConfig = DEFINE_LED(ledGetX(ledConfig), ledGetY(ledConfig), ledColor, ledGetDirection(ledConfig), ledGetFunction(ledConfig), ledGetOverlay(ledConfig), 0); + } +} + +OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; + +OSD_Entry menuLedstrip[] = +{ + {"--- LED TRIP ---", OME_Label, NULL, NULL}, + {"ENABLED", OME_Bool, NULL, &featureLedstrip}, + {"LED COLOR", OME_TAB, applyLedColor, &entryLed}, + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; +#endif // LED_STRIP + +#if defined(VTX) || defined(USE_RTC6705) +static const char * const vtxBandNames[] = { + "BOACAM A", "BOSCAM B", "BOSCAM E", "FATSHARK", "RACEBAND", }; -void update_vtx_band(int value_change_direction, uint8_t col) { - UNUSED(col); - if (value_change_direction) { - if (current_vtx_channel < (BANDS_NUMBER * CHANNELS_PER_BAND - CHANNELS_PER_BAND)) - current_vtx_channel += CHANNELS_PER_BAND; - } else { - if (current_vtx_channel >= CHANNELS_PER_BAND) - current_vtx_channel -= CHANNELS_PER_BAND; - } -} +OSD_TAB_t entryVtxBand = {&vtxBand,4,&vtxBandNames[0]}; +OSD_UINT8_t entryVtxChannel = {&vtxChannel, 1, 8, 1}; -void print_vtx_band(uint16_t pos, uint8_t col) { - UNUSED(col); - sprintf(string_buffer, "%s", vtx_bands[current_vtx_channel / CHANNELS_PER_BAND]); - max7456_write_string(string_buffer, pos); -} +#ifdef VTX +OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1}; +OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1}; +#endif // VTX -void update_vtx_channel(int value_change_direction, uint8_t col) { - UNUSED(col); - if (value_change_direction) { - if ((current_vtx_channel % CHANNELS_PER_BAND) < (CHANNELS_PER_BAND - 1)) - current_vtx_channel++; - } else { - if ((current_vtx_channel % CHANNELS_PER_BAND) > 0) - current_vtx_channel--; - } -} - -void print_vtx_channel(uint16_t pos, uint8_t col) { - UNUSED(col); - sprintf(string_buffer, "%d", current_vtx_channel % CHANNELS_PER_BAND + 1); - max7456_write_string(string_buffer, pos); -} - -void print_vtx_freq(uint16_t pos, uint8_t col) { - UNUSED(col); - sprintf(string_buffer, "%d M", vtx_freq[current_vtx_channel]); - max7456_write_string(string_buffer, pos); -} - -void update_vtx_power(int value_change_direction, uint8_t col) { - UNUSED(col); - if (value_change_direction) { - masterConfig.vtx_power = 0; - } else { - masterConfig.vtx_power = 1; - } - rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power); -} - -void print_vtx_power(uint16_t pos, uint8_t col) { - UNUSED(col); - sprintf(string_buffer, "%s", masterConfig.vtx_power ? "25MW" : "200MW"); - max7456_write_string(string_buffer, pos); -} - -#endif - -void print_pid(uint16_t pos, uint8_t col, int pid_term) { - switch(col) { - case 0: - sprintf(string_buffer, "%d", currentProfile->pidProfile.P8[pid_term]); - break; - case 1: - sprintf(string_buffer, "%d", currentProfile->pidProfile.I8[pid_term]); - break; - case 2: - sprintf(string_buffer, "%d", currentProfile->pidProfile.D8[pid_term]); - break; - default: - return; - } - max7456_write_string(string_buffer, pos); -} - -void print_roll_pid(uint16_t pos, uint8_t col) { - print_pid(pos, col, ROLL); -} - -void print_pitch_pid(uint16_t pos, uint8_t col) { - print_pid(pos, col, PITCH); -} - -void print_yaw_pid(uint16_t pos, uint8_t col) { - print_pid(pos, col, YAW); -} - -void print_roll_rate(uint16_t pos, uint8_t col) { - if (col == 0) { - sprintf(string_buffer, "%d", currentControlRateProfile->rates[FD_ROLL]); - max7456_write_string(string_buffer, pos); - } -} - -void print_pitch_rate(uint16_t pos, uint8_t col) { - if (col == 0) { - sprintf(string_buffer, "%d", currentControlRateProfile->rates[FD_PITCH]); - max7456_write_string(string_buffer, pos); - } -} - -void print_yaw_rate(uint16_t pos, uint8_t col) { - if (col == 0) { - sprintf(string_buffer, "%d", currentControlRateProfile->rates[FD_YAW]); - max7456_write_string(string_buffer, pos); - } -} - -void print_tpa_rate(uint16_t pos, uint8_t col) { - if (col == 0) { - sprintf(string_buffer, "%d", currentControlRateProfile->dynThrPID); - max7456_write_string(string_buffer, pos); - } -} - -void print_tpa_brkpt(uint16_t pos, uint8_t col) { - if (col == 0) { - sprintf(string_buffer, "%d", currentControlRateProfile->tpa_breakpoint); - max7456_write_string(string_buffer, pos); - } -} - -void update_int_pid(int value_change_direction, uint8_t col, int pid_term) { - void* ptr; - - switch(col) { - case 0: - ptr = ¤tProfile->pidProfile.P8[pid_term]; - break; - case 1: - ptr = ¤tProfile->pidProfile.I8[pid_term]; - break; - case 2: - ptr = ¤tProfile->pidProfile.D8[pid_term]; - break; - default: - return; - } - - if (value_change_direction) { - if (*(uint8_t*)ptr < 200) - *(uint8_t*)ptr += 1; - } else { - if (*(uint8_t*)ptr > 0) - *(uint8_t*)ptr -= 1; - } -} - -void update_roll_pid(int value_change_direction, uint8_t col) { - update_int_pid(value_change_direction, col, ROLL); -} - -void update_pitch_pid(int value_change_direction, uint8_t col) { - update_int_pid(value_change_direction, col, PITCH); -} - -void update_yaw_pid(int value_change_direction, uint8_t col) { - update_int_pid(value_change_direction, col, YAW); -} - -void update_roll_rate(int value_change_direction, uint8_t col) { - UNUSED(col); - - if (value_change_direction) { - if (currentControlRateProfile->rates[FD_ROLL] < CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX) - currentControlRateProfile->rates[FD_ROLL]++; - } else { - if (currentControlRateProfile->rates[FD_ROLL] > 0) - currentControlRateProfile->rates[FD_ROLL]--; - } -} - -void update_pitch_rate(int value_change_direction, uint8_t col) { - UNUSED(col); - - if (value_change_direction) { - if (currentControlRateProfile->rates[FD_PITCH] < CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX) - currentControlRateProfile->rates[FD_PITCH]++; - } else { - if (currentControlRateProfile->rates[FD_PITCH] > 0) - currentControlRateProfile->rates[FD_PITCH]--; - } -} - -void update_yaw_rate(int value_change_direction, uint8_t col) { - UNUSED(col); - - if (value_change_direction) { - if (currentControlRateProfile->rates[FD_YAW] < CONTROL_RATE_CONFIG_YAW_RATE_MAX) - currentControlRateProfile->rates[FD_YAW]++; - } else { - if (currentControlRateProfile->rates[FD_YAW] > 0) - currentControlRateProfile->rates[FD_YAW]--; - } -} - -void update_tpa_rate(int value_change_direction, uint8_t col) { - UNUSED(col); - - if (value_change_direction) { - if (currentControlRateProfile->dynThrPID < CONTROL_RATE_CONFIG_TPA_MAX) - currentControlRateProfile->dynThrPID++; - } else { - if (currentControlRateProfile->dynThrPID > 0) - currentControlRateProfile->dynThrPID--; - } -} - -void update_tpa_brkpt(int value_change_direction, uint8_t col) { - UNUSED(col); - - if (value_change_direction) { - if (currentControlRateProfile->tpa_breakpoint < PWM_RANGE_MAX) - currentControlRateProfile->tpa_breakpoint++; - } else { - if (currentControlRateProfile->tpa_breakpoint > PWM_RANGE_MIN) - currentControlRateProfile->tpa_breakpoint--; - } -} - -void print_average_system_load(uint16_t pos, uint8_t col) { - UNUSED(col); - - sprintf(string_buffer, "%d", averageSystemLoadPercent); - max7456_write_string(string_buffer, pos); -} - -void print_batt_voltage(uint16_t pos, uint8_t col) { - UNUSED(col); - - sprintf(string_buffer, "%d.%1d", vbat / 10, vbat % 10); - max7456_write_string(string_buffer, pos); -} - -osd_page_t menu_pages[] = { - { - .title = "STATUS", - .cols_number = 1, - .rows_number = 2, - .cols = { - { - .title = NULL, - .x_pos = 15 - } - }, - .rows = { - { - .title = "AVG LOAD", - .update = NULL, - .print = print_average_system_load - }, - { - .title = "BATT", - .update = NULL, - .print = print_batt_voltage - }, - } - }, +OSD_Entry menu_vtx[] = +{ + {"--- VTX ---", OME_Label, NULL, NULL}, + {"ENABLED", OME_Bool, NULL, &featureVtx}, +#ifdef VTX + {"VTX MODE", OME_UINT8, NULL, &entryVtxMode}, + {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz}, +#endif // VTX + {"BAND", OME_TAB, NULL, &entryVtxBand}, + {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel}, #ifdef USE_RTC6705 - { - .title = "VTX SETTINGS", - .cols_number = 1, - .rows_number = 4, - .cols = { - { - .title = NULL, - .x_pos = 15 - } - }, - .rows = { - { - .title = "BAND", - .update = update_vtx_band, - .print = print_vtx_band - }, - { - .title = "CHANNEL", - .update = update_vtx_channel, - .print = print_vtx_channel - }, - { - .title = "FREQUENCY", - .update = NULL, - .print = print_vtx_freq - }, - { - .title = "POWER", - .update = update_vtx_power, - .print = print_vtx_power - } - } - }, -#endif - { - .title = "PID SETTINGS", - .cols_number = 3, - .rows_number = 8, - .cols = { - { - .title = "P", - .x_pos = 13 - }, - { - .title = "I", - .x_pos = 19 - }, - { - .title = "D", - .x_pos = 25 - } - }, - .rows = { - { - .title = "ROLL", - .update = update_roll_pid, - .print = print_roll_pid - }, - { - .title = "PITCH", - .update = update_pitch_pid, - .print = print_pitch_pid - }, - { - .title = "YAW", - .update = update_yaw_pid, - .print = print_yaw_pid - }, - { - .title = "ROLL RATE", - .update = update_roll_rate, - .print = print_roll_rate - }, - { - .title = "PITCH RATE", - .update = update_pitch_rate, - .print = print_pitch_rate - }, - { - .title = "YAW RATE", - .update = update_yaw_rate, - .print = print_yaw_rate - }, - { - .title = "TPA RATE", - .update = update_tpa_rate, - .print = print_tpa_rate - }, - { - .title = "TPA BRKPT", - .update = update_tpa_brkpt, - .print = print_tpa_brkpt - }, - } - }, + {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power}, +#endif // USE_RTC6705 + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; +#endif // VTX || USE_RTC6705 + +OSD_UINT16_t entryMinThrottle = {&masterConfig.escAndServoConfig.minthrottle, 1020, 1300, 10}; +OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1}; +OSD_UINT16_t entryDtermLpf = {&masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 5}; +OSD_UINT16_t entryYawLpf = {&masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 5}; +OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 5}; +OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1}; +OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1}; + +OSD_Entry menuMisc[]= +{ + {"----- MISC -----", OME_Label, NULL, NULL}, + {"GYRO LOWPASS", OME_UINT8, NULL, &entryGyroSoftLpfHz}, + {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf}, + {"YAW LPF", OME_UINT16, NULL, &entryYawLpf}, + {"YAW P LIMIT", OME_UINT16, NULL, &entryYawPLimit}, + {"MINTHROTTLE", OME_UINT16, NULL, &entryMinThrottle}, + {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale}, + {"VBAT CELL MAX", OME_UINT8, NULL, &entryVbatMaxCell}, + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} }; -void show_menu(void) { - uint8_t line = 1; - uint16_t pos; - osd_col_t *col; - osd_row_t *row; - int16_t cursor_x = 0; - int16_t cursor_y = 0; +OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; - if (activating_menu) { - if (sticks[YAW] < 60 && sticks[YAW] > 40 && sticks[PITCH] > 40 && sticks[PITCH] < 60 && sticks[ROLL] > 40 && sticks[ROLL] < 60) - activating_menu = false; - else - return; - } - - if (sticks[YAW] > STICKMAX && sticks[ROLL] > STICKMIN && sticks[ROLL] < STICKMAX && sticks[PITCH] > STICKMIN && sticks[PITCH] < STICKMAX) { - if (cursor_row > MAX_MENU_ROWS) { - switch(cursor_col) { - case 0: - in_menu = false; - break; - case 1: - in_menu = false; -#ifdef USE_RTC6705 - if (masterConfig.vtx_channel != current_vtx_channel) { - masterConfig.vtx_channel = current_vtx_channel; - rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); - } -#endif - writeEEPROM(); - break; - case 2: - if (current_page < (sizeof(menu_pages) / sizeof(osd_page_t) - 1)) - current_page++; - else - current_page = 0; - } - } else { - if (menu_pages[current_page].rows[cursor_row].update) - menu_pages[current_page].rows[cursor_row].update(MENU_VALUE_INCREASE, cursor_col); - } - } - - if (sticks[YAW] < STICKMIN && sticks[ROLL] > STICKMIN && sticks[ROLL] < STICKMAX && sticks[PITCH] > STICKMIN && sticks[PITCH] < STICKMAX) { - if (cursor_row > MAX_MENU_ROWS) { - if (cursor_col == 2 && current_page > 0) { - current_page--; - } - } else { - if (menu_pages[current_page].rows[cursor_row].update) - menu_pages[current_page].rows[cursor_row].update(MENU_VALUE_DECREASE, cursor_col); - } - } - - if (sticks[PITCH] > STICKMAX && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { - if (cursor_row > MAX_MENU_ROWS) { - cursor_row = menu_pages[current_page].rows_number - 1; - cursor_col = 0; - } else { - if (cursor_row > 0) - cursor_row--; - } - } - if (sticks[PITCH] < STICKMIN && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { - if (cursor_row < (menu_pages[current_page].rows_number - 1)) - cursor_row++; - else - cursor_row = 255; - } - if (sticks[ROLL] > STICKMAX && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { - if (cursor_row > MAX_MENU_ROWS) { - if (cursor_col < 2) - cursor_col++; - } else { - if (cursor_col < (menu_pages[current_page].cols_number - 1)) - cursor_col++; - } - } - if (sticks[ROLL] < STICKMIN && sticks[YAW] > STICKMIN && sticks[YAW] < STICKMAX) { - if (cursor_col > 0) - cursor_col--; - } - - if (cursor_row > MAX_MENU_ROWS) { - cursor_row = 255; - cursor_y = -1; - switch(cursor_col) { - case 0: - cursor_x = 0; - break; - case 1: - cursor_x = 9; - break; - case 2: - cursor_x = 23; - break; - default: - cursor_x = 0; - } - } - - sprintf(string_buffer, "EXIT SAVE+EXIT PAGE"); - max7456_write_string(string_buffer, -29); - - pos = (OSD_LINE_LENGTH - strlen(menu_pages[current_page].title)) / 2 + line * OSD_LINE_LENGTH; - sprintf(string_buffer, "%s", menu_pages[current_page].title); - max7456_write_string(string_buffer, pos); - - line += 2; - - for (int i = 0; i < menu_pages[current_page].cols_number; i++){ - col = &menu_pages[current_page].cols[i]; - if (cursor_col == i && cursor_row < MAX_MENU_ROWS) - cursor_x = col->x_pos - 1; - - if (col->title) { - sprintf(string_buffer, "%s", col->title); - max7456_write_string(string_buffer, line * OSD_LINE_LENGTH + col->x_pos); - } - } - - line++; - for (int i = 0; i < menu_pages[current_page].rows_number; i++) { - row = &menu_pages[current_page].rows[i]; - if (cursor_row == i) - cursor_y = line; - - sprintf(string_buffer, "%s", row->title); - max7456_write_string(string_buffer, line * OSD_LINE_LENGTH + 1); - for (int j = 0; j < menu_pages[current_page].cols_number; j++) { - col = &menu_pages[current_page].cols[j]; - row->print(line * OSD_LINE_LENGTH + col->x_pos, j); - } - line++; - } - - max7456_write_string(">", cursor_x + cursor_y * OSD_LINE_LENGTH); -} - -// Write the artifical horizon to the screen buffer -void osdDrawArtificialHorizon(int rollAngle, int pitchAngle, uint8_t show_sidebars) { - uint16_t position = 194; - MAX7456_CHAR_TYPE *screenBuffer = max7456_get_screen_buffer(); - - if (pitchAngle > AHIPITCHMAX) - pitchAngle = AHIPITCHMAX; - if (pitchAngle < -AHIPITCHMAX) - pitchAngle = -AHIPITCHMAX; - if (rollAngle > AHIROLLMAX) - rollAngle = AHIROLLMAX; - if (rollAngle < -AHIROLLMAX) - rollAngle = -AHIROLLMAX; - - for (uint8_t X = 0; X <= 8; X++) { - if (X == 4) - X = 5; - int Y = (rollAngle * (4 - X)) / 64; - Y -= pitchAngle / 8; - Y += 41; - if (Y >= 0 && Y <= 81) { - uint16_t pos = position - 7 + LINE * (Y / 9) + 3 - 4 * LINE + X; - screenBuffer[pos] = MAX7456_CHAR(SYM_AH_BAR9_0 + (Y % 9)); - } - } - screenBuffer[position - 1] = MAX7456_CHAR(SYM_AH_CENTER_LINE); - screenBuffer[position + 1] = MAX7456_CHAR(SYM_AH_CENTER_LINE_RIGHT); - screenBuffer[position] = MAX7456_CHAR(SYM_AH_CENTER); - - if (show_sidebars) { - // Draw AH sides - int8_t hudwidth = AHISIDEBARWIDTHPOSITION; - int8_t hudheight = AHISIDEBARHEIGHTPOSITION; - for (int8_t X = -hudheight; X <= hudheight; X++) { - screenBuffer[position - hudwidth + (X * LINE)] = MAX7456_CHAR(SYM_AH_DECORATION); - screenBuffer[position + hudwidth + (X * LINE)] = MAX7456_CHAR(SYM_AH_DECORATION); - } - // AH level indicators - screenBuffer[position-hudwidth+1] = MAX7456_CHAR(SYM_AH_LEFT); - screenBuffer[position+hudwidth-1] = MAX7456_CHAR(SYM_AH_RIGHT); - } -} - -void updateOsd(uint32_t currentTime) +OSD_Entry menuImu[] = { - static uint8_t blink = 0; - static uint8_t arming = 0; - uint32_t seconds; - char line[30]; + {"-----CFG. IMU-----", OME_Label, NULL, NULL}, + {"PID", OME_Submenu, osdChangeScreen, &menuPid[0]}, + {"PID PROFILE", OME_UINT8, NULL, &entryPidProfile}, + {"RATE & RXPO", OME_Submenu, osdChangeScreen, &menuRateExpo[0]}, + {"RC PREVIEW", OME_Submenu, osdChangeScreen, &menuRc[0]}, + {"MISC", OME_Submenu, osdChangeScreen, &menuMisc[0]}, + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; - const bool updateNow = (int32_t)(currentTime - next_osd_update_at) >= 0L; - if (!updateNow) { - return; - } - next_osd_update_at = currentTime + OSD_UPDATE_FREQUENCY; - blink++; +uint8_t tempPid[4][3]; - if (ARMING_FLAG(ARMED)) { - if (!armed) { - armed = true; - armed_at = currentTime; - in_menu = false; - arming = 5; - } - } else { - if (armed) { - armed = false; - armed_seconds += ((currentTime - armed_at) / 1000000); - } - for (uint8_t channelIndex = 0; channelIndex < 4; channelIndex++) { - sticks[channelIndex] = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); - } - if (!in_menu && sticks[YAW] > STICKMAX && sticks[THROTTLE] > STICKMIN && sticks[THROTTLE] < STICKMAX && sticks[ROLL] > STICKMIN && sticks[ROLL] < STICKMAX && sticks[PITCH] > STICKMAX) { - in_menu = true; - cursor_row = 255; - cursor_col = 2; - activating_menu = true; - } - } - if (in_menu) { - show_menu(); - } else { - if (batteryWarningVoltage > vbat && (blink & 1) && masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] != -1) { - max7456_write_string("LOW VOLTAGE", masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING]); - } - if (arming && (blink & 1) && masterConfig.osdProfile.item_pos[OSD_ARMED] != -1) { - max7456_write_string("ARMED", masterConfig.osdProfile.item_pos[OSD_ARMED]); - arming--; - } - if (!armed && masterConfig.osdProfile.item_pos[OSD_DISARMED] != -1) { - max7456_write_string("DISARMED", masterConfig.osdProfile.item_pos[OSD_DISARMED]); - } +static OSD_UINT8_t entryRollP = {&tempPid[PIDROLL][0], 10, 150, 1}; +static OSD_UINT8_t entryRollI = {&tempPid[PIDROLL][1], 1, 150, 1}; +static OSD_UINT8_t entryRollD = {&tempPid[PIDROLL][2], 0, 150, 1}; - if (masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] != -1) { - line[0] = SYM_VOLT; - sprintf(line+1, "%d.%1d", vbat / 10, vbat % 10); - max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]); - } +static OSD_UINT8_t entryPitchP = {&tempPid[PIDPITCH][0], 10, 150, 1}; +static OSD_UINT8_t entryPitchI = {&tempPid[PIDPITCH][1], 1, 150, 1}; +static OSD_UINT8_t entryPitchD = {&tempPid[PIDPITCH][2], 0, 150, 1}; - if (masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW] != -1) { - line[0] = SYM_AMP; - sprintf(line+1, "%d.%02d", amperage / 100, amperage % 100); - max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW]); - } +static OSD_UINT8_t entryYawP = {&tempPid[PIDYAW][0], 10, 150, 1}; +static OSD_UINT8_t entryYawI = {&tempPid[PIDYAW][1], 1, 150, 1}; +static OSD_UINT8_t entryYawD = {&tempPid[PIDYAW][2], 0, 150, 1}; - if (masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] != -1) { - line[0] = SYM_MAH; - sprintf(line+1, "%d", mAhDrawn); - max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN]); - } +OSD_Entry menuPid[] = +{ + {"------- PID -------", OME_Label, NULL, NULL}, + {"ROLL P", OME_UINT8, NULL, &entryRollP}, + {"ROLL I", OME_UINT8, NULL, &entryRollI}, + {"ROLL D", OME_UINT8, NULL, &entryRollD}, - if (masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME] != -1) { - for (uint8_t i = 0; i < MAX_NAME_LENGTH; i++) { - line[i] = toupper((unsigned char)masterConfig.name[i]); - if (masterConfig.name[i] == 0) - break; - } - max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME]); - } + {"PITCH P", OME_UINT8, NULL, &entryPitchP}, + {"PITCH I", OME_UINT8, NULL, &entryPitchI}, + {"PITCH D", OME_UINT8, NULL, &entryPitchD}, - if (masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] != -1) { - line[0] = SYM_RSSI; - sprintf(line+1, "%d", rssi / 10); - max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]); - } - if (masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] != -1) { - line[0] = SYM_THR; - line[1] = SYM_THR1; - sprintf(line+2, "%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); - max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]); - } + {"YAW P", OME_UINT8, NULL, &entryYawP}, + {"YAW I", OME_UINT8, NULL, &entryYawI}, + {"YAW D", OME_UINT8, NULL, &entryYawD}, - if (masterConfig.osdProfile.item_pos[OSD_ALTITUDE] != -1) { - int16_t alt = BaroAlt; // Metre x 100 - char unitSym = 0xC; // m - if (masterConfig.osdProfile.units == OSD_UNIT_IMPERIAL) { - alt = (alt * 328) / 100; // Convert to feet x 100 - unitSym = 0xF; // ft - } + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; - sprintf(line, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), unitSym); - max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_ALTITUDE]); - } +controlRateConfig_t rateProfile; - if (masterConfig.osdProfile.item_pos[OSD_TIMER] != -1) { - if (armed) { - seconds = armed_seconds + ((currentTime - armed_at) / 1000000); - line[0] = SYM_FLY_M; - sprintf(line+1, " %02d:%02d", seconds / 60, seconds % 60); - } else { - line[0] = SYM_ON_M; - seconds = currentTime / 1000000; - sprintf(line+1, " %02d:%02d", seconds / 60, seconds % 60); - } - max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_TIMER]); - } - if (masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] != -1) { - print_average_system_load(masterConfig.osdProfile.item_pos[OSD_CPU_LOAD], 0); - } - if (masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON] != -1) { - osdDrawArtificialHorizon(attitude.values.roll, attitude.values.pitch, masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] != -1); - } - } - max7456_draw_screen(); +static OSD_FLOAT_t entryRollRate = {&rateProfile.rates[0], 0, 250, 1, 10}; +static OSD_FLOAT_t entryPitchRate = {&rateProfile.rates[1], 0, 250, 1, 10}; +static OSD_FLOAT_t entryYawRate = {&rateProfile.rates[2], 0, 250, 1, 10}; +static OSD_FLOAT_t entryRcRate = {&rateProfile.rcRate8, 0, 200, 1, 10}; +static OSD_FLOAT_t entryRcExpo = {&rateProfile.rcExpo8, 0, 100, 1, 10}; +static OSD_FLOAT_t entryRcExpoYaw = {&rateProfile.rcYawExpo8, 0, 100, 1, 10}; +static OSD_FLOAT_t extryTpaEntry = {&rateProfile.dynThrPID, 0, 70, 1, 10}; +static OSD_UINT16_t entryTpaBreak = {&rateProfile.tpa_breakpoint, 1100, 1800, 10}; +static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.ptermSRateWeight, 0, 100, 1, 10}; +static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10}; + +OSD_Entry menuRateExpo[] = +{ + {"----RATE & EXPO----", OME_Label, NULL, NULL}, + {"ROLL RATE", OME_FLOAT, NULL, &entryRollRate}, + {"PITCH RATE", OME_FLOAT, NULL, &entryPitchRate}, + {"YAW RATE", OME_FLOAT, NULL, &entryYawRate}, + {"RC RATE", OME_FLOAT, NULL, &entryRcRate}, + {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo}, + {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw}, + {"THR. PID ATT.", OME_FLOAT, NULL, &extryTpaEntry}, + {"TPA BREAKPOINT", OME_UINT16, NULL, &entryTpaBreak}, + {"PTERM SRATE RATIO", OME_FLOAT, NULL, &entryPSetpoint}, + {"D SETPOINT", OME_FLOAT, NULL, &entryDSetpoint}, + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; + +static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0}; +static OSD_INT16_t entryRcPitch = {&rcData[PITCH], 1, 2500, 0}; +static OSD_INT16_t entryRcThrottle = {&rcData[THROTTLE], 1, 2500, 0}; +static OSD_INT16_t entryRcYaw = {&rcData[YAW], 1, 2500, 0}; +static OSD_INT16_t entryRcAux1 = {&rcData[AUX1], 1, 2500, 0}; +static OSD_INT16_t entryRcAux2 = {&rcData[AUX2], 1, 2500, 0}; +static OSD_INT16_t entryRcAux3 = {&rcData[AUX3], 1, 2500, 0}; +static OSD_INT16_t entryRcAux4 = {&rcData[AUX4], 1, 2500, 0}; + +OSD_Entry menuRc[] = +{ + {"---- RC PREVIEW ----", OME_Label, NULL, NULL}, + {"ROLL", OME_INT16, NULL, &entryRcRoll}, + {"PITCH", OME_INT16, NULL, &entryRcPitch}, + {"THROTTLE", OME_INT16, NULL, &entryRcThrottle}, + {"YAW", OME_INT16, NULL, &entryRcYaw}, + {"AUX1", OME_INT16, NULL, &entryRcAux1}, + {"AUX2", OME_INT16, NULL, &entryRcAux2}, + {"AUX3", OME_INT16, NULL, &entryRcAux3}, + {"AUX4", OME_INT16, NULL, &entryRcAux4}, + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; + +OSD_Entry menuOsdLayout[] = +{ + {"---SCREEN LAYOUT---", OME_Label, NULL, NULL}, + {"ACTIVE ELEM.", OME_Submenu, osdChangeScreen, &menuOsdActiveElems[0]}, + {"POSITION", OME_Submenu, osdChangeScreen, &menuOsdElemsPositions[0]}, + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; + +OSD_UINT8_t entryAlarmRssi = {&OSD_cfg.rssi_alarm, 5, 90, 5}; +OSD_UINT16_t entryAlarmCapacity = {&OSD_cfg.cap_alarm, 50, 30000, 50}; +OSD_UINT16_t enryAlarmFlyTime = {&OSD_cfg.time_alarm, 1, 200, 1}; +OSD_UINT16_t entryAlarmAltitude = {&OSD_cfg.alt_alarm, 1, 200, 1}; + +OSD_Entry menuAlarms[] = +{ + {"------ ALARMS ------", OME_Label, NULL, NULL}, + {"RSSI", OME_UINT8, NULL, &entryAlarmRssi}, + {"MAIN BATT.", OME_UINT16, NULL, &entryAlarmCapacity}, + {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime}, + {"MAX ALTITUDE", OME_UINT16, NULL, &entryAlarmAltitude}, + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; + +OSD_Entry menuOsdElemsPositions[] = +{ + {"---POSITION---", OME_Label, NULL, NULL}, + {"RSSI", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]}, + {"MAIN BATTERY", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]}, + {"UPTIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ONTIME]}, + {"FLY TIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYTIME]}, + {"FLY MODE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYMODE]}, + {"NAME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME]}, + {"THROTTLE POS", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]}, +#ifdef VTX + {"VTX CHAN", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, +#endif // VTX + {"CURRENT (A)", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW]}, + {"USED MAH", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN]}, +#ifdef GPS + {"GPS SPEED", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED]}, + {"GPS SATS.", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS]}, +#endif // GPS + {"ALTITUDE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; + +OSD_Entry menuOsdActiveElems[] = +{ + {" --ACTIV ELEM.-- ", OME_Label, NULL, NULL}, + {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]}, + {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]}, + {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]}, + {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS]}, + {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME]}, + {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME]}, + {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE]}, + {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME]}, + {"THROTTLE POS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]}, +#ifdef VTX + {"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, +#endif // VTX + {"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW]}, + {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN]}, +#ifdef GPS + {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED]}, + {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS]}, +#endif // GPS + {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; + +void resetOsdConfig(osd_profile_t *osdProfile) +{ + osdProfile->item_pos[OSD_RSSI_VALUE] = OSD_POS(22, 0) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_ONTIME] = OSD_POS(22, 11) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_FLYTIME] = OSD_POS(22, 12) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_FLYMODE] = OSD_POS(12, 11) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_CRAFT_NAME] = OSD_POS(12, 12); + osdProfile->item_pos[OSD_THROTTLE_POS] = OSD_POS(1, 4); + osdProfile->item_pos[OSD_VTX_CHANNEL] = OSD_POS(8, 6); + osdProfile->item_pos[OSD_CURRENT_DRAW] = OSD_POS(1, 3); + osdProfile->item_pos[OSD_MAH_DRAWN] = OSD_POS(15, 3); + osdProfile->item_pos[OSD_GPS_SPEED] = OSD_POS(2, 2); + osdProfile->item_pos[OSD_GPS_SATS] = OSD_POS(2, 12); + osdProfile->item_pos[OSD_ALTITUDE] = OSD_POS(1, 5); + + osdProfile->rssi_alarm = 20; + osdProfile->cap_alarm = 2200; + osdProfile->time_alarm = 10; // in minutes + osdProfile->alt_alarm = 100; // meters or feet depend on configuration + + osdProfile->video_system = 0; } void osdInit(void) { - uint16_t x; - MAX7456_CHAR_TYPE* screen_buffer = max7456_get_screen_buffer(); + char x, string_buffer[30]; + + armState = ARMING_FLAG(ARMED); max7456_init(masterConfig.osdProfile.video_system); + max7456_clear_screen(); + // display logo and help - x = 160; + x = 160; for (int i = 1; i < 5; i++) { - for (int j = 3; j < 27; j++) + for (int j = 3; j < 27; j++) { if (x != 255) - screen_buffer[(i * LINE + j)] = MAX7456_CHAR(x++); + max7456_write_char(j, i, x++); + } } + sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); - max7456_write_string(string_buffer, LINE06 + 5); - max7456_write_string("MENU: THRT MID", LINE07 + 7); - max7456_write_string("YAW RIGHT", LINE08 + 13); - max7456_write_string("PITCH UP", LINE09 + 13); - max7456_draw_screen(); + max7456_write(5, 6, string_buffer); + max7456_write(7, 7, "MENU: THRT MID"); + max7456_write(13, 8, "YAW RIGHT"); + max7456_write(13, 9, "PITCH UP"); + max7456_refresh_all(); + + refreshTimeout = 4 * REFRESH_1S; } -void resetOsdConfig(osd_profile *osdProfile) +void osdUpdateAlarms(void) { - osdProfile->video_system = AUTO; - osdProfile->units = OSD_UNIT_IMPERIAL; - osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; - osdProfile->item_pos[OSD_RSSI_VALUE] = -59; - osdProfile->item_pos[OSD_TIMER] = -39; - osdProfile->item_pos[OSD_THROTTLE_POS] = -9; - osdProfile->item_pos[OSD_CPU_LOAD] = 26; - osdProfile->item_pos[OSD_VTX_CHANNEL] = 1; - osdProfile->item_pos[OSD_VOLTAGE_WARNING] = -80; - osdProfile->item_pos[OSD_ARMED] = -107; - osdProfile->item_pos[OSD_DISARMED] = -109; - osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = -1; - osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = -1; - osdProfile->item_pos[OSD_CURRENT_DRAW] = -23; - osdProfile->item_pos[OSD_MAH_DRAWN] = -18; - osdProfile->item_pos[OSD_CRAFT_NAME] = 1; - osdProfile->item_pos[OSD_ALTITUDE] = 60; + uint16_t rval = rssi * 100 / 1024; // zmiana zakresu + int32_t alt = BaroAlt / 100; + + if (rval < OSD_cfg.rssi_alarm) + OSD_cfg.item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; + else + OSD_cfg.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + + if (vbat <= (batteryWarningVoltage - 1)) + OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; + else + OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + + if (STATE(GPS_FIX) == 0) + OSD_cfg.item_pos[OSD_GPS_SATS] |= BLINK_FLAG; + else + OSD_cfg.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + + if (flyTime / 60 >= OSD_cfg.time_alarm && ARMING_FLAG(ARMED)) + OSD_cfg.item_pos[OSD_FLYTIME] |= BLINK_FLAG; + else + OSD_cfg.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + + if (mAhDrawn >= OSD_cfg.cap_alarm) + OSD_cfg.item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; + else + OSD_cfg.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; + + if (masterConfig.osdProfile.units == OSD_UNIT_IMPERIAL) { + alt = (alt * 328) / 100; // Convert to feet + } + + if (alt >= OSD_cfg.alt_alarm) + OSD_cfg.item_pos[OSD_ALTITUDE] |= BLINK_FLAG; + else + OSD_cfg.item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; } -#endif + +void osdResetAlarms(void) +{ + OSD_cfg.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + OSD_cfg.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + OSD_cfg.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + OSD_cfg.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; +} + +uint8_t osdHandleKey(uint8_t key) +{ + uint8_t res = BUTTON_TIME; + OSD_Entry *p; + + if (!currentMenu) + return res; + + if (key == KEY_ESC) { + osdMenuBack(); + return BUTTON_PAUSE; + } + + if (key == KEY_DOWN) { + if (currentMenuPos < currentMenuIdx) + currentMenuPos++; + else { + if (nextPage) // we have more pages + { + max7456_clear_screen(); + p = nextPage; + nextPage = currentMenu; + currentMenu = (OSD_Entry *)p; + currentMenuPos = 0; + osdUpdateMaxRows(); + } + currentMenuPos = 0; + } + } + + if (key == KEY_UP) { + currentMenuPos--; + + if ((currentMenu + currentMenuPos)->type == OME_Label && currentMenuPos > 0) + currentMenuPos--; + + if (currentMenuPos == -1 || (currentMenu + currentMenuPos)->type == OME_Label) { + if (nextPage) { + max7456_clear_screen(); + p = nextPage; + nextPage = currentMenu; + currentMenu = (OSD_Entry *)p; + currentMenuPos = 0; + osdUpdateMaxRows(); + } + currentMenuPos = currentMenuIdx; + } + } + + if (key == KEY_DOWN || key == KEY_UP) + return res; + + p = currentMenu + currentMenuPos; + + switch (p->type) { + case OME_POS: + if (key == KEY_RIGHT) { + uint32_t address = (uint32_t)p->data; + uint16_t *val; + + val = (uint16_t *)address; + if (!(*val & VISIBLE_FLAG)) // no submenu for hidden elements + break; + } + case OME_Submenu: + case OME_OSD_Exit: + if (p->func && key == KEY_RIGHT) { + p->func(p->data); + res = BUTTON_PAUSE; + } + break; + case OME_Back: + osdMenuBack(); + res = BUTTON_PAUSE; + break; + case OME_Bool: + if (p->data) { + uint8_t *val = p->data; + if (key == KEY_RIGHT) + *val = 1; + else + *val = 0; + } + break; + case OME_VISIBLE: + if (p->data) { + uint32_t address = (uint32_t)p->data; + uint16_t *val; + + val = (uint16_t *)address; + + if (key == KEY_RIGHT) + *val |= VISIBLE_FLAG; + else + *val %= ~VISIBLE_FLAG; + } + break; + case OME_UINT8: + case OME_FLOAT: + if (p->data) { + OSD_UINT8_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + } + break; + case OME_TAB: + if (p->type == OME_TAB) { + OSD_TAB_t *ptr = p->data; + + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += 1; + } + else { + if (*ptr->val > 0) + *ptr->val -= 1; + } + if (p->func) + p->func(p->data); + } + break; + case OME_INT8: + if (p->data) { + OSD_INT8_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + } + break; + case OME_UINT16: + if (p->data) { + OSD_UINT16_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + } + break; + case OME_INT16: + if (p->data) { + OSD_INT16_t *ptr = p->data; + if (key == KEY_RIGHT) { + if (*ptr->val < ptr->max) + *ptr->val += ptr->step; + } + else { + if (*ptr->val > ptr->min) + *ptr->val -= ptr->step; + } + } + break; + case OME_Label: + case OME_END: + break; + } + return res; +} + +void osdUpdateMaxRows(void) +{ + OSD_Entry *ptr; + + currentMenuIdx = 0; + for (ptr = currentMenu; ptr->type != OME_END; ptr++) + currentMenuIdx++; + + if (currentMenuIdx > MAX_MENU_ITEMS) + currentMenuIdx = MAX_MENU_ITEMS; + + currentMenuIdx--; +} + +void osdMenuBack(void) +{ + uint8_t i; + + // becasue pids and rates meybe stored in profiles we need some thicks to manipulate it + // hack to save pid profile + if (currentMenu == &menuPid[0]) { + for (i = 0; i < 3; i++) { + curr_profile.pidProfile.P8[i] = tempPid[i][0]; + curr_profile.pidProfile.I8[i] = tempPid[i][1]; + curr_profile.pidProfile.D8[i] = tempPid[i][2]; + } + + curr_profile.pidProfile.P8[PIDLEVEL] = tempPid[3][0]; + curr_profile.pidProfile.I8[PIDLEVEL] = tempPid[3][1]; + curr_profile.pidProfile.D8[PIDLEVEL] = tempPid[3][2]; + } + + // hack - save rate config for current profile + if (currentMenu == &menuRateExpo[0]) + memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t)); + + if (menuStackIdx) { + max7456_clear_screen(); + menuStackIdx--; + nextPage = NULL; + currentMenu = menuStack[menuStackIdx]; + currentMenuPos = menuStackHistory[menuStackIdx]; + + osdUpdateMaxRows(); + } + else + osdOpenMenu(); +} + +void simple_ftoa(int32_t value, char *floatString) +{ + uint8_t k; + // np. 3450 + + itoa(100000 + value, floatString, 10); // Create string from abs of integer value + + // 103450 + + floatString[0] = floatString[1]; + floatString[1] = floatString[2]; + floatString[2] = '.'; + + // 03.450 + // usuwam koncowe zera i kropke + for (k = 5; k > 1; k--) + if (floatString[k] == '0' || floatString[k] == '.') + floatString[k] = 0; + else + break; + + // oraz zero wiodonce + if (floatString[0] == '0') + floatString[0] = ' '; +} + +void osdDrawMenu(void) +{ + uint8_t i = 0; + OSD_Entry *p; + char buff[10]; + uint8_t top = (osdRows - currentMenuIdx) / 2 - 1; + if (!currentMenu) + return; + + if ((currentMenu + currentMenuPos)->type == OME_Label) // skip label + currentMenuPos++; + + for (p = currentMenu; p->type != OME_END; p++) { + if (currentMenuPos == i) + max7456_write(LEFT_MENU_COLUMN, i + top, " >"); + else + max7456_write(LEFT_MENU_COLUMN, i + top, " "); + max7456_write(LEFT_MENU_COLUMN + 2, i + top, p->text); + + switch (p->type) { + case OME_POS: { + uint32_t address = (uint32_t)p->data; + uint16_t *val; + + val = (uint16_t *)address; + if (!(*val & VISIBLE_FLAG)) + break; + } + case OME_Submenu: + max7456_write(RIGHT_MENU_COLUMN, i + top, ">"); + break; + case OME_Bool: + if (p->data) { + if (*((uint8_t *)(p->data))) + max7456_write(RIGHT_MENU_COLUMN, i + top, "YES"); + else + max7456_write(RIGHT_MENU_COLUMN, i + top, "NO "); + } + break; + case OME_TAB: { + OSD_TAB_t *ptr = p->data; + max7456_write(RIGHT_MENU_COLUMN - 5, i + top, (char *)ptr->names[*ptr->val]); + break; + } + case OME_VISIBLE: + if (p->data) { + uint32_t address = (uint32_t)p->data; + uint16_t *val; + + val = (uint16_t *)address; + + if (VISIBLE(*val)) + max7456_write(RIGHT_MENU_COLUMN, i + top, "YES"); + else + max7456_write(RIGHT_MENU_COLUMN, i + top, "NO "); + } + break; + case OME_UINT8: + if (p->data) { + OSD_UINT8_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + max7456_write(RIGHT_MENU_COLUMN, i + top, " "); + max7456_write(RIGHT_MENU_COLUMN, i + top, buff); + } + break; + case OME_INT8: + if (p->data) { + OSD_INT8_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + max7456_write(RIGHT_MENU_COLUMN, i + top, " "); + max7456_write(RIGHT_MENU_COLUMN, i + top, buff); + } + break; + case OME_UINT16: + if (p->data) { + OSD_UINT16_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + max7456_write(RIGHT_MENU_COLUMN, i + top, " "); + max7456_write(RIGHT_MENU_COLUMN, i + top, buff); + } + break; + case OME_INT16: + if (p->data) { + OSD_UINT16_t *ptr = p->data; + itoa(*ptr->val, buff, 10); + max7456_write(RIGHT_MENU_COLUMN, i + top, " "); + max7456_write(RIGHT_MENU_COLUMN, i + top, buff); + } + break; + case OME_FLOAT: + if (p->data) { + OSD_FLOAT_t *ptr = p->data; + simple_ftoa(*ptr->val * ptr->multipler, buff); + max7456_write(RIGHT_MENU_COLUMN - 1, i + top, " "); + max7456_write(RIGHT_MENU_COLUMN - 1, i + top, buff); + } + break; + case OME_OSD_Exit: + case OME_Label: + case OME_END: + case OME_Back: + break; + } + i++; + + if (i == MAX_MENU_ITEMS) // max per page + { + nextPage = currentMenu + i; + if (nextPage->type == OME_END) + nextPage = NULL; + break; + } + } +} + +void osdResetStats(void) +{ + stats.max_current = 0; + stats.max_speed = 0; + stats.min_voltage = 500; + stats.max_current = 0; + stats.min_rssi = 99; +} + +void osdUpdateStats(void) +{ + int16_t value; + + value = GPS_speed * 36 / 1000; + if (stats.max_speed < value) + stats.max_speed = value; + + if (stats.min_voltage > vbat) + stats.min_voltage = vbat; + + value = amperage / 100; + if (stats.max_current < value) + stats.max_current = value; + + if (stats.min_rssi > statRssi) + stats.min_rssi = statRssi; +} + +void osdShowStats(void) +{ + uint8_t top = 2; + char buff[10]; + + max7456_clear_screen(); + max7456_write(2, top++, " --- STATS ---"); + + if (STATE(GPS_FIX)) { + max7456_write(2, top, "MAX SPEED :"); + itoa(stats.max_speed, buff, 10); + max7456_write(22, top++, buff); + } + + max7456_write(2, top, "MIN BATTERY :"); + sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10); + max7456_write(22, top++, buff); + + max7456_write(2, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + max7456_write(22, top++, buff); + + if (feature(FEATURE_CURRENT_METER)) { + max7456_write(2, top, "MAX CURRENT :"); + itoa(stats.max_current / 10, buff, 10); + strcat(buff, "A"); + max7456_write(22, top++, buff); + + max7456_write(2, top, "USED MAH :"); + itoa(mAhDrawn, buff, 10); + strcat(buff, "\x07"); + max7456_write(22, top++, buff); + } + refreshTimeout = 60 * REFRESH_1S; +} + +// called when motors armed +void osdArmMotors(void) +{ + max7456_clear_screen(); + max7456_write(12, 7, "ARMED"); + refreshTimeout = REFRESH_1S / 2; + osdResetStats(); +} + +void updateOsd(uint32_t currentTime) +{ + static uint32_t counter; +#ifdef MAX7456_DMA_CHANNEL_TX + // don't touch buffers if DMA transaction is in progress + if (max7456_dma_in_progres()) + return; +#endif // MAX7456_DMA_CHANNEL_TX + + // redraw values in buffer + if (counter++ % 5 == 0) + osdUpdate(currentTime); + else // rest of time redraw screen 10 chars per idle to don't lock the main idle + max7456_draw_screen(); + + // do not allow ARM if we are in menu + if (inMenu) + DISABLE_ARMING_FLAG(OK_TO_ARM); +} + +void osdUpdate(uint32_t currentTime) +{ + static uint8_t rcDelay = BUTTON_TIME; + static uint8_t last_sec = 0; + uint8_t key = 0, sec; + + // detect enter to menu + if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) + osdOpenMenu(); + + // detect arm/disarm + if (armState != ARMING_FLAG(ARMED)) { + if (ARMING_FLAG(ARMED)) + osdArmMotors(); // reset statistic etc + else + osdShowStats(); // schow statistic + + armState = ARMING_FLAG(ARMED); + } + + osdUpdateStats(); + + sec = currentTime / 1000000; + + if (ARMING_FLAG(ARMED) && sec != last_sec) { + flyTime++; + last_sec = sec; + } + + if (refreshTimeout) { + if (IS_HI(THROTTLE) || IS_HI(PITCH)) // hide statistics + refreshTimeout = 1; + refreshTimeout--; + if (!refreshTimeout) + max7456_clear_screen(); + return; + } + + blinkState = (millis() / 200) % 2; + + if (inMenu) { + if (rcDelay) { + rcDelay--; + } + else if (IS_HI(PITCH)) { + key = KEY_UP; + rcDelay = BUTTON_TIME; + } + else if (IS_LO(PITCH)) { + key = KEY_DOWN; + rcDelay = BUTTON_TIME; + } + else if (IS_LO(ROLL)) { + key = KEY_LEFT; + rcDelay = BUTTON_TIME; + } + else if (IS_HI(ROLL)) { + key = KEY_RIGHT; + rcDelay = BUTTON_TIME; + } + else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != menuRc) // this menu is used to check transmitter signals so can exit using YAW + { + key = KEY_ESC; + rcDelay = BUTTON_TIME; + } + + if (key && !currentElement) { + rcDelay = osdHandleKey(key); + return; + } + if (currentElement) // edit position of element + { + if (key) { + if (key == KEY_ESC) { + // exit + osdMenuBack(); + rcDelay = BUTTON_PAUSE; + *currentElement &= ~BLINK_FLAG; + currentElement = NULL; + return; + } + else { + uint8_t x, y; + x = OSD_X(*currentElement); + y = OSD_Y(*currentElement); + switch (key) { + case KEY_UP: + y--; + break; + case KEY_DOWN: + y++; + break; + case KEY_RIGHT: + x++; + break; + case KEY_LEFT: + x--; + break; + } + + *currentElement &= 0xFC00; + *currentElement |= OSD_POS(x, y); + max7456_clear_screen(); + } + } + osdDrawElements(); + } + else + osdDrawMenu(); + } + else { + osdUpdateAlarms(); + osdDrawElements(); + } +} + +void osdChangeScreen(void *ptr) +{ + uint8_t i; + if (ptr) { + max7456_clear_screen(); + // hack - save profile to temp + if (ptr == &menuPid[0]) { + for (i = 0; i < 3; i++) { + tempPid[i][0] = curr_profile.pidProfile.P8[i]; + tempPid[i][1] = curr_profile.pidProfile.I8[i]; + tempPid[i][2] = curr_profile.pidProfile.D8[i]; + } + tempPid[3][0] = curr_profile.pidProfile.P8[PIDLEVEL]; + tempPid[3][1] = curr_profile.pidProfile.I8[PIDLEVEL]; + tempPid[3][2] = curr_profile.pidProfile.D8[PIDLEVEL]; + } + + if (ptr == &menuRateExpo[0]) + memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); + + menuStack[menuStackIdx] = currentMenu; + menuStackHistory[menuStackIdx] = currentMenuPos; + menuStackIdx++; + + currentMenu = (OSD_Entry *)ptr; + currentMenuPos = 0; + osdUpdateMaxRows(); + } +} + +#ifdef USE_FLASHFS +void osdEraseFlash(void *ptr) +{ + UNUSED(ptr); + + max7456_clear_screen(); + max7456_write(5, 3, "ERASING FLASH..."); + max7456_refresh_all(); + + flashfsEraseCompletely(); + while (!flashfsIsReady()) { + delay(100); + } + max7456_clear_screen(); + max7456_refresh_all(); +} +#endif // USE_FLASHFS + +void osdEditElement(void *ptr) +{ + uint32_t address = (uint32_t)ptr; + + // zsave position on menu stack + menuStack[menuStackIdx] = currentMenu; + menuStackHistory[menuStackIdx] = currentMenuPos; + menuStackIdx++; + + currentElement = (uint16_t *)address; + + *currentElement |= BLINK_FLAG; + max7456_clear_screen(); +} + +void osdExitMenu(void *ptr) +{ + max7456_clear_screen(); + max7456_write(5, 3, "RESTARTING IMU..."); + max7456_refresh_all(); + stopMotors(); + stopPwmAllMotors(); + delay(200); + + if (ptr) { + // save local variables to configuration + if (featureBlackbox) + featureSet(FEATURE_BLACKBOX); + + if (featureLedstrip) + featureSet(FEATURE_LED_STRIP); + +#if defined(VTX) || defined(USE_RTC6705) + if (featureVtx) + featureSet(FEATURE_VTX); +#endif // VTX || USE_RTC6705 + +#ifdef VTX + masterConfig.vtxBand = vtxBand; + masterConfig.vtx_channel = vtxChannel - 1; +#endif // VTX + +#ifdef USE_RTC6705 + masterConfig.vtx_channel = vtxBand * 8 + vtxChannel - 1; +#endif // USE_RTC6705 + + saveConfigAndNotify(); + } + + systemReset(); +} + +void osdOpenMenu(void) +{ + if (inMenu) + return; + + if (feature(FEATURE_LED_STRIP)) + featureLedstrip = 1; + + if (feature(FEATURE_BLACKBOX)) + featureBlackbox = 1; + +#if defined(VTX) || defined(USE_RTC6705) + if (feature(FEATURE_VTX)) + featureVtx = 1; +#endif // VTX || USE_RTC6705 + +#ifdef VTX + vtxBand = masterConfig.vtxBand; + vtxChannel = masterConfig.vtx_channel + 1; +#endif // VTX + +#ifdef USE_RTC6705 + vtxBand = masterConfig.vtx_channel / 8; + vtxChannel = masterConfig.vtx_channel % 8 + 1; +#endif // USE_RTC6705 + + osdRows = max7456_get_rows_count(); + inMenu = true; + refreshTimeout = 0; + max7456_clear_screen(); + currentMenu = &menuMain[0]; + osdResetAlarms(); + osdChangeScreen(currentMenu); + +#ifdef LED_STRIP + getLedColor(); +#endif // LED_STRIP +} + +void osdDrawElementPositioningHelp(void) +{ + max7456_write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), "--- HELP --- "); + max7456_write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 1, "USE ROLL/PITCH"); + max7456_write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 2, "TO MOVE ELEM. "); + max7456_write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 3, " "); + max7456_write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 4, "YAW - EXIT "); +} + +void osdDrawElements(void) +{ + max7456_clear_screen(); + + if (currentElement) + osdDrawElementPositioningHelp(); + else if (sensors(SENSOR_ACC) || inMenu) + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + + osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE); + osdDrawSingleElement(OSD_RSSI_VALUE); + osdDrawSingleElement(OSD_FLYTIME); + osdDrawSingleElement(OSD_ONTIME); + osdDrawSingleElement(OSD_FLYMODE); + osdDrawSingleElement(OSD_THROTTLE_POS); + osdDrawSingleElement(OSD_VTX_CHANNEL); + osdDrawSingleElement(OSD_CURRENT_DRAW); + osdDrawSingleElement(OSD_MAH_DRAWN); + osdDrawSingleElement(OSD_CRAFT_NAME); + osdDrawSingleElement(OSD_ALTITUDE); + +#ifdef GPS + if (sensors(SENSOR_GPS) || inMenu) { + osdDrawSingleElement(OSD_GPS_SATS); + osdDrawSingleElement(OSD_GPS_SPEED); + } +#endif // GPS +} + +#define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees +#define AH_MAX_ROLL 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees +#define AH_SIDEBAR_WIDTH_POS 7 +#define AH_SIDEBAR_HEIGHT_POS 3 +extern uint16_t rssi; // FIXME dependency on mw.c + +void osdDrawSingleElement(uint8_t item) +{ + if (!VISIBLE(OSD_cfg.item_pos[item]) || BLINK(OSD_cfg.item_pos[item])) + return; + + uint8_t elemPosX = OSD_X(OSD_cfg.item_pos[item]); + uint8_t elemPosY = OSD_Y(OSD_cfg.item_pos[item]); + char buff[32]; + + switch(item) { + case OSD_RSSI_VALUE: + { + uint16_t osdRssi = rssi * 100 / 1024; // change range + if (osdRssi >= 100) + osdRssi = 99; + + buff[0] = SYM_RSSI; + sprintf(buff + 1, "%d", osdRssi); + break; + } + + case OSD_MAIN_BATT_VOLTAGE: + { + buff[0] = SYM_BATT_5; + sprintf(buff + 1, "%d.%1dV", vbat / 10, vbat % 10); + break; + } + + case OSD_CURRENT_DRAW: + { + buff[0] = SYM_AMP; + sprintf(buff + 1, "%d.%02d", abs(amperage) / 100, abs(amperage) % 100); + break; + } + + case OSD_MAH_DRAWN: + { + buff[0] = SYM_MAH; + sprintf(buff + 1, "%d", mAhDrawn); + break; + } + +#ifdef GPS + case OSD_GPS_SATS: + { + buff[0] = 0x1f; + sprintf(buff + 1, "%d", GPS_numSat); + break; + } + + case OSD_GPS_SPEED: + { + sprintf(buff, "%d", GPS_speed * 36 / 1000); + break; + } +#endif // GPS + + case OSD_ALTITUDE: + { + int32_t alt = BaroAlt; // Metre x 100 + char unitSym = 0xC; // m + + if (!VISIBLE(OSD_cfg.item_pos[OSD_ALTITUDE]) || BLINK(OSD_cfg.item_pos[OSD_ALTITUDE])) + return; + + if (masterConfig.osdProfile.units == OSD_UNIT_IMPERIAL) { + alt = (alt * 328) / 100; // Convert to feet x 100 + unitSym = 0xF; // ft + } + + sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), unitSym); + break; + } + + case OSD_ONTIME: + { + uint32_t seconds = micros() / 1000000; + buff[0] = SYM_ON_M; + sprintf(buff + 1, "%02d:%02d", seconds / 60, seconds % 60); + break; + } + + case OSD_FLYTIME: + { + buff[0] = SYM_FLY_M; + sprintf(buff + 1, "%02d:%02d", flyTime / 60, flyTime % 60); + break; + } + + case OSD_FLYMODE: + { + char *p = "ACRO"; + + if (isAirmodeActive()) + p = "AIR"; + + if (FLIGHT_MODE(FAILSAFE_MODE)) + p = "!FS"; + else if (FLIGHT_MODE(ANGLE_MODE)) + p = "STAB"; + else if (FLIGHT_MODE(HORIZON_MODE)) + p = "HOR"; + + max7456_write(elemPosX, elemPosY, p); + return; + } + + case OSD_CRAFT_NAME: + { + if (strlen(masterConfig.name) == 0) + strcpy(buff, "CRAFT_NAME"); + else { + for (uint8_t i = 0; i < MAX_NAME_LENGTH; i++) { + buff[i] = toupper((unsigned char)masterConfig.name[i]); + if (masterConfig.name[i] == 0) + break; + } + } + + break; + } + + case OSD_THROTTLE_POS: + { + buff[0] = SYM_THR; + buff[1] = SYM_THR1; + sprintf(buff + 2, "%d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); + break; + } + +#ifdef VTX + case OSD_VTX_CHANNEL: + { + sprintf(buff, "CH:%d", current_vtx_channel % CHANNELS_PER_BAND + 1); + break; + } +#endif // VTX + + case OSD_ARTIFICIAL_HORIZON: + { + uint8_t *screenBuffer = max7456_get_screen_buffer(); + uint16_t position = 194; + + int rollAngle = attitude.values.roll; + int pitchAngle = attitude.values.pitch; + + if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) + position += 30; + + + if (pitchAngle > AH_MAX_PITCH) + pitchAngle = AH_MAX_PITCH; + if (pitchAngle < -AH_MAX_PITCH) + pitchAngle = -AH_MAX_PITCH; + if (rollAngle > AH_MAX_ROLL) + rollAngle = AH_MAX_ROLL; + if (rollAngle < -AH_MAX_ROLL) + rollAngle = -AH_MAX_ROLL; + + for (uint8_t x = 0; x <= 8; x++) { + if (x == 4) + x = 5; + int y = (rollAngle * (4 - x)) / 64; + y -= pitchAngle / 8; + y += 41; + if (y >= 0 && y <= 81) { + uint16_t pos = position - 7 + LINE * (y / 9) + 3 - 4 * LINE + x; + screenBuffer[pos] = (SYM_AH_BAR9_0 + (y % 9)); + } + } + + screenBuffer[position - 1] = (SYM_AH_CENTER_LINE); + screenBuffer[position + 1] = (SYM_AH_CENTER_LINE_RIGHT); + screenBuffer[position] = (SYM_AH_CENTER); + + osdDrawSingleElement(OSD_HORIZON_SIDEBARS); + return; + } + + case OSD_HORIZON_SIDEBARS: + { + uint8_t *screenBuffer = max7456_get_screen_buffer(); + uint16_t position = 194; + + if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) + position += 30; + + // Draw AH sides + int8_t hudwidth = AH_SIDEBAR_WIDTH_POS; + int8_t hudheight = AH_SIDEBAR_HEIGHT_POS; + for (int8_t x = -hudheight; x <= hudheight; x++) { + screenBuffer[position - hudwidth + (x * LINE)] = (SYM_AH_DECORATION); + screenBuffer[position + hudwidth + (x * LINE)] = (SYM_AH_DECORATION); + } + + // AH level indicators + screenBuffer[position - hudwidth + 1] = (SYM_AH_LEFT); + screenBuffer[position + hudwidth - 1] = (SYM_AH_RIGHT); + + return; + } + + default: + return; + } + + max7456_write(elemPosX, elemPosY, buff); +} + +#endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h old mode 100644 new mode 100755 index 80d3951c4..a1b1caad9 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -15,44 +15,25 @@ * along with Cleanflight. If not, see . */ -#define MAX_MENU_ROWS 8 -#define MAX_MENU_COLS 3 - -typedef struct { - const char* title; - uint8_t x_pos; -} osd_col_t; - -typedef struct { - const char* title; - void (*update)(int value_change_direction, uint8_t col); - void (*print)(uint16_t pos, uint8_t col); -} osd_row_t; - -typedef struct { - const char* title; - uint8_t cols_number; - uint8_t rows_number; - osd_col_t cols[MAX_MENU_COLS]; - osd_row_t rows[MAX_MENU_ROWS]; -} osd_page_t; +#pragma once +#include typedef enum { - OSD_MAIN_BATT_VOLTAGE, OSD_RSSI_VALUE, - OSD_TIMER, - OSD_THROTTLE_POS, - OSD_CPU_LOAD, - OSD_VTX_CHANNEL, - OSD_VOLTAGE_WARNING, - OSD_ARMED, - OSD_DISARMED, + OSD_MAIN_BATT_VOLTAGE, OSD_ARTIFICIAL_HORIZON, OSD_HORIZON_SIDEBARS, + OSD_ONTIME, + OSD_FLYTIME, + OSD_FLYMODE, + OSD_CRAFT_NAME, + OSD_THROTTLE_POS, + OSD_VTX_CHANNEL, OSD_CURRENT_DRAW, OSD_MAH_DRAWN, - OSD_CRAFT_NAME, + OSD_GPS_SPEED, + OSD_GPS_SATS, OSD_ALTITUDE, OSD_MAX_ITEMS, // MUST BE LAST } osd_items_t; @@ -63,12 +44,25 @@ typedef enum { } osd_unit_t; typedef struct { - // AUTO / PAL / NTSC in VIDEO_TYPES enum + uint16_t item_pos[OSD_MAX_ITEMS]; + + // Alarms + uint8_t rssi_alarm; + uint16_t cap_alarm; + uint16_t time_alarm; + uint16_t alt_alarm; + uint8_t video_system; osd_unit_t units; - int16_t item_pos[OSD_MAX_ITEMS]; -} osd_profile; +} osd_profile_t; + +typedef struct { + int16_t max_speed; + int16_t min_voltage; // /10 + int16_t max_current; // /10 + int16_t min_rssi; +} statistic_t; void updateOsd(uint32_t currentTime); void osdInit(void); -void resetOsdConfig(osd_profile *osdProfile); +void resetOsdConfig(osd_profile_t *osdProfile); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c old mode 100644 new mode 100755 index b75c4b441..2c8c5a1a6 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -918,24 +918,29 @@ const clivalue_t valueTable[] = { { "vtx_channel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, { "vtx_power", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_power, .config.minmax = { 0, 1 } }, #endif + #ifdef OSD { "osd_video_system", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.video_system, .config.minmax = { 0, 2 } }, { "osd_units", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.osdProfile.units, .config.lookup = { TABLE_UNIT } }, - { "osd_main_voltage_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { -480, 480 } }, - { "osd_rssi_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], .config.minmax = { -480, 480 } }, - { "osd_timer_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_TIMER], .config.minmax = { -480, 480 } }, - { "osd_throttle_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], .config.minmax = { -480, 480 } }, - { "osd_cpu_load_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_CPU_LOAD], .config.minmax = { -480, 480 } }, - { "osd_vtx_channel_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL], .config.minmax = { -480, 480 } }, - { "osd_voltage_warning_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING], .config.minmax = { -480, 480 } }, - { "osd_armed_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_ARMED], .config.minmax = { -480, 480 } }, - { "osd_disarmed_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_DISARMED], .config.minmax = { -480, 480 } }, - { "osd_artificial_horizon", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { -1, 0 } }, - { "osd_horizon_sidebars", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], .config.minmax = { -1, 0 } }, - { "osd_current_draw_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], .config.minmax = { -480, 480 } }, - { "osd_mah_drawn_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], .config.minmax = { -480, 480 } }, - { "osd_craft_name_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], .config.minmax = { -480, 480 } }, - { "osd_altitude_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], .config.minmax = { -480, 480 } }, + { "osd_rssi_alarm", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.rssi_alarm, .config.minmax = { 0, 100 } }, + { "osd_cap_alarm", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.cap_alarm, .config.minmax = { 0, 20000 } }, + { "osd_time_alarm", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.time_alarm, .config.minmax = { 0, 60 } }, + { "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.alt_alarm, .config.minmax = { 0, 10000 } }, + + { "osd_main_voltage_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], .config.minmax = { 0, 65536 } }, + { "osd_rssi_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], .config.minmax = { 0, 65536 } }, + { "osd_flytimer_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], .config.minmax = { 0, 65536 } }, + { "osd_ontime_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_ONTIME], .config.minmax = { 0, 65536 } }, + { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], .config.minmax = { 0, 65536 } }, + { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], .config.minmax = { 0, 65536 } }, + { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL], .config.minmax = { 0, 65536 } }, + { "osd_artificial_horizon", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { 0, 65536 } }, + { "osd_current_draw_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], .config.minmax = { 0, 65536 } }, + { "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], .config.minmax = { 0, 65536 } }, + { "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], .config.minmax = { 0, 65536 } }, + { "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], .config.minmax = { 0, 65536 } }, + { "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], .config.minmax = { 0, 65536 } }, + { "osd_altitude", VAR_UINT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], .config.minmax = { 0, 65536 } }, #endif }; @@ -3463,7 +3468,7 @@ static void cliStatus(char *cmdline) #ifdef USE_SDCARD cliSdInfo(NULL); -#endif +#endif } #ifndef SKIP_TASK_STATISTICS diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c old mode 100644 new mode 100755 index 8da4500f1..bbd3dab65 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -555,7 +555,7 @@ void mspSerialInit(serialConfig_t *serialConfig) #endif activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; - + if (feature(FEATURE_3D)) { activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; } @@ -567,11 +567,11 @@ void mspSerialInit(serialConfig_t *serialConfig) if (feature(FEATURE_INFLIGHT_ACC_CAL)) { activeBoxIds[activeBoxIdCount++] = BOXCALIB; } - + if (feature(FEATURE_OSD)) { activeBoxIds[activeBoxIdCount++] = BOXOSD; } - + #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch) { activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; @@ -1238,11 +1238,16 @@ static bool processOutCommand(uint8_t cmdMSP) case MSP_OSD_CONFIG: #ifdef OSD - headSerialReply(3 + (OSD_MAX_ITEMS * 2)); + headSerialReply(10 + (OSD_MAX_ITEMS * 2)); serialize8(1); // OSD supported // send video system (AUTO/PAL/NTSC) serialize8(masterConfig.osdProfile.video_system); serialize8(masterConfig.osdProfile.units); + serialize8(masterConfig.osdProfile.rssi_alarm); + serialize16(masterConfig.osdProfile.cap_alarm); + serialize16(masterConfig.osdProfile.time_alarm); + serialize16(masterConfig.osdProfile.alt_alarm); + for (i = 0; i < OSD_MAX_ITEMS; i++) { serialize16(masterConfig.osdProfile.item_pos[i]); } @@ -1624,6 +1629,10 @@ static bool processInCommand(void) if ((int8_t)addr == -1) { masterConfig.osdProfile.video_system = read8(); masterConfig.osdProfile.units = read8(); + masterConfig.osdProfile.rssi_alarm = read8(); + masterConfig.osdProfile.cap_alarm = read16(); + masterConfig.osdProfile.time_alarm = read16(); + masterConfig.osdProfile.alt_alarm = read16(); } // set a position setting else { @@ -1926,7 +1935,7 @@ static bool processInCommand(void) masterConfig.baro_hardware = read8(); masterConfig.mag_hardware = read8(); break; - + case MSP_SET_NAME: memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); for (i = 0; i < MIN(MAX_NAME_LENGTH, currentPort->dataSize); i++) { diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 09d87472e..75e61d044 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -158,7 +158,7 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER #define VBAT_ADC_PIN PC1 -#define RSSI_ADC_GPIO_PIN PC2 +#define RSSI_ADC_PIN PC2 #define CURRENT_METER_ADC_PIN PC3 #define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 2550b27fb..86c9d1699 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -97,6 +97,8 @@ #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 #define MAX7456_SPI_CS_PIN PB1 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) //#define MAX7456_DMA_CHANNEL_TX DMA1_Channel3 //#define MAX7456_DMA_CHANNEL_RX DMA1_Channel2 //#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_CH3_HANDLER diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 376a683c7..a9f162baa 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -27,9 +27,10 @@ #endif #define LED0 PB5 -#define LED1 PB4 +//#define LED1 PB4 #define BEEPER PB4 +#define BEEPER_INVERTED #define INVERTER PC0 // PC0 used as inverter select GPIO #define INVERTER_USART USART1 @@ -39,11 +40,11 @@ #define ACC #define USE_ACC_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG #define GYRO #define USE_GYRO_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_MPU6000_ALIGN CW180_DEG // MPU6000 interrupts #define USE_EXTI @@ -113,7 +114,7 @@ #define USE_ADC #define CURRENT_METER_ADC_PIN PC1 #define VBAT_ADC_PIN PC2 -#define RSSI_ADC_GPIO_PIN PA0 +#define RSSI_ADC_PIN PA0 #define SENSORS_SET (SENSOR_ACC) @@ -130,4 +131,4 @@ #define TARGET_IO_PORTD 0xffff #define USABLE_TIMER_CHANNEL_COUNT 12 -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/RACEBASE/hardware_revision.c b/src/main/target/RACEBASE/hardware_revision.c new file mode 100755 index 000000000..e880f2bab --- /dev/null +++ b/src/main/target/RACEBASE/hardware_revision.c @@ -0,0 +1,69 @@ +/* + * 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 . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build_config.h" + +#include "drivers/system.h" +#include "drivers/bus_spi.h" +#include "drivers/sensor.h" +#include "drivers/io.h" +#include "drivers/gpio.h" +#include "drivers/exti.h" +#include "drivers/accgyro.h" +#include "drivers/accgyro_mpu.h" +#include "drivers/accgyro_mpu6500.h" + +#include "hardware_revision.h" + +uint8_t hardwareRevision = 1; + +void detectHardwareRevision(void) +{ + gpio_config_t gpio; + + // GYRO CS as output + gpio.pin = GPIO_Pin_5; + gpio.mode = Mode_Out_PP; + gpio.speed = Speed_2MHz; + gpioInit(GPIOB, &gpio); + GPIO_SetBits(GPIOB, GPIO_Pin_5); + + gpio.pin = GPIO_Pin_7; + gpio.mode = Mode_Out_PP; + gpio.speed = Speed_2MHz; + gpioInit(GPIOA, &gpio); + GPIO_SetBits(GPIOA, GPIO_Pin_7); + + gpio.pin = GPIO_Pin_12; + gpio.mode = Mode_Out_PP; + gpio.speed = Speed_2MHz; + gpioInit(GPIOB, &gpio); + GPIO_SetBits(GPIOB, GPIO_Pin_12); +} + + +void updateHardwareRevision(void) +{ + +} + diff --git a/src/main/target/RACEBASE/hardware_revision.h b/src/main/target/RACEBASE/hardware_revision.h new file mode 100755 index 000000000..a331babad --- /dev/null +++ b/src/main/target/RACEBASE/hardware_revision.h @@ -0,0 +1,24 @@ +/* + * 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 . + */ +#pragma once + +#include "drivers/exti.h" + +extern uint8_t hardwareRevision; + +void updateHardwareRevision(void); +void detectHardwareRevision(void); diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index 8e892151d..e2bf73f64 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -18,6 +18,7 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "RBFC" +#define USE_HARDWARE_REVISION_DETECTION #define TARGET_CONFIG #define LED0 PB3 @@ -76,6 +77,9 @@ #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI2 #define MAX7456_SPI_CS_PIN PA7 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 57ad57659..a0cd29d06 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -27,8 +27,12 @@ #endif #define LED0 PB5 -#define LED1 PB4 +// Disable LED1, conflicts with AirbotF4/Flip32F4 beeper +//#define LED1 PB4 + #define BEEPER PB4 +#define BEEPER_INVERTED + #define INVERTER PC0 // PC0 used as inverter select GPIO #define INVERTER_USART USART1 @@ -102,12 +106,23 @@ #define USE_ADC #define CURRENT_METER_ADC_PIN PC1 #define VBAT_ADC_PIN PC2 -#define RSSI_ADC_GPIO_PIN PA0 +//#define RSSI_ADC_PIN PA0 +#define LED_STRIP +// LED Strip can run off Pin 6 (PA0) of the MOTOR outputs. +#define WS2811_GPIO_AF GPIO_AF_TIM5 +#define WS2811_PIN PA0 +#define WS2811_TIMER TIM5 +#define WS2811_TIMER_CHANNEL TIM_Channel_2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream2 +#define WS2811_DMA_CHANNEL DMA_Channel_6 +#define WS2811_DMA_IRQ DMA1_Stream2_IRQn +#define WS2811_DMA_FLAG DMA_FLAG_TCIF2 +#define WS2811_DMA_IT DMA_IT_TCIF2 #define SENSORS_SET (SENSOR_ACC) - #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_BLACKBOX) diff --git a/src/main/target/REVO/target.mk b/src/main/target/REVO/target.mk index 7347dd10f..e3fc89c21 100644 --- a/src/main/target/REVO/target.mk +++ b/src/main/target/REVO/target.mk @@ -4,5 +4,6 @@ FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c - + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f4xx.c diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index def63803d..f3540b06a 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -152,7 +152,7 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER #define VBAT_ADC_PIN PC0 -#define RSSI_ADC_GPIO_PIN PB1 +#define RSSI_ADC_PIN PB1 #define CURRENT_METER_ADC_PIN PA5 #define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY)