From df8148033a1f60400693e80c3732a43cc26e0ee2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Oct 2012 08:14:43 +0200 Subject: [PATCH] Cleaned up calibration, added text messages ring buffer --- apps/commander/commander.c | 76 +++++++++++++----------------- apps/drivers/hmc5883/hmc5883.cpp | 9 ++-- apps/mavlink/mavlink.c | 29 ++++++++---- apps/mavlink/mavlink_log.c | 80 ++++++++++++++++++++++++++++++++ apps/mavlink/mavlink_log.h | 29 +++++++++++- 5 files changed, 163 insertions(+), 60 deletions(-) create mode 100644 apps/mavlink/mavlink_log.c diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 285b11a457..dafd32ec2b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -291,23 +291,28 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) state_machine_publish(status_pub, status, mavlink_fd); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - orb_set_interval(sub_mag, 22); struct mag_report mag; - /* 30 seconds */ + /* 45 seconds */ uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 2000; unsigned int calibration_counter = 0; - /* - * FLT_MIN is not the most negative float number, - * but the smallest number by magnitude float can - * represent. Use -FLT_MAX to initialize the most - * negative number - */ - float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + // XXX old cal + // * FLT_MIN is not the most negative float number, + // * but the smallest number by magnitude float can + // * represent. Use -FLT_MAX to initialize the most + // * negative number + + // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; - int fd = open(MAG_DEVICE_PATH, 0); + int fd = open(MAG_DEVICE_PATH, O_RDONLY); /* erase old calibration */ struct mag_scale mscale_null = { @@ -332,7 +337,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* calibrate offsets */ - uint64_t calibration_start = hrt_absolute_time(); + // uint64_t calibration_start = hrt_absolute_time(); uint64_t axis_deadline = hrt_absolute_time(); uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; @@ -340,7 +345,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const char axislabels[3] = { 'Z', 'X', 'Y'}; int axis_index = -1; - const int calibration_maxcount = 2000; float *x = malloc(sizeof(float) * calibration_maxcount); float *y = malloc(sizeof(float) * calibration_maxcount); float *z = malloc(sizeof(float) * calibration_maxcount); @@ -426,27 +430,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) free(y); free(z); - float mag_offset[3] = {sphere_x, sphere_y, sphere_z}; - - // * - // * The offset is subtracted from the sensor values, so the result is the - // * POSITIVE number that has to be subtracted from the sensor data - // * to shift the center to zero - // * - // * offset = max - ((max - min) / 2.0f) - // * max - max/2 + min/2 - // * max/2 + min/2 - // * - // * which reduces to - // * - // * offset = (max + min) / 2.0f - - - // mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; - // mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; - // mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - - if (isfinite(mag_offset[0]) && isfinite(mag_offset[1]) && isfinite(mag_offset[2])) { + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { fd = open(MAG_DEVICE_PATH, 0); @@ -455,9 +439,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) warn("WARNING: failed to get scale / offsets for mag"); - mscale.x_offset = mag_offset[0]; - mscale.y_offset = mag_offset[1]; - mscale.z_offset = mag_offset[2]; + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) warn("WARNING: failed to set scale / offsets for mag"); @@ -495,13 +479,19 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) warn("WARNING: auto-save of params to EEPROM failed"); } - printf("[mag cal] scale: %.6f %.6f %.6f\n\t\toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - mscale.x_scale, mscale.y_scale, mscale.z_scale, - mscale.x_offset, mscale.y_offset, mscale.z_offset, sphere_radius); + printf("[mag cal]\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - // char buf[50]; - // sprintf(buf, "[commander] mag cal: x:%d y:%d z:%d mGa", (int)(mag_offset[0]*1000), (int)(mag_offset[1]*1000), (int)(mag_offset[2]*1000)); - // mavlink_log_info(mavlink_fd, buf); + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); } else { mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)"); diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index fc095bff89..8e78825c34 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -284,9 +284,9 @@ HMC5883::HMC5883(int bus) : _next_report(0), _oldest_report(0), _reports(nullptr), - _mag_topic(-1), _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), + _mag_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")) @@ -950,7 +950,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - if (OK != ioctl(filp, MAGIOCEXSTRAP, 0)) { + if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { warnx("failed to disable sensor calibration mode"); goto out; } @@ -969,9 +969,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) out: if (ret == OK) { - warnx("calibration successfully finished."); + warnx("mag scale calibration successfully finished."); } else { - warnx("calibration failed."); + warnx("mag scale calibration failed."); } return ret; } @@ -1200,7 +1200,6 @@ test() */ int calibrate() { - ssize_t sz; int ret; int fd = open(MAG_DEVICE_PATH, O_RDONLY); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 293bbe00c0..fd02eb3631 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1,7 +1,7 @@ - /**************************************************************************** +/**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ /** * @file mavlink.c * MAVLink 1.0 protocol implementation. + * + * @author Lorenz Meier */ #include @@ -114,9 +116,6 @@ mavlink_wpm_storage *wpm = &wpm_s; bool mavlink_hil_enabled = false; -/* buffer for message strings */ -static char mavlink_message_string[51] = {0}; - /* protocol interface */ static int uart; static int baudrate; @@ -127,6 +126,8 @@ static enum { MAVLINK_INTERFACE_MODE_ONBOARD } mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; +static struct mavlink_logbuffer lb; + static void mavlink_update_system(void); static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); static void usage(void); @@ -332,7 +333,9 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; - strncpy(mavlink_message_string, txt, 51); + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + mavlink_logbuffer_write(&lb, &msg); total_counter++; return OK; } @@ -489,6 +492,9 @@ void mavlink_update_system(void) */ int mavlink_thread_main(int argc, char *argv[]) { + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&lb, 5); + int ch; char *device_name = "/dev/ttyS1"; baudrate = 57600; @@ -662,9 +668,12 @@ int mavlink_thread_main(int argc, char *argv[]) usleep(10000); /* send one string at 10 Hz */ - if (mavlink_message_string[0] != '\0') { - mavlink_missionlib_send_gcs_string(mavlink_message_string); - mavlink_message_string[0] = '\0'; + if (!mavlink_logbuffer_is_empty(&lb)) { + struct mavlink_logmessage msg; + int lb_ret = mavlink_logbuffer_read(&lb, &msg); + if (lb_ret == OK) { + mavlink_missionlib_send_gcs_string(msg.text); + } } /* sleep 15 ms */ @@ -711,7 +720,7 @@ int mavlink_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, - argv); + (const char**)argv); exit(0); } diff --git a/apps/mavlink/mavlink_log.c b/apps/mavlink/mavlink_log.c new file mode 100644 index 0000000000..660e282f86 --- /dev/null +++ b/apps/mavlink/mavlink_log.c @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_log.c + * MAVLink text logging. + * + * @author Lorenz Meier + */ + +#include + +#include "mavlink_log.h" + +void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { + lb->size = size; + lb->start = 0; + lb->count = 0; + lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); +} + +int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { + return lb->count == lb->size; +} + +int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) { + return lb->count == 0; +} + +void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) { + int end = (lb->start + lb->count) % lb->size; + memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); + if (mavlink_logbuffer_is_full(lb)) { + lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ + } else { + ++lb->count; + } +} + +int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) { + if (!mavlink_logbuffer_is_empty(lb)) { + memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); + lb->start = (lb->start + 1) % lb->size; + --lb->count; + return 0; + } else { + return 1; + } +} \ No newline at end of file diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h index 1ad1364be9..cb6fd9d98c 100644 --- a/apps/mavlink/mavlink_log.h +++ b/apps/mavlink/mavlink_log.h @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ /** * @file mavlink_log.h * MAVLink text logging. + * + * @author Lorenz Meier */ #ifndef MAVLINK_LOG @@ -79,5 +81,28 @@ */ #define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +struct mavlink_logmessage { + char text[51]; + unsigned char severity; +}; + +struct mavlink_logbuffer { + unsigned int start; + // unsigned int end; + unsigned int size; + int count; + struct mavlink_logmessage *elems; +}; + +void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); + +int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); + +int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); + +void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem); + +int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); + #endif