dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Cleaned up calibration, added text messages ring buffer

This commit is contained in:
Lorenz Meier 2012-10-22 08:14:43 +02:00
parent 096bf2dc93
commit df8148033a
5 changed files with 163 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
apps/commander/commander.c
View File

@ -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);
int fd = open(MAG_DEVICE_PATH, 0);
// 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, 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)");

9
apps/drivers/hmc5883/hmc5883.cpp
View File

@ -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);

29
apps/mavlink/mavlink.c
View File

@ -1,7 +1,7 @@
/****************************************************************************
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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 <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
@ -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);
}

80
apps/mavlink/mavlink_log.c Normal file
View File

@ -0,0 +1,80 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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 <lm@inf.ethz.ch>
*/
#include <string.h>
#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;
}
}

29
apps/mavlink/mavlink_log.h
View File

@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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 <lm@inf.ethz.ch>
*/
#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