Added Wind Estimator to mavlink telemetry, but it shows constant bearing and speed

[tipoman9]

Added external temp sensors and wind estimator to mavlink telemetry. The former works, but the latter gives constant bearing and speed of 4m/s. I use qOpenHD to read telemetry , there is not problem in parsing.
Here is a video, the wind gauge is in middle left.
https://www.youtube.com/watch?v=a8oO-BhOeBA
The same FC when used with analogue VTx shows correct wind.
Here is the code I used to patch mavlink.c

float wind_direction=0,wind_speed=0;
void mavlinkSendWindEstimator(void)
{
    uint16_t windHeading; // centidegrees
   
    if (ARMING_FLAG(ARMED)){
        wind_speed =  getEstimatedHorizontalWindSpeed(&windHeading) / 100 ; //  cm/s to m/s

        //int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22);
        wind_direction = CENTIDEGREES_TO_DEGREES((float)windHeading);        
        
    }else{//Show smthng to test messate is working
        wind_direction+=3;
        wind_speed+=0.5;
    }
     if (wind_speed>25)//just in case
            wind_speed=1;
    if (wind_direction>360)
            wind_direction=3;
    
    mavlink_msg_wind_pack(mavSystemId, mavComponentId, &mavSendMsg,        
    wind_direction,
    wind_speed,0);
   

    mavlinkSendMessage();
}