Repository: ClemensElflein/OpenMower
File: Firmware/LowLevel/src/main.cpp (C++)

Code before:
struct ll_high_level_state last_high_level_state = {0};

// Struct for the current LEDs. This gets sent to the UI periodically
struct msg_set_leds leds_message = {0};

// A mutex which is used by core1 each time status_message is modified.
// We can lock it during message transmission to prevent core1 to modify data in this time.
auto_init_mutex(mtx_status_message);

bool emergency_latch = true;
bool sound_available = false;
bool charging_allowed = false;
bool ROS_running = false;
unsigned long charging_disabled_time = 0;

float imu_temp[9];

uint16_t ui_version = 0;                   // Last received UI firmware version
uint8_t ui_topic_bitmask = Topic_set_leds; // UI subscription, default to Set_LEDs
uint16_t ui_interval = 1000;               // UI send msg (LED/State) interval (ms)

// LL/HL config
#define CONFIG_FILENAME "/openmower.cfg"  // Where our llhl_config get saved in LittleFS (flash)
uint16_t config_crc_in_flash = 0;
struct ll_high_level_config llhl_config;  // LL/HL configuration (is initialized with YF-C500 defaults)
static_assert(sizeof(ConfigOptions) == 1, "Enlarging struct ConfigOption to a sizeof > 1 will break packet compatibilty");

// Hall input sources, same order as in ll_high_level_config.hall_configs
const std::function<bool()> halls[MAX_HALL_INPUTS] = {
    []() { return gpio_get(PIN_EMERGENCY_1); },                             // OM-Hall-1 (default Lift1)
    []() { return gpio_get(PIN_EMERGENCY_2); },                             // OM-Hall-2 (default Lift2)
    []() { return gpio_get(PIN_EMERGENCY_3); },                             // OM-Hall-3 (default Stop1)
    []() { return gpio_get(PIN_EMERGENCY_4); },                             // OM-Hall-4 (default Stop2)
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_LIFT; },   // CoverUI-LIFT | LIFTX (up to CoverUI FW 2.0x)
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_LIFTX; },  // CoverUI-LIFTX (as of CoverUI FW 2.1x)
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_BUMP; },   // CoverUI-LBUMP | RBUMP (up to CoverUI FW 2.0x)
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_RBUMP; },  // CoverUI-RBUMP (as of CoverUI FW 2.1x)
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_STOP1; },  // CoverUI-Stop1
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_STOP2; },  // CoverUI-Stop2
};
// Instead of iterating constantly over all halls, we use these compacted ones, whose contain the halls index of the used ones
uint8_t stop_halls[MAX_HALL_INPUTS] = {};
uint8_t lift_halls[MAX_HALL_INPUTS] = {};

void sendMessage(void *message, size_t size);
void sendUIMessage(void *message, size_t size);
void onPacketReceived(const uint8_t *buffer, size_t size);
void onUIPacketReceived(const uint8_t *buffer, size_t size);
void manageUISubscriptions();
void saveConfigToFlash();
void readConfigFromFlash();

Review comment (suggestion):
If feel that this should be right under the struct definition, but not sure if including it in the header has disadvantages.

Code after:
// current high level state
struct ll_high_level_state last_high_level_state = {0};

// Struct for the current LEDs. This gets sent to the UI periodically
struct msg_set_leds leds_message = {0};

// A mutex which is used by core1 each time status_message is modified.
// We can lock it during message transmission to prevent core1 to modify data in this time.
auto_init_mutex(mtx_status_message);

bool emergency_latch = true;
bool sound_available = false;
bool charging_allowed = false;
bool ROS_running = false;
unsigned long charging_disabled_time = 0;

float imu_temp[9];

uint16_t ui_version = 0;                   // Last received UI firmware version
uint8_t ui_topic_bitmask = Topic_set_leds; // UI subscription, default to Set_LEDs
uint16_t ui_interval = 1000;               // UI send msg (LED/State) interval (ms)

// LL/HL config
#define CONFIG_FILENAME "/openmower.cfg"  // Where our llhl_config get saved in LittleFS (flash)
uint16_t config_crc_in_flash = 0;
struct ll_high_level_config llhl_config;  // LL/HL configuration (is initialized with YF-C500 defaults)

// Available hall input sources, same order as in ll_high_level_config.hall_configs
const std::function<bool()> available_halls[MAX_HALL_INPUTS] = {
    []() { return gpio_get(PIN_EMERGENCY_1); },                             // OM-Hall-1 (default Lift1)
    []() { return gpio_get(PIN_EMERGENCY_2); },                             // OM-Hall-2 (default Lift2)
    []() { return gpio_get(PIN_EMERGENCY_3); },                             // OM-Hall-3 (default Stop1)
    []() { return gpio_get(PIN_EMERGENCY_4); },                             // OM-Hall-4 (default Stop2)
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_LIFT; },   // CoverUI-LIFT | LIFTX (up to CoverUI FW 2.0x)
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_LIFTX; },  // CoverUI-LIFTX (as of CoverUI FW 2.1x)
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_BUMP; },   // CoverUI-LBUMP | RBUMP (up to CoverUI FW 2.0x)
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_RBUMP; },  // CoverUI-RBUMP (as of CoverUI FW 2.1x)
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_STOP1; },  // CoverUI-Stop1
    []() { return stock_ui_emergency_state & LL_EMERGENCY_BIT_CU_STOP2; },  // CoverUI-Stop2
};
// Instead of iterating constantly over all available_halls, we use this compacted vector (which only contain the used halls)
etl::vector<HallHandle, MAX_HALL_INPUTS> halls;

void sendMessage(void *message, size_t size);
void sendUIMessage(void *message, size_t size);
void onPacketReceived(const uint8_t *buffer, size_t size);
void onUIPacketReceived(const uint8_t *buffer, size_t size);
void manageUISubscriptions();
void readConfigFromFlash();

void setRaspiPower(bool power) {