From 6fb2496c49d2e6e380207df4acdfa9f79f2c0c5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Mar 2013 18:38:40 +0100 Subject: Improved HIL startup script, added highres HIL receive routine --- apps/mavlink/mavlink_receiver.c | 68 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) (limited to 'apps') diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 4d9cd964d..28b0c50c9 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -376,6 +376,74 @@ handle_message(mavlink_message_t *msg) } } + if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) { + + mavlink_highres_imu_t imu; + mavlink_msg_highres_imu_decode(msg, &imu); + + /* packet counter */ + static uint16_t hil_counter = 0; + static uint16_t hil_frames = 0; + static uint64_t old_timestamp = 0; + + /* sensors general */ + hil_sensors.timestamp = imu.time_usec; + + /* hil gyro */ + static const float mrad2rad = 1.0e-3f; + hil_sensors.gyro_counter = hil_counter; + hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad; + hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; + hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; + hil_sensors.gyro_rad_s[0] = imu.xgyro; + hil_sensors.gyro_rad_s[1] = imu.ygyro; + hil_sensors.gyro_rad_s[2] = imu.zgyro; + + /* accelerometer */ + hil_sensors.accelerometer_counter = hil_counter; + static const float mg2ms2 = 9.8f / 1000.0f; + hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; + hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; + hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; + hil_sensors.accelerometer_m_s2[0] = imu.xacc; + hil_sensors.accelerometer_m_s2[1] = imu.yacc; + hil_sensors.accelerometer_m_s2[2] = imu.zacc; + hil_sensors.accelerometer_mode = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + + /* adc */ + hil_sensors.adc_voltage_v[0] = 0; + hil_sensors.adc_voltage_v[1] = 0; + hil_sensors.adc_voltage_v[2] = 0; + + /* magnetometer */ + float mga2ga = 1.0e-3f; + hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; + hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; + hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga; + hil_sensors.magnetometer_ga[0] = imu.xmag; + hil_sensors.magnetometer_ga[1] = imu.ymag; + hil_sensors.magnetometer_ga[2] = imu.zmag; + hil_sensors.magnetometer_range_ga = 32.7f; // int16 + hil_sensors.magnetometer_mode = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + + /* publish */ + orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + + // increment counters + hil_counter += 1 ; + hil_frames += 1 ; + + // output + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil imu at %d hz\n", hil_frames/10); + old_timestamp = timestamp; + hil_frames = 0; + } + } + if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) { mavlink_gps_raw_int_t gps; -- cgit v1.2.3 From 6eba7a9e41ca3e5fac9c3b5feb74af9f37774987 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Mar 2013 12:47:47 +0100 Subject: Fix gyro measurement noise variance --- apps/examples/kalman_demo/params.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 91e80a5ea..50642f067 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,7 +1,7 @@ #include /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f); +PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); @@ -14,4 +14,3 @@ PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); - -- cgit v1.2.3 From 9dbd2695d3b476e8ed0a2001b027329e8600bd29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Mar 2013 12:48:28 +0100 Subject: Hotfix missing yaw deadzone default (leads to continuous turns since zero speed is never commanded) --- apps/sensors/sensor_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'apps') diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index a1ef9d136..c850e3a1e 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); -- cgit v1.2.3 From a1c8d19c343454c5464ae21be1ec4456b0559996 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 7 Apr 2013 18:43:19 +0200 Subject: Added generation of pressure altitude in highres IMU message mode --- apps/mavlink/mavlink_receiver.c | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) (limited to 'apps') diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 28b0c50c9..798e509e0 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -308,6 +308,14 @@ handle_message(mavlink_message_t *msg) uint64_t timestamp = hrt_absolute_time(); + /* TODO, set ground_press/ temp during calib */ + static const float ground_press = 1013.25f; // mbar + static const float ground_tempC = 21.0f; + static const float ground_alt = 0.0f; + static const float T0 = 273.15; + static const float R = 287.05f; + static const float g = 9.806f; + if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) { mavlink_raw_imu_t imu; @@ -429,6 +437,15 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + hil_sensors.baro_pres_mbar = imu.abs_pressure; + + float tempC = imu.temperature; + float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; + float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure); + + hil_sensors.baro_alt_meter = h; + hil_sensors.baro_temp_celcius = imu.temperature; + /* publish */ orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); @@ -503,13 +520,6 @@ handle_message(mavlink_message_t *msg) hil_sensors.timestamp = press.time_usec; /* baro */ - /* TODO, set ground_press/ temp during calib */ - static const float ground_press = 1013.25f; // mbar - static const float ground_tempC = 21.0f; - static const float ground_alt = 0.0f; - static const float T0 = 273.15; - static const float R = 287.05f; - static const float g = 9.806f; float tempC = press.temperature / 100.0f; float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; -- cgit v1.2.3 From 8c70f4412d7e55f22cffa60e0ea953751780f462 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 8 Apr 2013 23:29:24 -0700 Subject: Fixed axis in L3GD20 driver --- apps/drivers/l3gd20/l3gd20.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'apps') diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index 6227df72a..c7f433dd4 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -684,9 +684,10 @@ L3GD20::measure() * 74 from all measurements centers them around zero. */ report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ - report->x_raw = raw_report.x; - report->y_raw = raw_report.y; + + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); report->z_raw = raw_report.z; report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; -- cgit v1.2.3