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

Code before:
        return;

    if (buffer[0] == PACKET_ID_LL_HEARTBEAT && size == sizeof(struct ll_heartbeat)) {

        // CRC and packet is OK, reset watchdog
        last_heartbeat_millis = millis();
        struct ll_heartbeat *heartbeat = (struct ll_heartbeat *) buffer;
        if (heartbeat->emergency_release_requested) {
            emergency_latch = false;
        }
        // Check in this order, so we can set it again in the same packet if required.
        if (heartbeat->emergency_requested) {
            emergency_latch = true;
        }
        if (!ROS_running) {
            // ROS is running (again (i.e. due to restart after reconfiguration))
            ROS_running = true;
        }
    } else if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_STATE && size == sizeof(struct ll_high_level_state)) {
        // copy the state
        last_high_level_state = *((struct ll_high_level_state *) buffer);
    } else if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ || buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP) {
        applyConfig(buffer + 1, size - 1);  // Skip type

        // Store in flash
        saveConfigToFlash();

        if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ)
            sendConfigMessage(PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP);  // Other side requested a config response
    }
}

// returns true, if it's a good idea to charge the battery (current, voltages, ...)
bool checkShouldCharge() {
    return status_message.v_charge < llhl_config.v_charge_cutoff && status_message.charging_current < llhl_config.i_charge_cutoff && status_message.v_battery < llhl_config.v_battery_cutoff;
}

void updateChargingEnabled() {
    if (charging_allowed) {
        if (!checkShouldCharge()) {
            digitalWrite(PIN_ENABLE_CHARGE, LOW);
            charging_allowed = false;
            charging_disabled_time = millis();
        }
    } else {
        // enable charging after CHARGING_RETRY_MILLIS
        if (millis() - charging_disabled_time > CHARGING_RETRY_MILLIS) {
            if (!checkShouldCharge()) {
                digitalWrite(PIN_ENABLE_CHARGE, LOW);
                charging_allowed = false;
                charging_disabled_time = millis();

Review comment (suggestion):
This will save the effective config, right? If we later change any defaults, it will still load the old values from flash. I think we need to save the packet from ROS instead. We can even store it in full length and keep the CRC, because applyConfig() will be used when loading it.

Code after:
        }
        if (!ROS_running) {
            // ROS is running (again (i.e. due to restart after reconfiguration))
            ROS_running = true;
        }
    } else if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_STATE && size == sizeof(struct ll_high_level_state)) {
        // copy the state
        last_high_level_state = *((struct ll_high_level_state *) buffer);
    } else if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ || buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP) {
        applyConfig(buffer + 1, size - 3);  // Skip packet- type and CRC

        // Response if requested (before save, to ensure REQ/RSP timing)
        if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ)
            sendConfigMessage(PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP);  // Other side requested a config response

        // Store the whole received packet in flash and not the live (applied) config. This has the following benefits:
        // - We're free to change ll_high_level_config defaults in future without fiddling with already stored ones
        // - Reuse the already calculated and validated CRC
        if (crc == config_crc_in_flash) return;  // Protect wear leveling
        File f = LittleFS.open(CONFIG_FILENAME, "w");
        if (!f) return;
        if (f.write(buffer, size) == size) config_crc_in_flash = crc;
        f.close();
    }
}

// returns true, if it's a good idea to charge the battery (current, voltages, ...)
bool checkShouldCharge() {
    return status_message.v_charge < llhl_config.v_charge_cutoff && status_message.charging_current < llhl_config.i_charge_cutoff && status_message.v_battery < llhl_config.v_battery_cutoff;
}

void updateChargingEnabled() {
    if (charging_allowed) {
        if (!checkShouldCharge()) {
            digitalWrite(PIN_ENABLE_CHARGE, LOW);
            charging_allowed = false;
            charging_disabled_time = millis();
        }
    } else {
        // enable charging after CHARGING_RETRY_MILLIS
        if (millis() - charging_disabled_time > CHARGING_RETRY_MILLIS) {
            if (!checkShouldCharge()) {
                digitalWrite(PIN_ENABLE_CHARGE, LOW);
                charging_allowed = false;
                charging_disabled_time = millis();
            } else {
                digitalWrite(PIN_ENABLE_CHARGE, HIGH);
                charging_allowed = true;
            }
        }
    }