diff --git a/src/main/config/config.c b/src/main/config/config.c index 137cdf3b07..6e803a920b 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -423,15 +423,17 @@ static void resetConf(void) #ifdef OSD featureSet(FEATURE_OSD); masterConfig.osdProfile.video_system = AUTO; - masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; - masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; - masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; - masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] = -9; - masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] = 26; - masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL] = 1; - masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] = -80; - masterConfig.osdProfile.item_pos[OSD_ARMED] = -107; - masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; + masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; + masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] = -9; + masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] = 26; + masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL] = 1; + masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] = -80; + masterConfig.osdProfile.item_pos[OSD_ARMED] = -107; + masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; + masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON] = 1; + masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] = -1; #endif #ifdef USE_RTC6705 diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 497fd62895..b03c1d616f 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -38,6 +38,11 @@ static IO_t max7456CsPin = IO_NONE; +/** 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 max_screen_size; uint8_t video_signal_type = 0; @@ -127,14 +132,49 @@ void max7456_write_string(const char *string, int16_t address) { char *dest; if (address >= 0) - dest = screen + address; + dest = max7456_screen + address; else - dest = screen + (max_screen_size + address); + dest = max7456_screen + (max_screen_size + address); - while(*string && dest < (screen + max_screen_size)) + while(*string && dest < (max7456_screen + max_screen_size)) *dest++ = *string++; } + +// Write the artifical horizon to the screen buffer +void max7456_artificial_horizon(int rollAngle, int pitchAngle, uint8_t show_sidebars) { + uint16_t position = 194; + + 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; + max7456_screen[pos] = SYM_AH_BAR9_0+(Y%9); + } + } + + if (show_sidebars) { + // Draw AH sides + int8_t hudwidth = AHISIDEBARWIDTHPOSITION; + int8_t hudheight = AHISIDEBARHEIGHTPOSITION; + for(int8_t X=-hudheight; X<=hudheight; X++) { + max7456_screen[position-hudwidth+(X*LINE)] = SYM_AH_DECORATION; + max7456_screen[position+hudwidth+(X*LINE)] = SYM_AH_DECORATION; + } + // AH level indicators + max7456_screen[position-hudwidth+1] = SYM_AH_LEFT; + max7456_screen[position+hudwidth-1] = SYM_AH_RIGHT; + } +} + void max7456_draw_screen(void) { uint16_t xx; if (!max7456_lock) { diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index d14d0c0dd4..cd33eae5e9 100644 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -149,5 +149,6 @@ char max7456_screen[VIDEO_BUFFER_CHARS_PAL]; void max7456_init(uint8_t system); void max7456_draw_screen(void); void max7456_draw_screen_fast(void); +void max7456_artificial_horizon(int rollAngle, int pitchAngle, uint8_t show_sidebars); void max7456_write_string(const char *string, int16_t address); void max7456_write_nvm(uint8_t char_address, uint8_t *font_data); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2236fabe6f..8963aa4ba6 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -699,6 +699,9 @@ void updateOsd(void) 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) { + max7456_artificial_horizon(attitude.values.roll, attitude.values.pitch, masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] != -1); + } } } else { max7456_draw_screen_fast(); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index a1f71e5866..486ff655a9 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -48,6 +48,8 @@ typedef enum { OSD_VOLTAGE_WARNING, OSD_ARMED, OSD_DISARMED, + OSD_ARTIFICIAL_HORIZON, + OSD_HORIZON_SIDEBARS, OSD_MAX_ITEMS, // MUST BE LAST } osd_items_t;