diff --git a/src/core/profile.c b/src/core/profile.c index 9c5c978f7..06abecca8 100644 --- a/src/core/profile.c +++ b/src/core/profile.c @@ -487,7 +487,7 @@ const profile_t default_profile = { ENCODE_OSD_ELEMENT(1, 0, 10, 13), // OSD_FLIGHT_MODE ENCODE_OSD_ELEMENT(1, 0, 24, 1), // OSD_RSSI ENCODE_OSD_ELEMENT(1, 0, 24, 13), // OSD_STOPWATCH - ENCODE_OSD_ELEMENT(1, 0, 8, 6), // OSD_SYSTEM_STATUS + ENCODE_OSD_ELEMENT(1, 0, 4, 6), // OSD_SYSTEM_STATUS ENCODE_OSD_ELEMENT(1, 0, 1, 13), // OSD_THROTTLE ENCODE_OSD_ELEMENT(0, 0, 1, 1), // OSD_VTX_CHANNEL ENCODE_OSD_ELEMENT(1, 0, 1, 14), // OSD_CURRENT_DRAW @@ -501,7 +501,7 @@ const profile_t default_profile = { ENCODE_OSD_ELEMENT(1, 0, 20, 16), // OSD_FLIGHT_MODE ENCODE_OSD_ELEMENT(1, 0, 44, 0), // OSD_RSSI ENCODE_OSD_ELEMENT(1, 0, 44, 16), // OSD_STOPWATCH - ENCODE_OSD_ELEMENT(1, 0, 18, 8), // OSD_SYSTEM_STATUS + ENCODE_OSD_ELEMENT(1, 0, 14, 8), // OSD_SYSTEM_STATUS ENCODE_OSD_ELEMENT(1, 0, 0, 16), // OSD_THROTTLE ENCODE_OSD_ELEMENT(0, 0, 0, 0), // OSD_VTX_CHANNEL ENCODE_OSD_ELEMENT(1, 0, 0, 17), // OSD_CURRENT_DRAW diff --git a/src/io/vbat.c b/src/io/vbat.c index 99c5a5819..ff77737d0 100644 --- a/src/io/vbat.c +++ b/src/io/vbat.c @@ -111,12 +111,7 @@ void vbat_calc() { const float vdrop_factor = VDROP_FACTOR; #endif - float hyst; - if (flags.lowbatt) - hyst = HYST; - else - hyst = 0.0f; - + const float hyst = flags.lowbatt ? HYST : 0.0f; state.vbat_compensated = tempvolt + vdrop_factor * thrfilt; state.vbat_compensated_cell_avg = state.vbat_compensated / (float)state.lipo_cell_count; diff --git a/src/osd/render.c b/src/osd/render.c index b74175bfc..fdada2ddf 100644 --- a/src/osd/render.c +++ b/src/osd/render.c @@ -1,46 +1,17 @@ #include "osd/render.h" -#include -#include #include -#include "core/debug.h" #include "core/flash.h" #include "core/looptime.h" #include "core/profile.h" -#include "core/project.h" #include "driver/osd.h" -#include "driver/time.h" #include "flight/control.h" -#include "flight/filter.h" #include "io/blackbox_device.h" #include "io/led.h" #include "io/vtx.h" #include "osd/menu.h" -#include "rx/rx.h" -#include "util/util.h" - -typedef enum { - CLEAR, - DISARM, - ARM, - BOOST_1, - BOOST_2, - FAILSAFE, - THRTL_SFTY, - ARM_SFTY, - LOW_BAT, - MOTOR_TEST, - TURTL, - LOOP -} osd_status_labels_t; - -typedef struct { - uint8_t loop_warning; - uint8_t aux_boost; - uint8_t aux_motor_test; - control_flags_t flags; -} osd_sys_status_t; +#include "osd/status.h" #define ICON_RSSI 0x1 #define ICON_CELSIUS 0xe @@ -134,7 +105,7 @@ static const vec3_t rate_defaults[RATE_MODE_ACTUAL + 1][3] = { #pragma GCC diagnostic pop -static uint8_t osd_attr(osd_element_t *el) { +uint8_t osd_attr(osd_element_t *el) { return el->attribute ? OSD_ATTR_INVERT : OSD_ATTR_TEXT; } @@ -384,199 +355,6 @@ static void print_osd_vtx(osd_element_t *el) { } } -// 3 stage return - 0 = stick and hold, 1 = move on but come back to clear, 2 = status print done -static uint8_t print_status(osd_element_t *el, uint8_t persistence, uint8_t label) { - const char *default_system_status_labels[12] = { - " ", - " **DISARMED** ", - " **ARMED** ", - " STICK BOOST 1 ", - " STICK BOOST 2 ", - " **FAILSAFE** ", - "THROTTLE SAFETY ", - " ARMING SAFETY ", - "**LOW BATTERY** ", - "**MOTOR TEST** ", - " **TURTLE** ", - " **LOOPTIME** ", - }; - const char *guac_system_status_labels[12] = { - " ", - " **GAME OVER** ", - " **HERE WE GO AGAIN**", - " STICK BOOST 1 ", - " STICK BOOST 2 ", - " **404 RX NOT FOUND**", - " **DANGER ZONE** ", - " **ARMING SAFETY** ", - "CONSTRUCT MORE PYLONS", - " **MOTOR TEST** ", - " \x60THIS SIDE UP\x60 ", - " **LOOPTIME** ", - }; - - const char **labels = profile.osd.guac_mode ? guac_system_status_labels : default_system_status_labels; - - static uint8_t last_label; - static uint8_t delay_counter = 25; - if (last_label != label) { // critical to reset indexers if a print sequence is interrupted by a new request - delay_counter = 25; - } - last_label = label; - - if (delay_counter == 25) { - // First run, print the label - osd_start(osd_attr(el) | OSD_ATTR_BLINK, el->pos_x, el->pos_y); - osd_write_data((uint8_t *)labels[label], 21); - - delay_counter--; - - return 0; - } - - if (persistence == HOLD) { - // label printed and we should hold - return 2; - } - - // label printed and its temporary - if (!delay_counter) { - // timer is elapsed, print clear label - osd_start(osd_attr(el), el->pos_x, el->pos_y); - osd_write_data((uint8_t *)labels[0], 21); - - delay_counter = 25; - - return 2; - } - - delay_counter--; - return 1; -} - -static uint8_t print_osd_system_status(osd_element_t *el) { - static uint8_t ready = 0; - uint8_t print_stage = 2; // 0 makes the main osd function stick and non zero lets it pass on - - static osd_sys_status_t last_sys_status; - static osd_sys_status_t printing = { - .loop_warning = 0, - .aux_boost = 0, - .aux_motor_test = 0, - .flags = {0}, - }; - - extern uint8_t looptime_warning; - - // things happen here - if (ready) { - if ((flags.arm_state != last_sys_status.flags.arm_state) || printing.flags.arm_state) { - last_sys_status.flags.arm_state = flags.arm_state; - printing.flags.arm_state = 1; - if (flags.arm_state) - print_stage = print_status(el, TEMP, ARM); - else - print_stage = print_status(el, TEMP, DISARM); - if (print_stage == 2) - printing.flags.arm_state = 0; - return print_stage; - } - if (((looptime_warning != last_sys_status.loop_warning) || printing.loop_warning) && (state.looptime_autodetect != LOOPTIME_8K)) { // mute warnings till we are on the edge of 4k->2k - last_sys_status.loop_warning = looptime_warning; - printing.loop_warning = 1; - print_stage = print_status(el, TEMP, LOOP); - if (print_stage == 2) - printing.loop_warning = 0; - return print_stage; - } - if (rx_aux_on(AUX_STICK_BOOST_PROFILE) != last_sys_status.aux_boost || printing.aux_boost) { - last_sys_status.aux_boost = rx_aux_on(AUX_STICK_BOOST_PROFILE); - printing.aux_boost = 1; - if (rx_aux_on(AUX_STICK_BOOST_PROFILE)) - print_stage = print_status(el, TEMP, BOOST_2); - else - print_stage = print_status(el, TEMP, BOOST_1); - if (print_stage == 2) - printing.aux_boost = 0; - return print_stage; - } - if (flags.failsafe != last_sys_status.flags.failsafe || printing.flags.failsafe) { - last_sys_status.flags.failsafe = flags.failsafe; - printing.flags.failsafe = 1; - if (flags.failsafe) - print_stage = print_status(el, HOLD, FAILSAFE); - else - print_stage = print_status(el, HOLD, CLEAR); - if (print_stage == 2) - printing.flags.failsafe = 0; - return print_stage; - } - if ((flags.arm_safety && !flags.failsafe) || printing.flags.arm_safety) { - printing.flags.arm_safety = 1; - if (flags.arm_safety) { - print_stage = print_status(el, HOLD, ARM_SFTY); - } else { - print_stage = print_status(el, HOLD, CLEAR); - if (print_stage == 2) - printing.flags.arm_safety = 0; - } - return print_stage; - } - if ((flags.throttle_safety && !flags.arm_safety) || printing.flags.throttle_safety) { - printing.flags.throttle_safety = 1; - if (flags.throttle_safety) { - print_stage = print_status(el, HOLD, THRTL_SFTY); - } else { - print_stage = print_status(el, HOLD, CLEAR); - if (print_stage == 2) - printing.flags.throttle_safety = 0; - } - return print_stage; - } - if ((flags.lowbatt != last_sys_status.flags.lowbatt && !flags.arm_safety && !flags.throttle_safety && !flags.failsafe) || printing.flags.lowbatt) { - last_sys_status.flags.lowbatt = flags.lowbatt; - printing.flags.lowbatt = 1; - if (flags.lowbatt) - print_stage = print_status(el, HOLD, LOW_BAT); - else - print_stage = print_status(el, HOLD, CLEAR); - if (print_stage == 2) - printing.flags.lowbatt = 0; - return print_stage; - } - if ((rx_aux_on(AUX_MOTOR_TEST) && !flags.arm_safety && !flags.throttle_safety && !flags.failsafe) || (printing.aux_motor_test && !flags.arm_safety && !flags.throttle_safety && !flags.failsafe)) { - printing.aux_motor_test = 1; - if (rx_aux_on(AUX_MOTOR_TEST)) { - print_stage = print_status(el, HOLD, MOTOR_TEST); - } else { - print_stage = print_status(el, HOLD, CLEAR); - if (print_stage == 2) - printing.aux_motor_test = 0; - } - return print_stage; - } - if ((flags.turtle && !flags.arm_safety && !flags.throttle_safety && !flags.failsafe) || (printing.flags.turtle && !flags.arm_safety && !flags.throttle_safety && !flags.failsafe)) { - printing.flags.turtle = 1; - if (flags.turtle) { - print_stage = print_status(el, HOLD, TURTL); - } else { - print_stage = print_status(el, HOLD, CLEAR); - if (print_stage == 2) - printing.flags.turtle = 0; - } - return print_stage; - } - } - if (ready == 0) { - last_sys_status.loop_warning = looptime_warning; - last_sys_status.aux_boost = rx_aux_on(AUX_STICK_BOOST_PROFILE); - last_sys_status.aux_motor_test = rx_aux_on(AUX_MOTOR_TEST); - last_sys_status.flags = flags; - ready = 1; - } - return ready; -} - void osd_init() { osd_device_init(); osd_intro(); @@ -657,7 +435,7 @@ static void osd_display_regular() { } case OSD_SYSTEM_STATUS: { - if (print_osd_system_status(el)) + if (osd_status_update(el)) osd_state.element++; break; } diff --git a/src/osd/render.h b/src/osd/render.h index d33399168..59210dbc2 100644 --- a/src/osd/render.h +++ b/src/osd/render.h @@ -135,6 +135,8 @@ void osd_display(); void osd_display_reset(); void osd_clear(); +uint8_t osd_attr(osd_element_t *el); + osd_screens_t osd_push_screen(osd_screens_t screen); osd_screens_t osd_push_screen_replace(osd_screens_t screen); osd_screens_t osd_pop_screen(); diff --git a/src/osd/status.c b/src/osd/status.c new file mode 100644 index 000000000..85c031e28 --- /dev/null +++ b/src/osd/status.c @@ -0,0 +1,197 @@ +#include "status.h" + +#include + +#include "core/looptime.h" +#include "core/profile.h" +#include "driver/osd.h" +#include "driver/time.h" +#include "flight/control.h" + +#define LABEL_LEN 22 + +typedef enum { + STATUS_RX_WAIT, + STATUS_FAILSAFE, + STATUS_LOOPTIME, + STATUS_ARM_SAFETY, + STATUS_THROTTLE_SAFETY, + STATUS_ARM, + STATUS_DISARM, + STATUS_LOW_BAT, + STATUS_MOTOR_TEST, + STATUS_TURTLE, + STATUS_MAX, +} osd_status_entries_t; + +typedef enum { + MODE_HOLD, + MODE_TEMP, +} osd_status_mode_t; + +typedef enum { + PRINT_IDLE, + PRINT_START, + PRINT_WAIT, + PRINT_CLEAR, + PRINT_DONE, +} osd_print_state_t; + +typedef struct { + osd_print_state_t state; + osd_status_mode_t mode; + uint8_t label[LABEL_LEN]; +} osd_status_t; + +static osd_status_t current_status = { + .state = PRINT_IDLE, +}; + +const char *default_system_status_labels[STATUS_MAX] = { + [STATUS_RX_WAIT] = "WAIT FOR RX", + [STATUS_FAILSAFE] = "**FAILSAFE**", + [STATUS_LOOPTIME] = "**LOOPTIME**", + [STATUS_ARM_SAFETY] = "ARMING SAFETY", + [STATUS_THROTTLE_SAFETY] = "THROTTLE SAFETY", + [STATUS_ARM] = "**ARMED**", + [STATUS_DISARM] = "**DISARMED**", + [STATUS_LOW_BAT] = "**LOW BATTERY**", + [STATUS_MOTOR_TEST] = "**MOTOR TEST**", + [STATUS_TURTLE] = "**TURTLE**", +}; + +const char *guac_system_status_labels[STATUS_MAX] = { + [STATUS_RX_WAIT] = "RX LOADING", + [STATUS_FAILSAFE] = "**404 RX NOT FOUND**", + [STATUS_LOOPTIME] = "**LOOPTIME**", + [STATUS_ARM_SAFETY] = "**ARMING SAFETY**", + [STATUS_THROTTLE_SAFETY] = "**DANGER ZONE**", + [STATUS_ARM] = "**HERE WE GO AGAIN**", + [STATUS_DISARM] = "**GAME OVER**", + [STATUS_LOW_BAT] = "CONSTRUCT MORE PYLONS", + [STATUS_MOTOR_TEST] = "**MOTOR TEST**", + [STATUS_TURTLE] = "\x60THIS SIDE UP\x60", +}; + +static void osd_status_show(osd_status_mode_t mode, const char *label) { + current_status.state = PRINT_START; + current_status.mode = mode; + const uint32_t len = strlen(label); + if (len >= LABEL_LEN) { + memcpy(current_status.label, label, LABEL_LEN); + } else { + const uint32_t offset = (LABEL_LEN - len) / 2; + memset(current_status.label, ' ', LABEL_LEN); + memcpy(current_status.label + offset, label, len); + } +} + +static bool osd_status_print(osd_element_t *el) { + static const uint8_t empty_label[LABEL_LEN] = {' '}; + static uint32_t start_time; + + switch (current_status.state) { + case PRINT_START: + osd_start(osd_attr(el) | OSD_ATTR_BLINK, el->pos_x, el->pos_y); + osd_write_data(current_status.label, LABEL_LEN); + + if (current_status.mode == MODE_HOLD) { + current_status.state = PRINT_DONE; + } else { + current_status.state = PRINT_WAIT; + } + start_time = time_millis(); + return true; + + case PRINT_WAIT: + if ((time_millis() - start_time) < 2500) { + return true; + } + current_status.state = PRINT_CLEAR; + return true; + + case PRINT_CLEAR: + osd_start(osd_attr(el) | OSD_ATTR_BLINK, el->pos_x, el->pos_y); + osd_write_data(empty_label, LABEL_LEN); + current_status.state = PRINT_DONE; + return true; + + case PRINT_DONE: + current_status.state = PRINT_IDLE; + return true; + + case PRINT_IDLE: + return false; + } + + return false; +} + +bool osd_status_update(osd_element_t *el) { + if (osd_status_print(el)) { + return true; + } + + const char **labels = profile.osd.guac_mode ? guac_system_status_labels : default_system_status_labels; + + if (!flags.rx_ready) { + osd_status_show(MODE_HOLD, labels[STATUS_RX_WAIT]); + return osd_status_print(el); + } + + if (flags.failsafe && flags.rx_ready) { + // only show failsafe if rx was ready + osd_status_show(MODE_HOLD, labels[STATUS_FAILSAFE]); + return osd_status_print(el); + } + + { + extern uint8_t looptime_warning; + if (looptime_warning && (state.looptime_autodetect != LOOPTIME_8K)) { + osd_status_show(MODE_HOLD, labels[STATUS_LOOPTIME]); + return osd_status_print(el); + } + } + + if (flags.arm_safety) { + osd_status_show(MODE_HOLD, labels[STATUS_ARM_SAFETY]); + return osd_status_print(el); + } + + if (flags.throttle_safety) { + osd_status_show(MODE_HOLD, labels[STATUS_THROTTLE_SAFETY]); + return osd_status_print(el); + } + + { + static uint8_t last_arm_state = 0; + if (flags.arm_state != last_arm_state) { + if (flags.arm_state) { + osd_status_show(MODE_TEMP, labels[STATUS_ARM]); + } else { + osd_status_show(MODE_TEMP, labels[STATUS_DISARM]); + } + last_arm_state = flags.arm_state; + return osd_status_print(el); + } + } + + if (flags.lowbatt && state.vbat >= 1.0f) { + // only show lowbat if we detect some voltage at all + osd_status_show(MODE_HOLD, labels[STATUS_LOW_BAT]); + return osd_status_print(el); + } + + if (rx_aux_on(AUX_MOTOR_TEST)) { + osd_status_show(MODE_HOLD, labels[STATUS_MOTOR_TEST]); + return osd_status_print(el); + } + + if (flags.turtle) { + osd_status_show(MODE_HOLD, labels[STATUS_TURTLE]); + return osd_status_print(el); + } + + current_status.state = PRINT_CLEAR; + return false; +} \ No newline at end of file diff --git a/src/osd/status.h b/src/osd/status.h new file mode 100644 index 000000000..8d2a42545 --- /dev/null +++ b/src/osd/status.h @@ -0,0 +1,7 @@ +#pragma once + +#include + +#include "osd/render.h" + +bool osd_status_update(osd_element_t *el); \ No newline at end of file