From f13bbacd5277123d6af2cb5ed21587c220031353 Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 12 Nov 2018 15:07:45 +0100 Subject: [PATCH] mc_att_control: copy sensor_correction topic once initially Signed-off-by: Roman --- src/modules/mc_att_control/mc_att_control_main.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 170d6b3afe..0c5445ccb1 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -613,6 +613,14 @@ MulticopterAttitudeControl::run() } _sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); + + // sensor correction topic is not being published regularly and we might have missed the first update. + // so copy it once initially so that we have the latest data. In future this will not be needed anymore as the + // behavior of the orb_check function will change + if (_sensor_correction_sub > 0) { + orb_copy(ORB_ID(sensor_correction), _sensor_correction_sub, &_sensor_correction); + } + _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); /* wakeup source: gyro data from sensor selected by the sensor app */