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

Merge commit 'bb5819a13fa8c46daf2e61a58c78a13232ffcd99' into multirotor

This commit is contained in:
Anton Babushkin 2013-08-25 19:33:36 +02:00
parent 557d3f22de bb5819a13f
commit 7ab129ba92
15 changed files with 296 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
Makefile
View File

@ -114,13 +114,26 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:
@$(ECHO) %%%%
@$(ECHO) %%%% Building $(config) in $(work_dir)
@$(ECHO) %%%%
$(Q) mkdir -p $(work_dir)
$(Q) make -r -C $(work_dir) \
$(Q) $(MKDIR) -p $(work_dir)
$(Q) $(MAKE) -r -C $(work_dir) \
-f $(PX4_MK_DIR)firmware.mk \
CONFIG=$(config) \
WORK_DIR=$(work_dir) \
$(FIRMWARE_GOAL)
#
# Make FMU firmwares depend on pre-packaged IO binaries.
#
# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency
# and forces the _default config in all cases. There has to be a better way to do this...
#
FMU_VERSION = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1))))
define FMU_DEP
$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4
endef
FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS))
$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config))))
#
# Build the NuttX export archives.
#
@ -147,12 +160,12 @@ $(ARCHIVE_DIR)%.export: configuration = nsh
$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC)
@$(ECHO) %% Configuring NuttX for $(board)
$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .)
$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration))
@$(ECHO) %% Exporting NuttX for $(board)
$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
$(Q) mkdir -p $(dir $@)
$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
$(Q) $(MKDIR) -p $(dir $@)
$(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@
$(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board))
@ -168,11 +181,11 @@ BOARD = $(BOARDS)
menuconfig: $(NUTTX_SRC)
@$(ECHO) %% Configuring NuttX for $(BOARD)
$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .)
$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh)
@$(ECHO) %% Running menuconfig for $(BOARD)
$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
$(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
@$(ECHO) %% Saving configuration file
$(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig
else
@ -191,7 +204,7 @@ $(NUTTX_SRC):
# Testing targets
#
testbuild:
$(Q) (cd $(PX4_BASE) && make distclean && make archives && make -j8)
$(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8)
#
# Cleanup targets. 'clean' should remove all built products and force
@ -206,8 +219,8 @@ clean:
.PHONY: distclean
distclean: clean
$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
$(Q) (cd $(NUTTX_SRC)/configs && find . -maxdepth 1 -type l -delete)
$(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean
$(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete)
#
# Print some help text
@ -229,9 +242,9 @@ help:
@$(ECHO) " A limited set of configs can be built with CONFIGS=<list-of-configs>"
@$(ECHO) ""
@for config in $(CONFIGS); do \
echo " $$config"; \
echo " Build just the $$config firmware configuration."; \
echo ""; \
$(ECHO) " $$config"; \
$(ECHO) " Build just the $$config firmware configuration."; \
$(ECHO) ""; \
done
@$(ECHO) " clean"
@$(ECHO) " Remove all firmware build pieces."

6
ROMFS/px4fmu_common/init.d/rcS
View File

@ -186,6 +186,12 @@ then
# 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

1
makefiles/config_px4fmu-v1_default.mk
View File

@ -6,6 +6,7 @@
# Use the configuration's ROMFS.
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
#
# Board support modules

4
makefiles/config_px4fmu-v2_default.mk
View File

@ -3,9 +3,11 @@
#
#
# Use the configuration's ROMFS.
# Use the configuration's ROMFS, copy the px4iov2 firmware into
# the ROMFS if it's available
#
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin
#
# Board support modules

22
makefiles/firmware.mk
View File

@ -322,7 +322,7 @@ endif
# a root from several templates. That would be a nice feature.
#
# Add dependencies on anything in the ROMFS root
# Add dependencies on anything in the ROMFS root directory
ROMFS_FILES += $(wildcard \
$(ROMFS_ROOT)/* \
$(ROMFS_ROOT)/*/* \
@ -334,7 +334,14 @@ ifeq ($(ROMFS_FILES),)
$(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files)
endif
ROMFS_DEPS += $(ROMFS_FILES)
# Extra files that may be copied into the ROMFS /extras directory
# ROMFS_EXTRA_FILES are required, ROMFS_OPTIONAL_FILES are optional
ROMFS_EXTRA_FILES += $(wildcard $(ROMFS_OPTIONAL_FILES))
ROMFS_DEPS += $(ROMFS_EXTRA_FILES)
ROMFS_IMG = romfs.img
ROMFS_SCRATCH = romfs_scratch
ROMFS_CSRC = $(ROMFS_IMG:.img=.c)
ROMFS_OBJ = $(ROMFS_CSRC:.c=.o)
LIBS += $(ROMFS_OBJ)
@ -345,9 +352,18 @@ $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS)
$(call BIN_TO_OBJ,$<,$@,romfs_img)
# Generate the ROMFS image from the root
$(ROMFS_IMG): $(ROMFS_DEPS) $(GLOBAL_DEPS)
$(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS)
@$(ECHO) "ROMFS: $@"
$(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol"
$(Q) $(GENROMFS) -f $@ -d $(ROMFS_SCRATCH) -V "NSHInitVol"
# Construct the ROMFS scratch root from the canonical root
$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS)
$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)
$(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH)
ifneq ($(ROMFS_EXTRA_FILES),)
$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras
$(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras
endif
EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG)

1
makefiles/setup.mk
View File

@ -73,6 +73,7 @@ export RMDIR = rm -rf
export GENROMFS = genromfs
export TOUCH = touch
export MKDIR = mkdir
export FIND = find
export ECHO = echo
export UNZIP_CMD = unzip
export PYTHON = python

18
src/modules/commander/accelerometer_calibration.cpp
View File

@ -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],

6
src/modules/commander/commander.cpp
View File

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

10
src/modules/commander/mag_calibration.cpp
View File

@ -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 */

10
src/modules/sensors/sensor_params.c
View File

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

37
src/modules/sensors/sensors.cpp
View File

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

3
src/modules/systemlib/module.mk
View File

@ -48,4 +48,5 @@ SRCS = err.c \
systemlib.c \
airspeed.c \
system_params.c \
mavlink_log.c
mavlink_log.c \
rc_check.c

148
src/modules/systemlib/rc_check.c Normal file
View File

@ -0,0 +1,148 @@
/****************************************************************************
*
* 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);
}
}
}

52
src/modules/systemlib/rc_check.h Normal file
View File

@ -0,0 +1,52 @@
/****************************************************************************
*
* 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

97
src/systemcmds/preflight_check/preflight_check.c
View File

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