diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 4be13a5f34..756287dd81 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -361,11 +361,14 @@ GPS::task_main() _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.timestamp_time = hrt_absolute_time(); - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + if (!(_pub_blocked)) { + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } } // GPS is obviously detected successfully, reset statistics diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6adf66356a..4e3da6c7c0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1007,7 +1007,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) * which makes the corner positions unreachable. * scale yaw up and clip it to overcome this. */ - rc.values[2] = man.r / 1.5f + 1500; + rc.values[2] = man.r / 1.2f + 1500; if (rc.values[2] > 2000) { rc.values[2] = 2000; } else if (rc.values[2] < 1000) {