Commit e119bbb0 authored by Lorenz Meier's avatar Lorenz Meier
Browse files

A lot more on calibration and RC checks. Needs more testing, but no known issues

parent 8df6acbf
......@@ -182,6 +182,12 @@ fi
# Try to get an USB console
nshterm /dev/ttyACM0 &
# Start any custom extensions that might be missing
if [ -f /fs/microsd/etc/rc.local ]
then
sh /fs/microsd/etc/rc.local
fi
# If none of the autostart scripts triggered, get a minimal setup
if [ $MODE == autostart ]
then
......
......@@ -136,6 +136,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
int do_accel_calibration(int mavlink_fd) {
/* announce change */
mavlink_log_info(mavlink_fd, "accel calibration started");
mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
/* measure and calculate offsets & scales */
float accel_offs[3];
......@@ -211,17 +212,28 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
}
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
unsigned done_count = 0;
while (true) {
bool done = true;
char str[80];
char str[60];
int str_ptr;
str_ptr = sprintf(str, "keep vehicle still:");
unsigned old_done_count = done_count;
done_count = 0;
for (int i = 0; i < 6; i++) {
if (!data_collected[i]) {
str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]);
done = false;
} else {
done_count++;
}
}
if (old_done_count != done_count)
mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count);
if (done)
break;
mavlink_log_info(mavlink_fd, str);
......@@ -236,8 +248,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
continue;
}
sprintf(str, "meas started: %s", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, str);
// sprintf(str,
mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[orient][0],
......
......@@ -84,6 +84,7 @@
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/cpuload.h>
#include <systemlib/rc_check.h>
#include "px4_custom_mode.h"
#include "commander_helper.h"
......@@ -617,6 +618,8 @@ int commander_thread_main(int argc, char *argv[])
bool updated = false;
bool rc_calibration_ok = (OK == rc_calibration_check());
/* Subscribe to safety topic */
int safety_sub = orb_subscribe(ORB_ID(safety));
memset(&safety, 0, sizeof(safety));
......@@ -727,6 +730,9 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_system_id, &(status.system_id));
param_get(_param_component_id, &(status.component_id));
status_changed = true;
/* Re-check RC calibration */
rc_calibration_ok = (OK == rc_calibration_check());
}
}
......
......@@ -100,6 +100,8 @@ int do_mag_calibration(int mavlink_fd)
close(fd);
mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
/* calibrate offsets */
// uint64_t calibration_start = hrt_absolute_time();
......@@ -135,9 +137,8 @@ int do_mag_calibration(int mavlink_fd)
axis_index++;
char buf[50];
sprintf(buf, "please rotate around %c", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter);
tune_neutral();
axis_deadline += calibration_interval / 3;
......@@ -251,6 +252,8 @@ int do_mag_calibration(int mavlink_fd)
warnx("Setting Z mag scale failed!\n");
}
mavlink_log_info(mavlink_fd, "mag cal progress <90> percent");
/* auto-save to EEPROM */
int save_ret = param_save_default();
......@@ -274,6 +277,7 @@ int do_mag_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "mag calibration done");
mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
return OK;
/* third beep by cal end routine */
......
......@@ -157,7 +157,6 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000);
PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
......@@ -184,10 +183,7 @@ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */
PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */
PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */
PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */
PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f);
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f);
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f);
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
......@@ -305,8 +305,6 @@ private:
int board_rotation;
int external_mag_rotation;
int rc_type;
int rc_map_roll;
int rc_map_pitch;
int rc_map_yaw;
......@@ -342,9 +340,6 @@ private:
param_t max[_rc_max_chan_count];
param_t rev[_rc_max_chan_count];
param_t dz[_rc_max_chan_count];
param_t rc_type;
param_t rc_demix;
param_t gyro_offset[3];
param_t gyro_scale[3];
......@@ -566,8 +561,6 @@ Sensors::Sensors() :
}
_parameter_handles.rc_type = param_find("RC_TYPE");
/* mandatory input switched, mapped to channels 1-4 per default */
_parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL");
_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
......@@ -692,11 +685,6 @@ Sensors::parameters_update()
if (!rc_valid)
warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
/* remote control type */
if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) {
warnx("Failed getting remote control type");
}
/* channel mapping */
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
warnx("Failed getting roll chan index");
......@@ -738,26 +726,11 @@ Sensors::parameters_update()
// warnx("Failed getting offboard control mode sw chan index");
// }
if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
warnx("Failed getting mode aux 1 index");
}
if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) {
warnx("Failed getting mode aux 2 index");
}
if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) {
warnx("Failed getting mode aux 3 index");
}
if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) {
warnx("Failed getting mode aux 4 index");
}
if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) {
warnx("Failed getting mode aux 5 index");
}
param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2));
param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
......
......@@ -48,4 +48,5 @@ SRCS = err.c \
systemlib.c \
airspeed.c \
system_params.c \
mavlink_log.c
mavlink_log.c \
rc_check.c
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* 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 rc_check.c
*
* RC calibration check
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <fcntl.h>
#include <systemlib/rc_check.h>
#include <systemlib/param/param.h>
#include <mavlink/mavlink_log.h>
#include <uORB/topics/rc_channels.h>
int rc_calibration_check(void) {
char nbuf[20];
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
_parameter_handles_rev, _parameter_handles_dz;
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
float param_min, param_max, param_trim, param_rev, param_dz;
/* first check channel mappings */
/* check which map param applies */
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
// /* give system time to flush error message in case there are more */
// usleep(100000);
// count++;
// }
for (int i = 0; i < RC_CHANNELS_MAX; i++) {
/* should the channel be enabled? */
uint8_t count = 0;
/* min values */
sprintf(nbuf, "RC%d_MIN", i + 1);
_parameter_handles_min = param_find(nbuf);
param_get(_parameter_handles_min, &param_min);
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
_parameter_handles_trim = param_find(nbuf);
param_get(_parameter_handles_trim, &param_trim);
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
_parameter_handles_max = param_find(nbuf);
param_get(_parameter_handles_max, &param_max);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
_parameter_handles_rev = param_find(nbuf);
param_get(_parameter_handles_rev, &param_rev);
/* channel deadzone */
sprintf(nbuf, "RC%d_DZ", i + 1);
_parameter_handles_dz = param_find(nbuf);
param_get(_parameter_handles_dz, &param_dz);
/* assert min..center..max ordering */
if (param_min < 500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_max > 2500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim < param_min) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim > param_max) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > 500) {
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
count++;
}
/* check which map param applies */
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
// /* give system time to flush error message in case there are more */
// usleep(100000);
// count++;
// }
/* sanity checks pass, enable channel */
if (count) {
mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
usleep(100000);
}
}
}
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* 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 rc_check.h
*
* RC calibration check
*/
#pragma once
__BEGIN_DECLS
/**
* Check the RC calibration
*
* @return 0 / OK if RC calibration is ok, index + 1 of the first
* channel that failed else (so 1 == first channel failed)
*/
__EXPORT int rc_calibration_check(void);
__END_DECLS
......@@ -57,6 +57,7 @@
#include <drivers/drv_baro.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/rc_check.h>
__EXPORT int preflight_check_main(int argc, char *argv[]);
static int led_toggle(int leds, int led);
......@@ -139,101 +140,7 @@ int preflight_check_main(int argc, char *argv[])
/* ---- RC CALIBRATION ---- */
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
_parameter_handles_rev, _parameter_handles_dz;
float param_min, param_max, param_trim, param_rev, param_dz;
bool rc_ok = true;
char nbuf[20];
/* first check channel mappings */
/* check which map param applies */
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
// /* give system time to flush error message in case there are more */
// usleep(100000);
// count++;
// }
for (int i = 0; i < 12; i++) {
/* should the channel be enabled? */
uint8_t count = 0;
/* min values */
sprintf(nbuf, "RC%d_MIN", i + 1);
_parameter_handles_min = param_find(nbuf);
param_get(_parameter_handles_min, &param_min);
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
_parameter_handles_trim = param_find(nbuf);
param_get(_parameter_handles_trim, &param_trim);
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
_parameter_handles_max = param_find(nbuf);
param_get(_parameter_handles_max, &param_max);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
_parameter_handles_rev = param_find(nbuf);
param_get(_parameter_handles_rev, &param_rev);
/* channel deadzone */
sprintf(nbuf, "RC%d_DZ", i + 1);
_parameter_handles_dz = param_find(nbuf);
param_get(_parameter_handles_dz, &param_dz);
/* assert min..center..max ordering */
if (param_min < 500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_max > 2500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim < param_min) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim > param_max) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > 500) {
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
count++;
}
/* check which map param applies */
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
// /* give system time to flush error message in case there are more */
// usleep(100000);
// count++;
// }
/* sanity checks pass, enable channel */
if (count) {
mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
usleep(100000);
rc_ok = false;
}
}
bool rc_ok = (OK == rc_calibration_check());
/* require RC ok to keep system_ok */
system_ok &= rc_ok;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment