Send buffer to UDP

[nopnop2002]

Send buffer to UDP

The following cord of mavesp8266_vehicle.cpp is not correct. The direction of memcpy is opposite.

//-- Read MavLink message from UAS
void
MavESP8266Vehicle::readMessage()
{
    if(_queue_count < UAS_QUEUE_SIZE) {
        if(_readMessage()) {
            _queue_count++;
        }
    }
    //-- Do we have a message to send and is it time to forward data?
    if(_queue_count && (_queue_count >= UAS_QUEUE_THRESHOLD || (millis() - _queue_time) > UAS_QUEUE_TIMEOUT)) {
        int sent = _forwardTo->sendMessage(_message, _queue_count);
        //-- Sent it all?
        if(sent == _queue_count) {
            memset(_message, 0, sizeof(_message));
            _queue_count = 0;
            _queue_time  = millis();
        //-- Sent at least some?
        } else if(sent) {
            //-- Move the pending ones up the queue
            int left = _queue_count - sent;
            for(int i = 0; i < left; i++) {
                // nop memcpy(&_message[sent+i], &_message[i], sizeof(mavlink_message_t)); --> !!!
                memcpy(&_message[i], &_message[sent+i], sizeof(mavlink_message_t));
            }
            _queue_count = left;
        }
        //-- Maintain buffer status
        float cur_status  = 0.0;
        float buffer_size = (float)UAS_QUEUE_THRESHOLD;
        float buffer_left = (float)(UAS_QUEUE_THRESHOLD - _queue_count);
        if(buffer_left > 0.0)
            cur_status = ((buffer_left / buffer_size) * 100.0f);
        _buffer_status = (_buffer_status * 0.05f) + (cur_status * 0.95);
    }