From 5ac5fae020dcea70898b31159a1b5996ea03562b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 11:48:56 -0700 Subject: [PATCH 1/3] MAVLink app: better yaw scaling --- src/modules/mavlink/mavlink_receiver.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index efdd457689..e91e834231 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -937,9 +937,25 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.rc_ppm_frame_length = 0; rc.input_source = RC_INPUT_SOURCE_MAVLINK; rc.rssi = RC_INPUT_RSSI_MAX; + + /* roll */ rc.values[0] = man.x / 2 + 1500; + /* pitch */ rc.values[1] = man.y / 2 + 1500; - rc.values[2] = man.r / 2 + 1500; + + /* + * yaw needs special handling as some joysticks have a circular mechanical mask, + * which makes the corner positions unreachable. + * scale yaw up and clip it to overcome this. + */ + rc.values[2] = man.r / 1.5f + 1500; + if (rc.values[2] > 2000) { + rc.values[2] = 2000; + } else if (rc.values[2] < 1000) { + rc.values[2] = 1000; + } + + /* throttle */ rc.values[3] = man.z + 1000; rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000; From aab379cde940112f90a26dc4e4bd01a0d6fd08bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 17:41:03 -0700 Subject: [PATCH 2/3] MAVLink app: Fix yaw scaling for joystick input --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e91e834231..8ebe0c696d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -948,7 +948,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) { From 46920cfd27fba27e558351995f94b341fc545fc4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 May 2015 17:41:26 -0700 Subject: [PATCH 3/3] GPS driver: Obey non-publish flag in all modes --- src/drivers/gps/gps.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index e3706cf671..30fbdfd8d2 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 > 0) { - 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 > 0) { + 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