From 767f253976746a5533dd1a258373368539b4359e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Aug 2012 11:15:44 +0200 Subject: [PATCH 1/4] Fixed attitude rate limiting --- apps/mavlink/mavlink.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 47eab6be87..b667269ede 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -467,7 +467,7 @@ static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) break; case MAVLINK_MSG_ID_ATTITUDE: /* attitude sub triggers attitude */ - orb_set_interval(att_sub, 100); + orb_set_interval(att_sub, min_interval); break; default: /* not found */ @@ -520,6 +520,7 @@ static void *uorb_receiveloop(void *arg) /* --- ATTITUDE VALUE --- */ /* subscribe to ORB for attitude */ att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + orb_set_interval(att_sub, 100); fds[fdsc_count].fd = att_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; From 0d28187960ff1c87c9f6ca57f544b53ee6ed8d9c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Aug 2012 11:36:44 +0200 Subject: [PATCH 2/4] Fixed attitude mag scaling --- apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index c917ff7b26..1e2e6b6253 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -92,9 +92,9 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul accel_values.z = raw->accelerometer_m_s2[2] * 9.81f; float_vect3 mag_values; - mag_values.x = raw->magnetometer_ga[0]*100.0f; - mag_values.y = raw->magnetometer_ga[1]*100.0f; - mag_values.z = raw->magnetometer_ga[2]*100.0f; + mag_values.x = raw->magnetometer_ga[0]*510.0f; + mag_values.y = raw->magnetometer_ga[1]*510.0f; + mag_values.z = raw->magnetometer_ga[2]*510.0f; attitude_blackmagic(&accel_values, &mag_values, &gyro_values); From aaf2a23f18eb802c9fea1e45199106eefbab59f3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Aug 2012 12:38:45 +0200 Subject: [PATCH 3/4] Reduced optimistic send rates, better mag scaling --- apps/mavlink/mavlink.c | 12 ++++++------ .../attitude_estimator_bm/attitude_estimator_bm.c | 6 +++--- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index b667269ede..7fba65cb73 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1318,17 +1318,17 @@ int mavlink_thread_main(int argc, char *argv[]) /* all subscriptions are now active, set up initial guess about rate limits */ if (baudrate >= 921600) { - /* set no limit */ /* 500 Hz / 2 ms */ - //set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 2); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 2); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 2); } else if (baudrate >= 460800) { /* 250 Hz / 4 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 4); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 4); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 5); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 5); } else if (baudrate >= 115200) { /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 20); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 20); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 50); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100); diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index 1e2e6b6253..feaed66954 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -87,9 +87,9 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul gyro_values.z = raw->gyro_rad_s[2]; float_vect3 accel_values; - accel_values.x = raw->accelerometer_m_s2[0] * 9.81f; - accel_values.y = raw->accelerometer_m_s2[1] * 9.81f; - accel_values.z = raw->accelerometer_m_s2[2] * 9.81f; + accel_values.x = raw->accelerometer_m_s2[0] * 9.81f * 9.0f; + accel_values.y = raw->accelerometer_m_s2[1] * 9.81f * 9.0f; + accel_values.z = raw->accelerometer_m_s2[2] * 9.81f * 9.0f; float_vect3 mag_values; mag_values.x = raw->magnetometer_ga[0]*510.0f; From 78db6c990b609a773b2bb1eb0e1d979e77933ced Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Aug 2012 13:11:19 +0200 Subject: [PATCH 4/4] Testing larger stack for sensors app --- apps/sensors/Makefile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile index 125839bb35..f8dc64ed13 100644 --- a/apps/sensors/Makefile +++ b/apps/sensors/Makefile @@ -37,6 +37,7 @@ APPNAME = sensors PRIORITY = SCHED_PRIORITY_MAX-5 -STACKSIZE = 4096 +# Reduce to 4096 on next occasion +STACKSIZE = 8000 include $(APPDIR)/mk/app.mk