Merge stable master to free_rtos

We have more stable version flight control now. So we need to port these change to free_rtos branch testing.