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

Code before:
    if (rcv_config.lift_period != 0xffff) new_config.lift_period = rcv_config.lift_period;
    if (rcv_config.tilt_period != 0xffff) new_config.tilt_period = rcv_config.tilt_period;
    if (rcv_config.volume != 0xff) new_config.volume = rcv_config.volume;

    // Take over those hall inputs which are not undefined
    for (size_t i = 0; i < MAX_HALL_INPUTS; i++) {
        if (rcv_config.hall_configs[i].mode != HallMode::UNDEFINED)
            new_config.hall_configs[i] = rcv_config.hall_configs[i];
    }

    // Apply active Emergency/Hall configurations to our compacted stop/lift-hall arrays which get used by updateEmergency()
    uint8_t stop_hall_idx = 0, lift_hall_idx = 0;
    for (size_t i = 0; i < MAX_HALL_INPUTS; i++) {
        switch (new_config.hall_configs[i].mode) {
            case HallMode::LIFT_TILT:
                lift_halls[lift_hall_idx] = i;
                lift_hall_idx++;
                break;
            case HallMode::STOP:
                stop_halls[stop_hall_idx] = i;
                stop_hall_idx++;
                break;
        }
    }
    lift_halls[lift_hall_idx] = 0xff;  // End of lift halls marker
    stop_halls[stop_hall_idx] = 0xff;  // End of stop halls marker

    llhl_config = new_config;  // Make new config live
}

void onPacketReceived(const uint8_t *buffer, size_t size) {
    // sanity check for CRC to work (1 type, 1 data, 2 CRC)
    if (size < 4)
        return;

    // check the CRC
    uint16_t crc = CRC16.ccitt(buffer, size - 2);

    if (buffer[size - 1] != ((crc >> 8) & 0xFF) ||
        buffer[size - 2] != (crc & 0xFF))
        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.

Review comment (bug):
If you have all 10x hall or 10x stop, this will overflow. I have an idea for the hall handling, but need to write it on my laptop.

Code after:

    // Handle all emergency/halls
    halls.clear();
    for (size_t i = 0; i < MAX_HALL_INPUTS; i++) {
        // Take over those halls which are not "undefined"
        if (rcv_config.hall_configs[i].mode != HallMode::UNDEFINED)
            new_config.hall_configs[i] = rcv_config.hall_configs[i];

        if (new_config.hall_configs[i].mode == HallMode::OFF)
            continue;

        // Apply used hall to our compacted stop/lift-hall vector which get used by updateEmergency()
        halls.push_back({new_config.hall_configs[i], available_halls[i]});
    }
    llhl_config = new_config;  // Make new config live
}

void onPacketReceived(const uint8_t *buffer, size_t size) {
    // sanity check for CRC to work (1 type, 1 data, 2 CRC)
    if (size < 4)
        return;

    // check the CRC
    uint16_t crc = CRC16.ccitt(buffer, size - 2);

    if (buffer[size - 1] != ((crc >> 8) & 0xFF) ||
        buffer[size - 2] != (crc & 0xFF))
        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 - 3);  // Skip packet- type and CRC