/* * Copyright (C) 2023 Daniele Lacamera * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU Lesser General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program 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 Lesser General Public License for more details. * * You should have received a copy of the GNU Lesser General Public License * along with this program. If not, see . */ #include #include #include "ui.h" #include "button.h" #include #include #include #include "ksp-serial.h" #include "led.h" #include "adc.h" #include #include "arm_math.h" #include "ui.h" #include "trig.h" #define ROLL_CALIB (30) #define PITCH_CALIB (0) #define M_PI_INT 3 const char main_deck_name[] = "Main Deck"; static void main_deck_draw(void); static void main_deck_run(uint32_t ev, void *arg); static char _soi_target[20]; static int vdata_valid = 0; static int show_pitch_roll_yaw = 0; static char *soi_target(uint8_t SOI) { switch (SOI) { case 100: strcpy(_soi_target, "Kerbol"); break; case 110: strcpy(_soi_target, "Moho"); break; case 120: strcpy(_soi_target, "Eve"); break; case 121: strcpy(_soi_target, "Gilly"); break; case 130: strcpy(_soi_target, "Kerbin"); break; case 131: strcpy(_soi_target, "Mun"); break; case 132: strcpy(_soi_target, "Minmus"); break; case 140: strcpy(_soi_target, "Duna"); break; case 141: strcpy(_soi_target, "Ike"); break; case 150: strcpy(_soi_target, "Dres"); break; case 160: strcpy(_soi_target, "Jool"); break; case 161: strcpy(_soi_target, "Laythe"); break; case 162: strcpy(_soi_target, "Vall"); break; case 163: strcpy(_soi_target, "Tylo"); break; case 164: strcpy(_soi_target, "Bop"); break; case 165: strcpy(_soi_target, "Pol"); break; case 170: strcpy(_soi_target, "Eloo"); break; default: strcpy(_soi_target, "Unknown"); } return _soi_target; } static void main_deck_init(void) { } struct screen main_deck_screen = { .draw = main_deck_draw, .name = main_deck_name }; struct task main_deck_task = { .init = main_deck_init, .run = main_deck_run, .events = EV_BUTTON | EV_HEARTBEAT | EV_VESSELDATA, .screen = &main_deck_screen, .name = main_deck_name }; static char * _float_to_str(float x, char *p) { char *s = p + 20; // go to end of buffer uint16_t decimals; // variable to store the decimals int units; // variable to store the units (part to left of decimal place) memset(p, ' ', 20); if (x < 0.0) { // take care of negative numbers decimals = (int)(x * -100) % 100; // make 1000 for 3 decimals etc. units = (int)(-1 * x); } else { // positive numbers decimals = (int)(x * 100) % 100; units = (int)x; } *--s = (decimals % 10) + '0'; decimals /= 10; // repeat for as many decimal places as you need *--s = (decimals % 10) + '0'; *--s = '.'; while (units > 0) { *--s = (units % 10) + '0'; units /= 10; } if (x < 0) *--s = '-'; // unary minus sign for negative numbers return s; } float approx_sqrt(float x) { float guess = x / 2.0; float prev_guess; float error = 1e-3; // set desired level of accuracy here do { prev_guess = guess; guess = (guess + x / guess) / 2.0; } while (fabs(guess - prev_guess) > error); return guess; } #ifndef PI #define PI 3.14159265 #endif void draw_circle(int x, int y, int radius, uint8_t color) { int i, j; uint8_t *screen = ui_get_screen(0); int x0 = x - 10; for (i = x - radius; i <= x + radius; i++) { for (j = y - radius; j <= y + radius; j++) { if ((i - x) * (i - x) + (j - y) * (j - y) <= radius * radius) { ui_draw_h_segment(0, color, i, j, 1); } } } } void ui_draw_circle_slice(uint32_t x, uint32_t y, uint32_t r, int alpha, int beta, uint8_t color) { int i, j; uint32_t start, end; int cx = x; int cy = y; alpha += 90; beta += 90; if (alpha >= 360) alpha-=360; if (beta >= 360) beta -= 360; if (alpha < 0) alpha += 360; if (beta < 0) beta += 360; ui_draw_h_segment(0, color, x-1, y, 3); if (alpha < beta) { for (j = 0; j < r; j++) { for (i = alpha; i < beta; i++) { int px = cx + (int)(j * pcos(i)); int py = cy + (int)(j * psin(i)); if (px != cx || py != cy) { ui_draw_h_segment(0, color, px, py, 1); } } } } else { for (j = 0; j < r; j++) { for (i = alpha; i < 360; i++) { int px = cx + (int)(j * pcos(i)); int py = cy + (int)(j * psin(i)); if (px != cx || py != cy) { ui_draw_h_segment(0, color, px, py, 1); } } for (i = 0; i < beta; i++) { int px = cx + (int)(j * pcos(i)); int py = cy + (int)(j * psin(i)); if (px != cx || py != cy) { ui_draw_h_segment(0, color, px, py, 1); } } } } } #define ptan(a) psin(a)/pcos(a) void ui_draw_circle_sector(int x, int y, int r, int px, int py, int rho, int include_center) { int rho_norm = (0 - rho) + 90; int i, j; float m,q; uint8_t upc, downc; while (rho_norm < 0) rho_norm+=360; while (rho_norm >= 360) rho_norm -= 360; if ((rho == 0) || (rho == 180)) { upc = BROWN; downc = CYAN; if (rho == 180){ upc = CYAN; downc = BROWN; } for (i = x - r; i <= x + r; i++) { for (j = y - r; j <= y + r; j++) { if ((i - x) * (i - x) + (j - y) * (j - y) <= r * r) { if (i > px) ui_draw_h_segment(0, upc, i, j, 1); else ui_draw_h_segment(0, downc, i, j, 1); } } } return; } if (rho > 0) { upc = CYAN; downc = BROWN; }else { upc = BROWN; downc = CYAN; } m = ptan(rho_norm); q = py - (m * px); for (i = x - r; i <= x + r; i++) { for (j = y - r; j <= y + r; j++) { if ((i - x) * (i - x) + (j - y) * (j - y) <= r * r) { if (j > (m * i + q)) ui_draw_h_segment(0, upc, i, j, 1); else ui_draw_h_segment(0, downc, i, j, 1); } } } } void draw_navball(int x, int y, int pitch, int roll) { static int last_p_navball = -400; static int last_r_navball = - 400; if ((last_p_navball == pitch) && (last_r_navball == roll)) return; ui_fill_area(0, BLACK, 150, 120, 320, 200); last_p_navball = pitch; last_r_navball = roll; if (pitch > 45 && pitch < 135) { draw_circle(x,y, 40, CYAN); } else if (pitch < -45 && pitch > -135) { draw_circle(x,y, 40, BROWN); } else if (pitch == 0) { int a = 0 - roll; int b = a + 180; ui_draw_circle_slice(x,y, 40, a, b, CYAN); ui_draw_circle_slice(x,y, 40, b, a, BROWN); } else { uint32_t px, py; px = x + 40 * psin(pitch) * pcos(roll); py = y - 40 * psin(pitch) * psin(roll); ui_draw_circle_sector(x, y, 40, px, py, roll, 1); } ui_draw_h_segment(0, ORANGE, x, y - 6, 5); ui_draw_h_segment(0, ORANGE, x, y + 1, 5); ui_draw_h_segment(0, ORANGE, x + 1, y - 2, 3); ui_draw_h_segment(0, ORANGE, x + 1, y + 1, 3); ui_draw_h_segment(0, ORANGE, x + 2, y, 1); } static void fuel_display(unsigned x, unsigned y, unsigned fuel, unsigned fuel_tot, char *label, int col) { unsigned tx = x; unsigned ty = y; unsigned i; unsigned pct; if (!vdata_valid) return; pct = fuel * 100 / fuel_tot; ui_fill_area(0, BLACK, tx - 10, ty + 100, tx + 8, ty + 110); ui_fill_area(0, BLACK, tx, ty, tx + 8, ty + 100); ui_text_at(0, col, tx - 10, ty + 100, label); for (i = 0; i < 100; i++) { if (pct < 10) col = RED; if (pct > i) { ui_fill_area(0, col, tx, ty + 100 - i, tx + 8, ty + 101 - i); } } } void avionics_display(controlPacket_t *cp) { uint32_t throttle, potr; int32_t pitch, yaw, roll; int i; int tx = 322; int ty = 150; /* Throttle display */ pot_read(NULL, &potr); throttle = ((potr * 100) / 4096); ui_fill_area(0, BLACK, 245, 250, 332, 270); ui_fill_area(0, BLACK, tx, ty, tx + 10, ty + 100); ui_text_at(0, YELLOW, 245, 250, "Throttle:"); ui_text_at(0, YELLOW, 265, 260, ui_printn((potr * 100) / 4096)); for (i = 0; i < 100; i++) { int col = YELLOW; if (i > 90) col = RED; if (throttle > i) { ui_fill_area(0, col, tx, ty + 100 - i, tx + 10, ty + 101 - i); } } cp->Throttle = (potr * 1000) / 4096; if (show_pitch_roll_yaw) { unsigned px, py, rx,ry, yx, yy; int col = GREEN; int labelcol = GREEN; char label[15]; px = 55; py = 70; rx = 2; ry = 75; yx = 2; yy = 110; ui_fill_area(0, 238, 2, 52, 103, 138); /* Pitch display */ pitch = 0 - (cp->Pitch / 50); snprintf(label, 15, "P %d", pitch); ui_text_at(0, labelcol, px + 10, py + 20, label); ui_fill_area(0, 235, px, py, px + 10, py + 40); for (i = -20; i < 0; i++) { if (pitch < i) { ui_fill_area(0, col, px, py + 20 + i, px + 10, py + 21 + i); } } for (i = 0; i < 20; i++) { if (pitch > i) { ui_fill_area(0, col, px, py + 20 + i, px + 10, py + 21 + i); } } ui_fill_area(0, RED, px - 2, py + 20, px + 12, py + 21); /* Roll display */ roll = (cp->Roll / 50); snprintf(label, 15, "R %d", roll); ui_text_at(0, labelcol, rx, ry - 10, label); ui_fill_area(0, 235, rx, ry, rx + 40, ry + 10); for (i = -20; i < 0; i++) { if (roll < i) { ui_fill_area(0, col, rx + 20 + i, ry, rx + 21 + i, ry + 10); } } for (i = 0; i < 20; i++) { if (roll > i) { ui_fill_area(0, col, rx + 20 + i, ry, rx + 21 + i, ry + 10); } } ui_fill_area(0, RED, rx + 20, ry - 2, rx + 21, ry + 12); /* Yaw display */ yaw = (cp->Yaw / 50); snprintf(label, 15, "Y %d", yaw); ui_text_at(0, labelcol, yx, yy - 10, label); ui_fill_area(0, 235, yx, yy, yx + 40, yy + 10); for (i = -20; i < 0; i++) { if (yaw < i) { ui_fill_area(0, col, yx + 20 + i, yy, yx + 21 + i, yy + 10); } } for (i = 0; i < 20; i++) { if (yaw > i) { ui_fill_area(0, col, yx + 20 + i, yy, yx + 21 + i, yy + 10); } } ui_fill_area(0, RED, yx + 20, yy - 2, yx + 21, yy + 12); } else ui_fill_area(0, 235, 1, 51, 104, 138); } static void navi_display(void) { char Status[100] = ""; char Points[100] = ""; int ralt = (int)cur_vdata->RAlt; int i = 0; char hdg_bar[30] = ""; int hdg_dec, hdg_ref; unsigned x_alt, y_alt; int vvi; uint8_t pointcolor; if (((int)cur_vdata->VVI == 0) && ((int)cur_vdata->RAlt < 10)) { strcpy(Status, "Landed at "); } else if ((int)cur_vdata->e != 0) { strcpy(Status, "Escaping from "); } else if ((int) cur_vdata->PE < 0) { strcpy(Status, "In flight over "); } else { strcpy(Status, "Orbiting "); } strcat(Status, soi_target(cur_vdata->SOINumber)); if ((int)(cur_vdata->PE < 0)){ snprintf(Points, 100, "Apo: %d, Peri: Negative", (int)(cur_vdata->AP)); pointcolor = PURPLE; } else { snprintf(Points, 100, "Apo: %d, Peri: %d", (int)(cur_vdata->AP), (int)(cur_vdata->PE)); pointcolor = CYAN; } ui_fill_area(0, BLACK, 140, 20, 380, 60); ui_text_at(0, WHITE, 150, 20, Status); ui_text_at(0, pointcolor, 150, 30, Points); /* Navball */ draw_navball(160, 233, (int)cur_vdata->Pitch, (int)cur_vdata->Roll); /* Altimeter */ x_alt = 105; y_alt = 99; ui_fill_area(0, DARKGREY, x_alt, y_alt, x_alt + 45, y_alt + 101); if (ralt < 100000) { ui_fill_area(0, BLACK, x_alt+45, y_alt, x_alt + 55, y_alt + 101); for (i = 0; i < 100; i+=10) ui_fill_area(0, DARKGREY, x_alt + 46, (y_alt + 99 - i), x_alt + 54, (y_alt + 101) - i); i = ralt / 1000; ui_fill_area(0, GREEN, x_alt + 45, (y_alt + 101) - i, x_alt + 55, (y_alt + 101)); ui_fill_area(0, BLACK, x_alt, (y_alt + 90) - i, x_alt + 50, (y_alt + 100) - i); ui_text_at(0, WHITE, x_alt, (y_alt + 90) - i, ui_printn(ralt)); ui_text_at(0, WHITE, x_alt + 40, (y_alt + 90) - i, "m"); ui_fill_area(0, BRIGHT(GREEN), x_alt + 20, (y_alt + 101) - i, x_alt + 55, (y_alt + 102) - i); } ui_fill_area(0, WHITE, x_alt + 55, y_alt + 60, x_alt + 70, y_alt + 61); vvi = (int)(cur_vdata->VVI); if (vvi > 100) vvi = 100; if (vvi < -100) vvi = -100; if (vvi > 0) { ui_fill_area(0, BRIGHT(GREEN), x_alt+56, y_alt + 60 - (vvi / 4), x_alt +60, y_alt + 61); } else { ui_fill_area(0, BRIGHT(RED), x_alt+56, y_alt + 60, x_alt +60, (y_alt + 60) - (vvi / 4)); } /* Flight Data */ char pitch_s[6], roll_s[6], hdg_s[4] = "000"; ui_fill_area(0, BLACK, 155, 100, 320, 120); ui_fill_area(0, BLACK, 222, 140, 246, 148); snprintf(hdg_s, 4, "%03d", (int)(cur_vdata->Heading)); ui_text_at(0, WHITE, 222, 140, hdg_s); ui_text_at(0, GREEN, 205, 110, "SPD"); ui_text_at(0, WHITE, 235, 110, ui_printn((int)(cur_vdata->IAS))); /* Heading bar */ ui_fill_area(0, BLACK, 150, 200, 320, 220); hdg_ref = ((int)cur_vdata->Heading) - 10; if (hdg_ref < 0) hdg_ref += 360; for (i = 0; i < 20; i++) { if (((i + hdg_ref) % 10) == 9) { int hdg_pr = (hdg_ref + 1 + i) / 10 * 10; if (hdg_pr >= 360) hdg_pr -= 360; snprintf(hdg_bar + i, 4,"%03d", hdg_pr); i += 2; continue; } else if (((i + hdg_ref) % 5) == 0) { hdg_bar[i] = '|'; } else { hdg_bar[i] = '.'; } } hdg_bar[21] = '\0'; ui_text_at(0, WHITE, 150, 200, " |"); ui_text_at(0, WHITE, 150, 210, hdg_bar); } static int button = -1; #define BUTTON_THRESHOLD 2 static void main_deck_run(uint32_t event, void *arg) { int32_t x, y; uint32_t torque; struct sample *s; int ts; int throttle = 0; int pitch = 0, roll = 0, yaw = 0; int main_ctrl = 0; static int sas = 0; static int rcs = 0; static int brk = 0; static int gear = 0; static int light = 0; static uint8_t navmode = 0; int i; char btn_str[6]; int staging = 0; controlPacket_t cp = {}; if (screen_get_focus() != &main_deck_screen) return; if (ui_menu_is_on()) { if (event & EV_BUTTON) ui_redraw(EV_BUTTON); return; } ts = input_detect_touch(); if (ts == TS_TOUCH_NONE) { } else { event |= EV_BUTTON; button = 12; } if (event & EV_VESSELDATA) { static int ev_data_ctr = 0; if ((++ev_data_ctr == 10) || (vdata_valid == 0)) { brk = (cur_vdata->ActionGroups & (1 << 4)) >> 4; gear = (cur_vdata->ActionGroups & (1 << 3)) >> 3; light = (cur_vdata->ActionGroups & (1 << 2)) >> 2; rcs = (cur_vdata->ActionGroups & (1 << 6)) >> 6; sas = (cur_vdata->ActionGroups & (1 << 7)) >> 7; navmode = cur_vdata->NavballSASMode; ev_data_ctr = 0; } vdata_valid = 1; navi_display(); clear_event(EV_VESSELDATA); } if (event & EV_BUTTON) { if (button < 0) button = ui_process_button_pressed(); switch(button) { case 12: /* TS */ show_pitch_roll_yaw = !!!show_pitch_roll_yaw; break; case BUTTON_DPADU: cp.Pitch = -1000; break; case BUTTON_DPADD: cp.Pitch = 1000; break; case BUTTON_DPADL: cp.Yaw = -1000; break; case BUTTON_DPADR: cp.Yaw = 1000; break; case 7: ui_menu(1); ui_redraw(EV_NONE); break; case 10: if (sas) navmode++; if ((navmode & 0xF) > 10) { navmode &= 0xF0; navmode |= 0x01; } break; case 8: if (vdata_valid) gear = !(((cur_vdata->ActionGroups & (1 << 3)) >> 3)); break; case 9: if (vdata_valid) light = !light; break; case 5: if (vdata_valid) brk = !(((cur_vdata->ActionGroups & (1 << 4)) >> 4)); break; case 4: staging++; break; } button = -1; clear_event(EV_BUTTON); } /* Sample joystick */ joy_read(&x, &y); pot_read(&torque, NULL); x-=2048 + ROLL_CALIB; y-=2048 + PITCH_CALIB; x*=2000 * torque / 4096; y*=2000 * torque / 4096; x/=4096; y/=4096; if ((x > 20) || (x < -20)) cp.Roll = x; else cp.Roll = 0; if ((y > 20) || (y < -20)) cp.Pitch = y; else cp.Pitch = 0; avionics_display(&cp); if (vdata_valid) { fuel_display(370, 100, (int)(cur_vdata->LiquidFuel), (int)(cur_vdata->LiquidFuelTot), "LF", YELLOW ); fuel_display(388, 100, (int)(cur_vdata->Oxidizer), (int)(cur_vdata->OxidizerTot), "Ox", CYAN); fuel_display(406, 100, (int)(cur_vdata->SolidFuel), (int)(cur_vdata->SolidFuelTot), "SF", GREY); fuel_display(424, 100, (int)(cur_vdata->ECharge), (int)(cur_vdata->EChargeTot), "el" , BRIGHT(GREEN)); fuel_display(442, 100, (int)(cur_vdata->XenonGas), (int)(cur_vdata->XenonGasTot), "Xe" , PINK); if ((cur_vdata->ActionGroups & (1 << 1)) == (1 << 1)) fuel_display(32, 150, (int)(cur_vdata->MonoProp), (int)(cur_vdata->MonoPropTot), "MP", BRIGHT(YELLOW)); else ui_fill_area(0, DARKGREY, 20, 150, 50, 280); if ((cur_vdata->Density > 0)) { int dens = (int)(cur_vdata->Density * 100); if (dens > 100) dens = 100; fuel_display (50, 150, dens, 100, "At", 87); fuel_display (68, 150, (int)(cur_vdata->IntakeAir),(int)(cur_vdata->IntakeAirTot), "Ai", CYAN); } } /* Lamps */ ui_fill_area(0, 242, 35, 0, 64, 27); ui_fill_area(0, light?58:233, 38, 0, 64, 24); ui_text_at (0, (light?BLACK:238), 38, 8, "LIG"); ui_fill_area(0, 242, 65, 0, 94, 27); ui_fill_area(0, gear?28:233, 68, 0, 94, 24); ui_text_at (0, (gear?BLACK:238), 68, 8, "GEA"); ui_fill_area(0, 242, 95, 0, 124, 27); ui_fill_area(0, brk?BLOOD:233, 98, 0, 124, 24); ui_text_at (0, (brk?WHITE:238), 98, 8, "(!)"); /* Switches */ sas = input_get_swr()?1:0; rcs = input_get_swl()?1:0; if (sas == 0) { navmode &= 0xF0; } ui_fill_area(0, 242, 447, 0, 474, 28); ui_fill_area(0, sas?122:233, 450, 0, 474, 24); ui_text_at (0, (sas?BLUE:238), 450, 8, "SAS"); ui_fill_area(0, 242, 3, 0, 30, 28); ui_fill_area(0, rcs?GREEN:235, 6, 0, 30, 24); ui_text_at (0, rcs?BRIGHT(GREEN):238, 6, 8, "RCS"); cp.id = 101; cp.NavBallSASMode = navmode; if (!rcs) staging = 0; if (vdata_valid) { cp.MainControls = ((sas << 7) | (rcs << 6) | (light << 5) | (gear << 4) | (brk << 3) | (staging << 0)); ksp_serial_send(&cp, sizeof(cp)); } if (sas) uled_on(4); else uled_off(4); if (rcs) uled_on(5); else uled_off(5); if(light) uled_on(0); else uled_off(0); if (gear) uled_on(1); else uled_off(1); if (brk) uled_on(2); else uled_off(2); } static void main_deck_draw(void) { int i; char btn_str[4] = ""; /* background to foreground */ ui_fill_area(0, DARKGREY, 0, 0, xres, yres); ui_fill_area(0, WHITE, 0, 50, 105, 140); /* Pitch panel */ /* Logo & appname */ //image_at(0, logo, 5, 4, logox, logoy); } void main_deck_setup(void) { register_task(&main_deck_task); register_screen(&main_deck_task, &main_deck_screen); }