From 114b7b696d54d031c899db4ffb91c125204fbba2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 14 Oct 2013 11:14:56 +0200 Subject: sdlog2: VER message added instead of FWRV --- src/modules/sdlog2/sdlog2.c | 35 +++++++++++++++++--- src/modules/sdlog2/sdlog2_messages.h | 24 ++++---------- src/modules/sdlog2/sdlog2_version.h | 62 ++++++++++++++++++++++++++++++++++++ 3 files changed, 99 insertions(+), 22 deletions(-) create mode 100644 src/modules/sdlog2/sdlog2_version.h diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec8033202..edb21c7ab 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -90,6 +90,7 @@ #include "logbuffer.h" #include "sdlog2_format.h" #include "sdlog2_messages.h" +#include "sdlog2_version.h" #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ @@ -182,6 +183,10 @@ static void sdlog2_stop_log(void); */ static void write_formats(int fd); +/** + * Write version message to log file. + */ +static void write_version(int fd); static bool file_exist(const char *filename); @@ -359,6 +364,9 @@ static void *logwriter_thread(void *arg) /* write log messages formats */ write_formats(log_file); + /* write version */ + write_version(log_file); + int poll_count = 0; void *read_ptr; @@ -487,14 +495,13 @@ void sdlog2_stop_log() sdlog2_status(); } - void write_formats(int fd) { /* construct message format packet */ struct { LOG_PACKET_HEADER; struct log_format_s body; - } log_format_packet = { + } log_msg_format = { LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), }; @@ -502,13 +509,33 @@ void write_formats(int fd) int i; for (i = 0; i < log_formats_num; i++) { - log_format_packet.body = log_formats[i]; - log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet)); + log_msg_format.body = log_formats[i]; + log_bytes_written += write(fd, &log_msg_format, sizeof(log_msg_format)); } fsync(fd); } +void write_version(int fd) +{ + /* construct version message */ + struct { + LOG_PACKET_HEADER; + struct log_VER_s body; + } log_msg_VER = { + LOG_PACKET_HEADER_INIT(127), + }; + + /* fill message format packet for each format and write to log */ + int i; + + strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git)); + strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); + log_bytes_written += write(fd, &log_msg_VER, sizeof(log_msg_VER)); + + fsync(fd); +} + int sdlog2_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0e88da054..9f709e459 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -253,28 +253,16 @@ struct log_GVSP_s { float vz; }; -/* --- FWRV - FIRMWARE REVISION --- */ -#define LOG_FWRV_MSG 20 -struct log_FWRV_s { - char fw_revision[64]; +/* --- VER - VERSION --- */ +#define LOG_VER_MSG 127 +struct log_VER_s { + char arch[16]; + char fw_git[64]; }; #pragma pack(pop) - -/* - GIT_VERSION is defined at build time via a Makefile call to the - git command line. We create a fake log message format for - the firmware revision "FWRV" that is written to every log - header. This makes it easier to determine which version - of the firmware was running when a log was created. - */ -#define FREEZE_STR(s) #s -#define STRINGIFY(s) FREEZE_STR(s) -#define FW_VERSION_STR STRINGIFY(GIT_VERSION) - /* construct list of all message formats */ - static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), @@ -295,7 +283,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), - LOG_FORMAT(FWRV,"Z",FW_VERSION_STR), + LOG_FORMAT(VER, "NZ", "Arch,FwGit"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); diff --git a/src/modules/sdlog2/sdlog2_version.h b/src/modules/sdlog2/sdlog2_version.h new file mode 100644 index 000000000..c6a9ba638 --- /dev/null +++ b/src/modules/sdlog2/sdlog2_version.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 sdlog2_version.h + * + * Tools for system version detection. + * + * @author Anton Babushkin + */ + +#ifndef SDLOG2_VERSION_H_ +#define SDLOG2_VERSION_H_ + +/* + GIT_VERSION is defined at build time via a Makefile call to the + git command line. + */ +#define FREEZE_STR(s) #s +#define STRINGIFY(s) FREEZE_STR(s) +#define FW_GIT STRINGIFY(GIT_VERSION) + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#define HW_ARCH "PX4FMU_V1" +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#define HW_ARCH "PX4FMU_V2" +#endif + +#endif /* SDLOG2_VERSION_H_ */ -- cgit v1.2.3 From 6d406968eaba0924fe48945fd8a71b6f8a0adc1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Oct 2013 11:11:15 +0200 Subject: Added hexrotor support --- ROMFS/px4fmu_common/init.d/12-13_hex | 95 ++++++++++++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 14 ++++++ 2 files changed, 109 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/12-13_hex diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex new file mode 100644 index 000000000..0f0bb05ce --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12-13_hex @@ -0,0 +1,95 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on Hex" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ +# 13 = hexarotor +# +param set MAV_TYPE 13 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 900 900 + px4io min 1200 1200 1200 1200 1200 1200 + px4io max 1900 1900 1900 1900 1900 1900 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +mixer load /dev/pwm_output $MIXER + +# +# Set PWM output frequency to 400 Hz +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 93e76184d..ced875001 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -177,6 +177,20 @@ then sh /etc/init.d/11_dji_f450 set MODE custom fi + + if param compare SYS_AUTOSTART 12 + then + set MIXER /etc/mixers/FMU_hex_x.mix + sh /etc/init.d/12-13_hex + set MODE custom + fi + + if param compare SYS_AUTOSTART 13 + then + set MIXER /etc/mixers/FMU_hex_+.mix + sh /etc/init.d/12-13_hex + set MODE custom + fi if param compare SYS_AUTOSTART 15 then -- cgit v1.2.3 From 233a068a7b6eb4851aa47b80a1852851bc851d73 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 17:54:57 +0200 Subject: quad hil + rotor configuration startup script --- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 110 ++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 40 ++++++---- 2 files changed, 135 insertions(+), 15 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil new file mode 100644 index 000000000..1c85e04f2 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -0,0 +1,110 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILS quadrotor + starting.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.0 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.05 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 3.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.05 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.5 + param set MPC_THR_MIN 0.1 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ced875001..fd588017f 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -110,23 +110,33 @@ then then sh /etc/init.d/1000_rc_fw.hil set MODE custom - else - if param compare SYS_AUTOSTART 1001 - then - sh /etc/init.d/1001_rc_quad.hil - set MODE custom - else - if param compare SYS_AUTOSTART 1002 - then - sh /etc/init.d/1002_rc_fw_state.hil - set MODE custom - else - # Try to get an USB console - nshterm /dev/ttyACM0 & - fi - fi fi + if param compare SYS_AUTOSTART 1001 + then + sh /etc/init.d/1001_rc_quad.hil + set MODE custom + fi + + if param compare SYS_AUTOSTART 1002 + then + sh /etc/init.d/1002_rc_fw_state.hil + set MODE custom + fi + + if param compare SYS_AUTOSTART 1003 + then + sh /etc/init.d/1003_rc_quad_+.hil + set MODE custom + fi + + if [ $MODE != custom ] + then + # Try to get an USB console + nshterm /dev/ttyACM0 & + fi + + # # Upgrade PX4IO firmware # -- cgit v1.2.3 From 5cd675d8cce6a3c35eab1970cfb8668b0a6b24e5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 19 Oct 2013 11:42:06 +0200 Subject: The mavlink baudrate was too high in the custom_io_esc startup script --- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc index e645d9d54..2bfaed76c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -58,7 +58,7 @@ usleep 10000 if px4io detect then # Start MAVLink (depends on orb) - mavlink start -d /dev/ttyS1 -b 115200 + mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 sh /etc/init.d/rc.io @@ -78,7 +78,7 @@ then else fmu mode_pwm # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS1 -b 115200 + mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes -- cgit v1.2.3 From dbb49c035bb85876b234fc057d9f244b43453a44 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Oct 2013 15:19:42 +1100 Subject: rgbled: fixed detection of device on PX4v1 There is a serial EEPROM on the PX4IOv1 board that answers on I2C address 0x55. We need some extra I2C transfers to ensure we are talking to a real RGBLED device. --- src/drivers/rgbled/rgbled.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index ea87b37d9..34a45c739 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -172,7 +172,20 @@ RGBLED::probe() bool on, powersave; uint8_t r, g, b; - ret = get(on, powersave, r, g, b); + /** + this may look strange, but is needed. There is a serial + EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to + a bunch of I2C addresses, including the 0x55 used by this + LED device. So we need to do enough operations to be sure + we are talking to the right device. These 3 operations seem + to be enough, as the 3rd one consistently fails if no + RGBLED is on the bus. + */ + if ((ret=get(on, powersave, r, g, b)) != OK || + (ret=send_led_enable(false) != OK) || + (ret=send_led_enable(false) != OK)) { + return ret; + } return ret; } -- cgit v1.2.3 From e3fe4437204b2aecdaa1131fcb4bb0c7751a43df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Oct 2013 15:21:02 +1100 Subject: rgbled: fixed getopt() handling this allows the -a option to be used, for example rgbled -a 0x55 start --- src/drivers/rgbled/rgbled.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 34a45c739..d49211b7b 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -574,7 +574,7 @@ rgbled_main(int argc, char *argv[]) int ch; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) { + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { switch (ch) { case 'a': rgbledadr = strtol(optarg, NULL, 0); @@ -590,7 +590,12 @@ rgbled_main(int argc, char *argv[]) } } - const char *verb = argv[1]; + if (optind >= argc) { + rgbled_usage(); + exit(1); + } + + const char *verb = argv[optind]; int fd; int ret; -- cgit v1.2.3 From 14e2464fabbae27f9655efdb3e5ee55479c469f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Oct 2013 15:21:46 +1100 Subject: rgbled: don't try the same bus twice on PX4v1 the external I2C bus is the same as the LED bus --- src/drivers/rgbled/rgbled.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index d49211b7b..727c86e02 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -616,6 +616,9 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + errx(1, "init failed"); + } i2cdevice = PX4_I2C_BUS_LED; } } -- cgit v1.2.3 From 6a624ff753ad9ba6e7fb74783b6b8294a1c17957 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sat, 19 Oct 2013 23:04:36 +0200 Subject: Fix gyro calibration for rotated sensors. The calibration routine now uses the raw sensor values instead of the already rotated values. --- src/modules/commander/gyro_calibration.cpp | 178 +++++++++++++---------------- 1 file changed, 78 insertions(+), 100 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 369e6da62..b6de5141f 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -60,17 +60,7 @@ int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); - const unsigned calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - unsigned calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { + struct gyro_scale gyro_scale = { 0.0f, 1.0f, 0.0f, @@ -79,27 +69,40 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, }; - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + struct gyro_report gyro_report; + + /* reset all offsets to zero and all scales to one */ + int fd = open(GYRO_DEVICE_PATH, 0); + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to reset scale / offsets for gyro"); close(fd); + + /*** --- OFFSETS --- ***/ + + /* determine gyro mean values */ + const unsigned calibration_count = 5000; + unsigned calibration_counter = 0; unsigned poll_errcount = 0; while (calibration_counter < calibration_count) { /* wait blocking for new data */ struct pollfd fds[1]; - fds[0].fd = sub_sensor_combined; + fds[0].fd = sub_sensor_gyro; fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + gyro_scale.x_offset += gyro_report.x; + gyro_scale.y_offset += gyro_report.y; + gyro_scale.z_offset += gyro_report.z; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); @@ -110,67 +113,49 @@ int do_gyro_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); - close(sub_sensor_combined); + close(sub_sensor_gyro); return ERROR; } } - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; - - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warnx("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, "gyro store failed"); - close(sub_sensor_combined); - return ERROR; - } - - tune_neutral(); - /* third beep by cal end routine */ + gyro_scale.x_offset /= calibration_count; + gyro_scale.y_offset /= calibration_count; + gyro_scale.z_offset /= calibration_count; - } else { - mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); - close(sub_sensor_combined); + if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { + mavlink_log_info(mavlink_fd, "gyro offset calibration FAILED (NaN)"); + close(sub_sensor_gyro); return ERROR; } + /* beep on calibration end */ mavlink_log_info(mavlink_fd, "offset calibration done."); + tune_neutral(); + + /* set offset parameters to new values */ + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offset parameters failed!"); + } + -#if 0 /*** --- SCALING --- ***/ +#if 0 + /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); + /* apply new offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to apply new offsets for gyro"); + + close(fd); + + unsigned rotations_count = 30; float gyro_integral = 0.0f; float baseline_integral = 0.0f; @@ -246,52 +231,45 @@ int do_gyro_calibration(int mavlink_fd) warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); -#else - float gyro_scales[] = { 1.0f, 1.0f, 1.0f }; -#endif - - - if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { + if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { + mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); + close(sub_sensor_gyro); + return ERROR; + } - if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0])) - || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1])) - || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!"); - } + /* beep on calibration end */ + mavlink_log_info(mavlink_fd, "scale calibration done."); + tune_neutral(); - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - gyro_scales[0], - gyro_offset[1], - gyro_scales[1], - gyro_offset[2], - gyro_scales[2], - }; +#endif - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); + /* set scale parameters to new values */ + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "Setting gyro scale parameters failed!"); + } - close(fd); + /* apply new scaling and offsets */ + fd = open(GYRO_DEVICE_PATH, 0); - /* auto-save to EEPROM */ - int save_ret = param_save_default(); + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to apply new scale for gyro"); - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } + close(fd); - mavlink_log_info(mavlink_fd, "gyro calibration done"); + /* auto-save to EEPROM */ + int save_ret = param_save_default(); - /* third beep by cal end routine */ - close(sub_sensor_combined); - return OK; - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - close(sub_sensor_combined); + if (save_ret != 0) { + warnx("WARNING: auto-save of params to storage failed"); + mavlink_log_critical(mavlink_fd, "gyro store failed"); + close(sub_sensor_gyro); return ERROR; } + + close(sub_sensor_gyro); + return OK; } -- cgit v1.2.3 From 8cffd2b8a33ccb4d556a181d5a6e55111c1b1f53 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 12:16:00 +0200 Subject: fix scaling (unit) of airspeed in HIL src/modules/mavlink/mavlink_receiver.cpp --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8243748dc..c51a6de08 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -422,13 +422,13 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_counter = hil_counter; /* differential pressure */ - hil_sensors.differential_pressure_pa = imu.diff_pressure; + hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa hil_sensors.differential_pressure_counter = hil_counter; /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; - float ias = calc_indicated_airspeed(imu.diff_pressure); + float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa); // XXX need to fix this float tas = ias; -- cgit v1.2.3 From ef6f1f6f808e49ff3aca68aa08001e37093b89ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Oct 2013 19:36:42 +0200 Subject: get_rot_matrix() moved to separate library, accel calibration of rotated board fixed --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/lib/conversion/module.mk | 38 +++++++++ src/lib/conversion/rotation.cpp | 30 +++++++ src/lib/conversion/rotation.h | 89 +++++++++++++++++++++ .../commander/accelerometer_calibration.cpp | 45 ++++++++--- src/modules/commander/module.mk | 1 - src/modules/sensors/sensors.cpp | 93 +--------------------- 8 files changed, 192 insertions(+), 106 deletions(-) create mode 100644 src/lib/conversion/module.mk create mode 100644 src/lib/conversion/rotation.cpp create mode 100644 src/lib/conversion/rotation.h diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 46640f3c5..862abde92 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -115,6 +115,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/conversion # # Demo apps diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bb589cb9f..71986c3a3 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -111,6 +111,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/conversion # # Demo apps diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk new file mode 100644 index 000000000..102711aaf --- /dev/null +++ b/src/lib/conversion/module.mk @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Rotation library +# + +SRCS = rotation.cpp diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp new file mode 100644 index 000000000..b65226cf1 --- /dev/null +++ b/src/lib/conversion/rotation.cpp @@ -0,0 +1,30 @@ +/* + * rotation.cpp + * + * Created on: 20.10.2013 + * Author: ton + */ + +#include "math.h" +#include "rotation.h" + +__EXPORT void +get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +{ + /* first set to zero */ + rot_matrix->Matrix::zero(3, 3); + + float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; + float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; + float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; + + math::EulerAngles euler(roll, pitch, yaw); + + math::Dcm R(euler); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + (*rot_matrix)(i, j) = R(i, j); + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h new file mode 100644 index 000000000..97c6a0907 --- /dev/null +++ b/src/lib/conversion/rotation.h @@ -0,0 +1,89 @@ +/* + * rotation.h + * + * Created on: 20.10.2013 + * Author: ton + */ + +#ifndef ROTATION_H_ +#define ROTATION_H_ + +#include +#include + +/** + * Enum for board and external compass rotations. + * This enum maps from board attitude to airframe attitude. + */ +enum Rotation { + ROTATION_NONE = 0, + ROTATION_YAW_45 = 1, + ROTATION_YAW_90 = 2, + ROTATION_YAW_135 = 3, + ROTATION_YAW_180 = 4, + ROTATION_YAW_225 = 5, + ROTATION_YAW_270 = 6, + ROTATION_YAW_315 = 7, + ROTATION_ROLL_180 = 8, + ROTATION_ROLL_180_YAW_45 = 9, + ROTATION_ROLL_180_YAW_90 = 10, + ROTATION_ROLL_180_YAW_135 = 11, + ROTATION_PITCH_180 = 12, + ROTATION_ROLL_180_YAW_225 = 13, + ROTATION_ROLL_180_YAW_270 = 14, + ROTATION_ROLL_180_YAW_315 = 15, + ROTATION_ROLL_90 = 16, + ROTATION_ROLL_90_YAW_45 = 17, + ROTATION_ROLL_90_YAW_90 = 18, + ROTATION_ROLL_90_YAW_135 = 19, + ROTATION_ROLL_270 = 20, + ROTATION_ROLL_270_YAW_45 = 21, + ROTATION_ROLL_270_YAW_90 = 22, + ROTATION_ROLL_270_YAW_135 = 23, + ROTATION_PITCH_90 = 24, + ROTATION_PITCH_270 = 25, + ROTATION_MAX +}; + +typedef struct { + uint16_t roll; + uint16_t pitch; + uint16_t yaw; +} rot_lookup_t; + +const rot_lookup_t rot_lookup[] = { + { 0, 0, 0 }, + { 0, 0, 45 }, + { 0, 0, 90 }, + { 0, 0, 135 }, + { 0, 0, 180 }, + { 0, 0, 225 }, + { 0, 0, 270 }, + { 0, 0, 315 }, + {180, 0, 0 }, + {180, 0, 45 }, + {180, 0, 90 }, + {180, 0, 135 }, + { 0, 180, 0 }, + {180, 0, 225 }, + {180, 0, 270 }, + {180, 0, 315 }, + { 90, 0, 0 }, + { 90, 0, 45 }, + { 90, 0, 90 }, + { 90, 0, 135 }, + {270, 0, 0 }, + {270, 0, 45 }, + {270, 0, 90 }, + {270, 0, 135 }, + { 0, 90, 0 }, + { 0, 270, 0 } +}; + +/** + * Get the rotation matrix + */ +__EXPORT void +get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); + +#endif /* ROTATION_H_ */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index cfa7d9e8a..c9bfedbda 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -112,11 +112,13 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -144,24 +146,41 @@ int do_accel_calibration(int mavlink_fd) { int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); if (res == OK) { - /* measurements complete successfully, set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { + /* measurements complete successfully, rotate calibration values */ + param_t board_rotation_h = param_find("SENS_BOARD_ROT"); + enum Rotation board_rotation_id; + param_get(board_rotation_h, &(board_rotation_id)); + math::Matrix board_rotation(3, 3); + get_rot_matrix(board_rotation_id, &board_rotation); + board_rotation = board_rotation.transpose(); + math::Vector3 vect(3); + vect(0) = accel_offs[0]; + vect(1) = accel_offs[1]; + vect(2) = accel_offs[2]; + math::Vector3 accel_offs_rotated = board_rotation * vect; + vect(0) = accel_scale[0]; + vect(1) = accel_scale[1]; + vect(2) = accel_scale[2]; + math::Vector3 accel_scale_rotated = board_rotation * vect; + + /* set parameters */ + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale_rotated(0))) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale_rotated(1))) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale_rotated(2)))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { - accel_offs[0], - accel_scale[0], - accel_offs[1], - accel_scale[1], - accel_offs[2], - accel_scale[2], + accel_offs_rotated(0), + accel_scale_rotated(0), + accel_offs_rotated(1), + accel_scale_rotated(1), + accel_offs_rotated(2), + accel_scale_rotated(2), }; if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 91d75121e..554dfcb08 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -47,4 +47,3 @@ SRCS = commander.cpp \ baro_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp - diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fcacaf8f..1b79de8fd 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include @@ -135,75 +136,6 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) -/** - * Enum for board and external compass rotations. - * This enum maps from board attitude to airframe attitude. - */ -enum Rotation { - ROTATION_NONE = 0, - ROTATION_YAW_45 = 1, - ROTATION_YAW_90 = 2, - ROTATION_YAW_135 = 3, - ROTATION_YAW_180 = 4, - ROTATION_YAW_225 = 5, - ROTATION_YAW_270 = 6, - ROTATION_YAW_315 = 7, - ROTATION_ROLL_180 = 8, - ROTATION_ROLL_180_YAW_45 = 9, - ROTATION_ROLL_180_YAW_90 = 10, - ROTATION_ROLL_180_YAW_135 = 11, - ROTATION_PITCH_180 = 12, - ROTATION_ROLL_180_YAW_225 = 13, - ROTATION_ROLL_180_YAW_270 = 14, - ROTATION_ROLL_180_YAW_315 = 15, - ROTATION_ROLL_90 = 16, - ROTATION_ROLL_90_YAW_45 = 17, - ROTATION_ROLL_90_YAW_90 = 18, - ROTATION_ROLL_90_YAW_135 = 19, - ROTATION_ROLL_270 = 20, - ROTATION_ROLL_270_YAW_45 = 21, - ROTATION_ROLL_270_YAW_90 = 22, - ROTATION_ROLL_270_YAW_135 = 23, - ROTATION_PITCH_90 = 24, - ROTATION_PITCH_270 = 25, - ROTATION_MAX -}; - -typedef struct { - uint16_t roll; - uint16_t pitch; - uint16_t yaw; -} rot_lookup_t; - -const rot_lookup_t rot_lookup[] = { - { 0, 0, 0 }, - { 0, 0, 45 }, - { 0, 0, 90 }, - { 0, 0, 135 }, - { 0, 0, 180 }, - { 0, 0, 225 }, - { 0, 0, 270 }, - { 0, 0, 315 }, - {180, 0, 0 }, - {180, 0, 45 }, - {180, 0, 90 }, - {180, 0, 135 }, - { 0, 180, 0 }, - {180, 0, 225 }, - {180, 0, 270 }, - {180, 0, 315 }, - { 90, 0, 0 }, - { 90, 0, 45 }, - { 90, 0, 90 }, - { 90, 0, 135 }, - {270, 0, 0 }, - {270, 0, 45 }, - {270, 0, 90 }, - {270, 0, 135 }, - { 0, 90, 0 }, - { 0, 270, 0 } -}; - /** * Sensor app start / stop handling function * @@ -384,11 +316,6 @@ private: */ int parameters_update(); - /** - * Get the rotation matrices - */ - void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); - /** * Do accel-related initialisation. */ @@ -803,24 +730,6 @@ Sensors::parameters_update() return OK; } -void -Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) -{ - /* first set to zero */ - rot_matrix->Matrix::zero(3, 3); - - float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; - float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; - float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; - - math::EulerAngles euler(roll, pitch, yaw); - - math::Dcm R(euler); - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - (*rot_matrix)(i, j) = R(i, j); -} - void Sensors::accel_init() { -- cgit v1.2.3 From b75c8e672fb401d9b1673d53a1972b9dddfa6b15 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Oct 2013 23:16:23 +0200 Subject: accelerometer calibration fix --- .../commander/accelerometer_calibration.cpp | 45 ++++++++-------------- 1 file changed, 17 insertions(+), 28 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index c9bfedbda..b6aa64aa4 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -129,7 +129,7 @@ #endif static const int ERROR = -1; -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); int mat_invert3(float src[3][3], float dst[3][3]); @@ -142,8 +142,8 @@ int do_accel_calibration(int mavlink_fd) { /* measure and calculate offsets & scales */ float accel_offs[3]; - float accel_scale[3]; - int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); + float accel_T[3][3]; + int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); if (res == OK) { /* measurements complete successfully, rotate calibration values */ @@ -153,34 +153,31 @@ int do_accel_calibration(int mavlink_fd) { math::Matrix board_rotation(3, 3); get_rot_matrix(board_rotation_id, &board_rotation); board_rotation = board_rotation.transpose(); - math::Vector3 vect(3); - vect(0) = accel_offs[0]; - vect(1) = accel_offs[1]; - vect(2) = accel_offs[2]; - math::Vector3 accel_offs_rotated = board_rotation * vect; - vect(0) = accel_scale[0]; - vect(1) = accel_scale[1]; - vect(2) = accel_scale[2]; - math::Vector3 accel_scale_rotated = board_rotation * vect; + math::Vector3 accel_offs_vec; + accel_offs_vec.set(&accel_offs[0]); + math::Vector3 accel_offs_rotated = board_rotation * accel_offs_vec; + math::Matrix accel_T_mat(3, 3); + accel_T_mat.set(&accel_T[0][0]); + math::Matrix accel_T_rotated = board_rotation.transpose() * accel_T_mat * board_rotation; /* set parameters */ if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale_rotated(0))) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale_rotated(1))) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale_rotated(2)))) { + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { accel_offs_rotated(0), - accel_scale_rotated(0), + accel_T_rotated(0, 0), accel_offs_rotated(1), - accel_scale_rotated(1), + accel_T_rotated(1, 1), accel_offs_rotated(2), - accel_scale_rotated(2), + accel_T_rotated(2, 2), }; if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) @@ -206,7 +203,7 @@ int do_accel_calibration(int mavlink_fd) { /* exit accel calibration mode */ } -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) { const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; @@ -282,21 +279,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } close(sensor_combined_sub); - /* calculate offsets and rotation+scale matrix */ - float accel_T[3][3]; + /* calculate offsets and transform matrix */ int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); return ERROR; } - /* convert accel transform matrix to scales, - * rotation part of transform matrix is not used by now - */ - for (int i = 0; i < 3; i++) { - accel_scale[i] = accel_T[i][i]; - } - return OK; } -- cgit v1.2.3 From 0dc9c9ac262c10866cbaf23ca98c8ce4582b5993 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Oct 2013 23:28:09 +0200 Subject: accelerometer_calibration: code style fixed, lib/conversion copyright fix --- src/lib/conversion/module.mk | 2 +- src/lib/conversion/rotation.cpp | 40 ++++++- src/lib/conversion/rotation.h | 40 ++++++- .../commander/accelerometer_calibration.cpp | 122 +++++++++++++-------- 4 files changed, 149 insertions(+), 55 deletions(-) diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk index 102711aaf..f5f59a2dc 100644 --- a/src/lib/conversion/module.mk +++ b/src/lib/conversion/module.mk @@ -32,7 +32,7 @@ ############################################################################ # -# Rotation library +# Conversion library # SRCS = rotation.cpp diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index b65226cf1..b078562c2 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -1,8 +1,40 @@ -/* - * rotation.cpp +/**************************************************************************** * - * Created on: 20.10.2013 - * Author: ton + * 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 rotation.cpp + * + * Vector rotation library */ #include "math.h" diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 97c6a0907..85c63c0fc 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -1,8 +1,40 @@ -/* - * rotation.h +/**************************************************************************** * - * Created on: 20.10.2013 - * Author: ton + * 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 rotation.h + * + * Vector rotation library */ #ifndef ROTATION_H_ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index b6aa64aa4..4880af907 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -135,7 +135,8 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp int mat_invert3(float src[3][3], float dst[3][3]); int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); -int do_accel_calibration(int mavlink_fd) { +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"); @@ -162,11 +163,11 @@ int do_accel_calibration(int mavlink_fd) { /* set parameters */ if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } @@ -194,6 +195,7 @@ int do_accel_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "accel calibration done"); return OK; + } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); @@ -203,7 +205,8 @@ int do_accel_calibration(int mavlink_fd) { /* exit accel calibration mode */ } -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) { +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) +{ const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; @@ -243,12 +246,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", - (!data_collected[0]) ? "x+ " : "", - (!data_collected[1]) ? "x- " : "", - (!data_collected[2]) ? "y+ " : "", - (!data_collected[3]) ? "y- " : "", - (!data_collected[4]) ? "z+ " : "", - (!data_collected[5]) ? "z- " : ""); + (!data_collected[0]) ? "x+ " : "", + (!data_collected[1]) ? "x- " : "", + (!data_collected[2]) ? "y+ " : "", + (!data_collected[3]) ? "y- " : "", + (!data_collected[4]) ? "z+ " : "", + (!data_collected[5]) ? "z- " : ""); if (old_done_count != done_count) mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); @@ -257,6 +260,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; int orient = detect_orientation(mavlink_fd, sensor_combined_sub); + if (orient < 0) { close(sensor_combined_sub); return ERROR; @@ -270,17 +274,19 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], - (double)accel_ref[orient][0], - (double)accel_ref[orient][1], - (double)accel_ref[orient][2]); + (double)accel_ref[orient][0], + (double)accel_ref[orient][1], + (double)accel_ref[orient][2]); data_collected[orient] = true; tune_neutral(); } + close(sensor_combined_sub); /* calculate offsets and transform matrix */ int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + if (res != 0) { mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); return ERROR; @@ -295,7 +301,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float * @return 0..5 according to orientation when vehicle is still and ready for measurements, * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 */ -int detect_orientation(int mavlink_fd, int sub_sensor_combined) { +int detect_orientation(int mavlink_fd, int sub_sensor_combined) +{ struct sensor_combined_s sensor; /* exponential moving average of accel */ float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; @@ -326,30 +333,35 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { while (true) { /* wait blocking for new data */ int poll_ret = poll(fds, 1, 1000); + if (poll_ret) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); t = hrt_absolute_time(); float dt = (t - t_prev) / 1000000.0f; t_prev = t; float w = dt / ema_len; + for (int i = 0; i < 3; i++) { accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); + if (d > accel_disp[i]) accel_disp[i] = d; } + /* still detector with hysteresis */ - if ( accel_disp[0] < still_thr2 && - accel_disp[1] < still_thr2 && - accel_disp[2] < still_thr2 ) { + if (accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2) { /* is still now */ if (t_still == 0) { /* first time */ mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); t_still = t; t_timeout = t + timeout; + } else { /* still since t_still */ if (t > t_still + still_time) { @@ -357,18 +369,21 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { break; } } - } else if ( accel_disp[0] > still_thr2 * 2.0f || - accel_disp[1] > still_thr2 * 2.0f || - accel_disp[2] > still_thr2 * 2.0f) { + + } else if (accel_disp[0] > still_thr2 * 2.0f || + accel_disp[1] > still_thr2 * 2.0f || + accel_disp[2] > still_thr2 * 2.0f) { /* not still, reset still start time */ if (t_still != 0) { mavlink_log_info(mavlink_fd, "detected motion, please hold still..."); t_still = 0; } } + } else if (poll_ret == 0) { poll_errcount++; } + if (t > t_timeout) { poll_errcount++; } @@ -379,29 +394,34 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } - if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 0; // [ g, 0, 0 ] - if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 1; // [ -g, 0, 0 ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 2; // [ 0, g, 0 ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 3; // [ 0, -g, 0 ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) return 4; // [ 0, 0, g ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) return 5; // [ 0, 0, -g ] mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); @@ -412,7 +432,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* * Read specified number of accelerometer samples, calculate average and dispersion. */ -int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) +{ struct pollfd fds[1]; fds[0].fd = sensor_combined_sub; fds[0].events = POLLIN; @@ -423,12 +444,16 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp while (count < samples_num) { int poll_ret = poll(fds, 1, 1000); + if (poll_ret == 1) { struct sensor_combined_s sensor; orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + for (int i = 0; i < 3; i++) accel_sum[i] += sensor.accelerometer_m_s2[i]; + count++; + } else { errcount++; continue; @@ -445,10 +470,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp return OK; } -int mat_invert3(float src[3][3], float dst[3][3]) { +int mat_invert3(float src[3][3], float dst[3][3]) +{ float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + - src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + if (det == 0.0f) return ERROR; // Singular matrix @@ -465,7 +492,8 @@ int mat_invert3(float src[3][3], float dst[3][3]) { return OK; } -int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) +{ /* calculate offsets */ for (int i = 0; i < 3; i++) { accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; @@ -474,6 +502,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* fill matrix A for linear equations system*/ float mat_A[3][3]; memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { float a = accel_ref[i * 2][j] - accel_offs[j]; @@ -483,6 +512,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* calculate inverse matrix for A */ float mat_A_inv[3][3]; + if (mat_invert3(mat_A, mat_A_inv) != OK) return ERROR; -- cgit v1.2.3 From ed79b686c57f41d9d6bd3726fe0071e11740b3d7 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sun, 20 Oct 2013 00:08:36 +0200 Subject: Adjusted mavlink info messages during gyro calibration to not break QGroundControl. --- src/modules/commander/gyro_calibration.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index b6de5141f..e1d6e8340 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -129,7 +129,7 @@ int do_gyro_calibration(int mavlink_fd) } /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "offset calibration done."); + mavlink_log_info(mavlink_fd, "gyro offset calibration done."); tune_neutral(); /* set offset parameters to new values */ @@ -240,7 +240,7 @@ int do_gyro_calibration(int mavlink_fd) } /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "scale calibration done."); + mavlink_log_info(mavlink_fd, "gyro scale calibration done."); tune_neutral(); #endif @@ -270,6 +270,8 @@ int do_gyro_calibration(int mavlink_fd) return ERROR; } + mavlink_log_info(mavlink_fd, "gyro calibration done."); + close(sub_sensor_gyro); return OK; } -- cgit v1.2.3 From ea89f23c917733f8a682c82e24e1e4f223f79843 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Oct 2013 20:07:47 +0200 Subject: calibration: bugs fixed, mavlink messages cleanup --- src/drivers/drv_accel.h | 2 +- .../commander/accelerometer_calibration.cpp | 181 ++++++++++-------- src/modules/commander/commander.cpp | 54 +++--- src/modules/commander/gyro_calibration.cpp | 211 ++++++++++++--------- 4 files changed, 252 insertions(+), 196 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index eff5e7349..8a4f68428 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -66,7 +66,7 @@ struct accel_report { int16_t temperature_raw; }; -/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */ +/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ struct accel_scale { float x_offset; float x_scale; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 4880af907..d11d7eb5d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -100,6 +100,24 @@ * accel_T = A^-1 * g * g = 9.80665 * + * ===== Rotation ===== + * + * Calibrating using model: + * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r) + * + * Actual correction: + * accel_corr = rot * accel_T * (accel_raw - accel_offs) + * + * Known: accel_T_r, accel_offs_r, rot + * Unknown: accel_T, accel_offs + * + * Solution: + * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs) + * => accel_T = rot^-1 * accel_T_r * rot + * => accel_offs = rot^-1 * accel_offs_r + * * @author Anton Babushkin */ @@ -137,72 +155,97 @@ 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"); + mavlink_log_info(mavlink_fd, "accel calibration: started"); + mavlink_log_info(mavlink_fd, "accel calibration: progress <0>"); + + struct accel_scale accel_scale = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + int res = OK; + + /* reset all offsets to zero and all scales to one */ + int fd = open(ACCEL_DEVICE_PATH, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + } /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_T[3][3]; - int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); if (res == OK) { - /* measurements complete successfully, rotate calibration values */ + /* measurements completed successfully, rotate calibration values */ param_t board_rotation_h = param_find("SENS_BOARD_ROT"); - enum Rotation board_rotation_id; - param_get(board_rotation_h, &(board_rotation_id)); + int32_t board_rotation_int; + param_get(board_rotation_h, &(board_rotation_int)); + enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; math::Matrix board_rotation(3, 3); get_rot_matrix(board_rotation_id, &board_rotation); - board_rotation = board_rotation.transpose(); + math::Matrix board_rotation_t = board_rotation.transpose(); math::Vector3 accel_offs_vec; accel_offs_vec.set(&accel_offs[0]); - math::Vector3 accel_offs_rotated = board_rotation * accel_offs_vec; + math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec; math::Matrix accel_T_mat(3, 3); accel_T_mat.set(&accel_T[0][0]); - math::Matrix accel_T_rotated = board_rotation.transpose() * accel_T_mat * board_rotation; + math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + + accel_scale.x_offset = accel_offs_rotated(0); + accel_scale.x_scale = accel_T_rotated(0, 0); + accel_scale.y_offset = accel_offs_rotated(1); + accel_scale.y_scale = accel_T_rotated(1, 1); + accel_scale.z_offset = accel_offs_rotated(2); + accel_scale.z_scale = accel_T_rotated(2, 2); /* set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset)) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset)) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset)) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale)) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale)) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting accel params failed"); + res = ERROR; } + } + if (res == OK) { + /* apply new scaling and offsets */ int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offs_rotated(0), - accel_T_rotated(0, 0), - accel_offs_rotated(1), - accel_T_rotated(1, 1), - accel_offs_rotated(2), - accel_T_rotated(2, 2), - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for accel"); + } + } + + if (res == OK) { /* auto-save to EEPROM */ - int save_ret = param_save_default(); + res = param_save_default(); - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); } + } - mavlink_log_info(mavlink_fd, "accel calibration done"); - return OK; + if (res == OK) { + mavlink_log_info(mavlink_fd, "accel calibration: done"); } else { - /* measurements error */ - mavlink_log_info(mavlink_fd, "accel calibration aborted"); - return ERROR; + mavlink_log_info(mavlink_fd, "accel calibration: failed"); } - /* exit accel calibration mode */ + return res; } int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) @@ -212,27 +255,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; - /* reset existing calibration */ - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); - close(fd); - - if (OK != ioctl_res) { - warn("ERROR: failed to set scale / offsets for accel"); - return ERROR; - } - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); unsigned done_count = 0; + int res = OK; while (true) { bool done = true; @@ -245,6 +271,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } } + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, "accel calibration: progress <%u>", 17 * done_count); + + if (done) + break; + mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", (!data_collected[1]) ? "x- " : "", @@ -253,17 +285,11 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float (!data_collected[4]) ? "z+ " : "", (!data_collected[5]) ? "z- " : ""); - if (old_done_count != done_count) - mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); - - if (done) - break; - int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { - close(sensor_combined_sub); - return ERROR; + res = ERROR; + break; } if (data_collected[orient]) { @@ -284,15 +310,16 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float close(sensor_combined_sub); - /* calculate offsets and transform matrix */ - int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + if (res == OK) { + /* calculate offsets and transform matrix */ + res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); - return ERROR; + if (res != OK) { + mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); + } } - return OK; + return res; } /* @@ -309,7 +336,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* max-hold dispersion of accel */ float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; /* EMA time constant in seconds*/ - float ema_len = 0.2f; + float ema_len = 0.5f; /* set "still" threshold to 0.25 m/s^2 */ float still_thr2 = pow(0.25f, 2); /* set accel error threshold to 5m/s^2 */ @@ -342,8 +369,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) float w = dt / ema_len; for (int i = 0; i < 3; i++) { - accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; - float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; + float d = sensor.accelerometer_m_s2[i] - accel_ema[i]; + accel_ema[i] += d * w; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); @@ -389,8 +416,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) } if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor"); - return -1; + mavlink_log_critical(mavlink_fd, "ERROR: failed reading sensor"); + return ERROR; } } @@ -424,9 +451,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) return 5; // [ 0, 0, -g ] - mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); - return -2; // Can't detect orientation + return ERROR; // Can't detect orientation } /* diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ef509980..9545ef171 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -369,8 +369,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (hil_ret == OK && control_mode->flag_system_hil_enabled) { /* reset the arming mode to disarmed */ arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + } else { mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } @@ -481,27 +483,28 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: - { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; - if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { - if (safety->safety_switch_available && !safety->safety_off) { - print_reject_arm("NOT ARMING: Press safety switch first."); - arming_res = TRANSITION_DENIED; + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + transition_result_t arming_res = TRANSITION_NOT_CHANGED; - } else { - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); - } + if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + } + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } } - } break; default: @@ -940,7 +943,7 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; /* check if board is connected via USB */ - struct stat statbuf; + //struct stat statbuf; //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } @@ -970,6 +973,7 @@ int commander_thread_main(int argc, char *argv[]) if (armed.armed) { arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + } else { arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); } @@ -1244,12 +1248,14 @@ int commander_thread_main(int argc, char *argv[]) counter++; int blink_state = blink_msg_state(); + if (blink_state > 0) { /* blinking LED message, don't touch LEDs */ if (blink_state == 2) { /* blinking LED message completed, restore normal state */ control_status_leds(&status, &armed, true); } + } else { /* normal state */ control_status_leds(&status, &armed, status_changed); @@ -1264,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[]) ret = pthread_join(commander_low_prio_thread, NULL); if (ret) { - warn("join failed", ret); + warn("join failed: %d", ret); } rgbled_set_mode(RGBLED_MODE_OFF); @@ -1308,6 +1314,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan /* driving rgbled */ if (changed) { bool set_normal_color = false; + /* set mode */ if (status->arming_state == ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); @@ -1332,6 +1339,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { rgbled_set_color(RGBLED_COLOR_AMBER); } + /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { @@ -1694,11 +1702,10 @@ void *commander_low_prio_loop(void *arg) fds[0].events = POLLIN; while (!thread_should_exit) { - - /* wait for up to 100ms for data */ + /* wait for up to 200ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); - /* timed out - periodic check for _task_should_exit, etc. */ + /* timed out - periodic check for thread_should_exit, etc. */ if (pret == 0) continue; @@ -1773,7 +1780,7 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_rc_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { @@ -1854,7 +1861,6 @@ void *commander_low_prio_loop(void *arg) /* send acknowledge command */ // XXX TODO } - } close(cmd_sub); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index e1d6e8340..219ae6cb2 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -58,7 +58,7 @@ static const int ERROR = -1; int do_gyro_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); + mavlink_log_info(mavlink_fd, "gyro calibration: started"); struct gyro_scale gyro_scale = { 0.0f, @@ -69,79 +69,85 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, }; - /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); - struct gyro_report gyro_report; + int res = OK; /* reset all offsets to zero and all scales to one */ int fd = open(GYRO_DEVICE_PATH, 0); - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) - warn("WARNING: failed to reset scale / offsets for gyro"); - + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + } - /*** --- OFFSETS --- ***/ - - /* determine gyro mean values */ - const unsigned calibration_count = 5000; - unsigned calibration_counter = 0; - unsigned poll_errcount = 0; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_sensor_gyro; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + struct gyro_report gyro_report; - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); - gyro_scale.x_offset += gyro_report.x; - gyro_scale.y_offset += gyro_report.y; - gyro_scale.z_offset += gyro_report.z; - calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) - mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); - - } else { - poll_errcount++; + if (res == OK) { + /* determine gyro mean values */ + const unsigned calibration_count = 5000; + unsigned calibration_counter = 0; + unsigned poll_errcount = 0; + + while (calibration_counter < calibration_count) { + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_gyro; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + gyro_scale.x_offset += gyro_report.x; + gyro_scale.y_offset += gyro_report.y; + gyro_scale.z_offset += gyro_report.z; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, "gyro calibration: progress <%u>", (calibration_counter * 100) / calibration_count); + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, "ERROR: failed reading gyro sensor"); + res = ERROR; + break; + } } - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); - close(sub_sensor_gyro); - return ERROR; - } + gyro_scale.x_offset /= calibration_count; + gyro_scale.y_offset /= calibration_count; + gyro_scale.z_offset /= calibration_count; } - gyro_scale.x_offset /= calibration_count; - gyro_scale.y_offset /= calibration_count; - gyro_scale.z_offset /= calibration_count; - - if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { - mavlink_log_info(mavlink_fd, "gyro offset calibration FAILED (NaN)"); - close(sub_sensor_gyro); - return ERROR; + if (res == OK) { + /* check offsets */ + if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { + mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); + res = ERROR; + } } - /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "gyro offset calibration done."); - tune_neutral(); - - /* set offset parameters to new values */ - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offset parameters failed!"); + if (res == OK) { + /* set offset parameters to new values */ + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting gyro offs params failed"); + res = ERROR; + } } - - /*** --- SCALING --- ***/ #if 0 + /* beep on offset calibration end */ + mavlink_log_info(mavlink_fd, "gyro offset calibration done"); + tune_neutral(); + + /* scale calibration */ /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); @@ -163,9 +169,11 @@ int do_gyro_calibration(int mavlink_fd) // XXX change to mag topic orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; - if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; + float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); + + if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F; + + if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F; uint64_t last_time = hrt_absolute_time(); @@ -175,7 +183,7 @@ int do_gyro_calibration(int mavlink_fd) /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) { + && (hrt_absolute_time() - start_time > 5 * 1e6)) { mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); close(sub_sensor_combined); return OK; @@ -203,14 +211,17 @@ int do_gyro_calibration(int mavlink_fd) // calculate error between estimate and measurement // apply declination correction for true heading as well. //float mag = -atan2f(magNav(1),magNav(0)); - float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag > M_PI_F) mag -= 2*M_PI_F; - if (mag < -M_PI_F) mag += 2*M_PI_F; + float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); + + if (mag > M_PI_F) mag -= 2 * M_PI_F; + + if (mag < -M_PI_F) mag += 2 * M_PI_F; float diff = mag - mag_last; - if (diff > M_PI_F) diff -= 2*M_PI_F; - if (diff < -M_PI_F) diff += 2*M_PI_F; + if (diff > M_PI_F) diff -= 2 * M_PI_F; + + if (diff < -M_PI_F) diff += 2 * M_PI_F; baseline_integral += diff; mag_last = mag; @@ -220,15 +231,15 @@ int do_gyro_calibration(int mavlink_fd) // warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); - // } else if (poll_ret == 0) { - // /* any poll failure for 1s is a reason to abort */ - // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - // return; + // } else if (poll_ret == 0) { + // /* any poll failure for 1s is a reason to abort */ + // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + // return; } } float gyro_scale = baseline_integral / gyro_integral; - + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); @@ -236,42 +247,54 @@ int do_gyro_calibration(int mavlink_fd) if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); close(sub_sensor_gyro); + mavlink_log_critical(mavlink_fd, "gyro calibration failed"); return ERROR; } /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "gyro scale calibration done."); + mavlink_log_info(mavlink_fd, "gyro scale calibration done"); tune_neutral(); #endif - /* set scale parameters to new values */ - if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) - || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) - || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "Setting gyro scale parameters failed!"); - } + close(sub_sensor_gyro); - /* apply new scaling and offsets */ - fd = open(GYRO_DEVICE_PATH, 0); + if (res == OK) { + /* set scale parameters to new values */ + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting gyro scale params failed"); + res = ERROR; + } + } - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) - warn("WARNING: failed to apply new scale for gyro"); + if (res == OK) { + /* apply new scaling and offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + close(fd); - close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for gyro"); + } + } - /* auto-save to EEPROM */ - int save_ret = param_save_default(); + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); - if (save_ret != 0) { - warnx("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, "gyro store failed"); - close(sub_sensor_gyro); - return ERROR; + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + } } - mavlink_log_info(mavlink_fd, "gyro calibration done."); + if (res == OK) { + mavlink_log_info(mavlink_fd, "gyro calibration: done"); - close(sub_sensor_gyro); - return OK; + } else { + mavlink_log_info(mavlink_fd, "gyro calibration: failed"); + } + + return res; } -- cgit v1.2.3 From ef42ef15c6991800111b25374b0f6e3935c2a9a9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Oct 2013 22:24:59 +0200 Subject: accel/gyro/mag calibration: big cleanup, use common messages --- .../commander/accelerometer_calibration.cpp | 40 +-- src/modules/commander/calibration_messages.h | 57 +++++ src/modules/commander/gyro_calibration.cpp | 37 +-- src/modules/commander/mag_calibration.cpp | 276 ++++++++++----------- 4 files changed, 239 insertions(+), 171 deletions(-) create mode 100644 src/modules/commander/calibration_messages.h diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d11d7eb5d..49a8d6b33 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -122,6 +122,7 @@ */ #include "accelerometer_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include @@ -147,6 +148,8 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "accel"; + int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); @@ -155,8 +158,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "accel calibration: started"); - mavlink_log_info(mavlink_fd, "accel calibration: progress <0>"); + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); struct accel_scale accel_scale = { 0.0f, @@ -175,7 +177,7 @@ int do_accel_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } /* measure and calculate offsets & scales */ @@ -213,7 +215,7 @@ int do_accel_calibration(int mavlink_fd) || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale)) || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale)) || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting accel params failed"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); res = ERROR; } } @@ -225,7 +227,7 @@ int do_accel_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for accel"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } @@ -234,15 +236,15 @@ int do_accel_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } } if (res == OK) { - mavlink_log_info(mavlink_fd, "accel calibration: done"); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); } else { - mavlink_log_info(mavlink_fd, "accel calibration: failed"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } return res; @@ -266,13 +268,16 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float done_count = 0; for (int i = 0; i < 6; i++) { - if (!data_collected[i]) { + if (data_collected[i]) { + done_count++; + + } else { done = false; } } if (old_done_count != done_count) - mavlink_log_info(mavlink_fd, "accel calibration: progress <%u>", 17 * done_count); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); if (done) break; @@ -293,7 +298,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } if (data_collected[orient]) { - mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); continue; } @@ -374,6 +379,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); + if (d > still_thr2 * 8.0f) + d = still_thr2 * 8.0f; + if (d > accel_disp[i]) accel_disp[i] = d; } @@ -397,12 +405,12 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) } } - } else if (accel_disp[0] > still_thr2 * 2.0f || - accel_disp[1] > still_thr2 * 2.0f || - accel_disp[2] > still_thr2 * 2.0f) { + } else if (accel_disp[0] > still_thr2 * 4.0f || + accel_disp[1] > still_thr2 * 4.0f || + accel_disp[2] > still_thr2 * 4.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "detected motion, please hold still..."); + mavlink_log_info(mavlink_fd, "detected motion, hold still..."); t_still = 0; } } @@ -416,7 +424,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) } if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, "ERROR: failed reading sensor"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); return ERROR; } } diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h new file mode 100644 index 000000000..fd8b8564d --- /dev/null +++ b/src/modules/commander/calibration_messages.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 calibration_messages.h + * + * Common calibration messages. + * + * @author Anton Babushkin + */ + +#ifndef CALIBRATION_MESSAGES_H_ +#define CALIBRATION_MESSAGES_H_ + +#define CAL_STARTED_MSG "%s calibration: started" +#define CAL_DONE_MSG "%s calibration: done" +#define CAL_FAILED_MSG "%s calibration: failed" +#define CAL_PROGRESS_MSG "%s calibration: progress <%u>" + +#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor" +#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration" +#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration" +#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters" +#define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters" + +#endif /* CALIBRATION_MESSAGES_H_ */ diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 219ae6cb2..30cd0d48d 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -33,10 +33,12 @@ /** * @file gyro_calibration.cpp + * * Gyroscope calibration routine */ #include "gyro_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include @@ -56,9 +58,12 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "gyro"; + int do_gyro_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "gyro calibration: started"); + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); struct gyro_scale gyro_scale = { 0.0f, @@ -77,19 +82,19 @@ int do_gyro_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } - /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); - struct gyro_report gyro_report; - if (res == OK) { /* determine gyro mean values */ const unsigned calibration_count = 5000; unsigned calibration_counter = 0; unsigned poll_errcount = 0; + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + struct gyro_report gyro_report; + while (calibration_counter < calibration_count) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -106,19 +111,21 @@ int do_gyro_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) - mavlink_log_info(mavlink_fd, "gyro calibration: progress <%u>", (calibration_counter * 100) / calibration_count); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); } else { poll_errcount++; } if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, "ERROR: failed reading gyro sensor"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); res = ERROR; break; } } + close(sub_sensor_gyro); + gyro_scale.x_offset /= calibration_count; gyro_scale.y_offset /= calibration_count; gyro_scale.z_offset /= calibration_count; @@ -137,7 +144,7 @@ int do_gyro_calibration(int mavlink_fd) if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting gyro offs params failed"); + mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); res = ERROR; } } @@ -257,14 +264,12 @@ int do_gyro_calibration(int mavlink_fd) #endif - close(sub_sensor_gyro); - if (res == OK) { /* set scale parameters to new values */ if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting gyro scale params failed"); + mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); res = ERROR; } } @@ -276,7 +281,7 @@ int do_gyro_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for gyro"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } @@ -285,15 +290,15 @@ int do_gyro_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } } if (res == OK) { - mavlink_log_info(mavlink_fd, "gyro calibration: done"); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); } else { - mavlink_log_info(mavlink_fd, "gyro calibration: failed"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } return res; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index b0d4266be..09f4104f8 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -33,12 +33,14 @@ /** * @file mag_calibration.cpp + * * Magnetometer calibration routine */ #include "mag_calibration.h" #include "commander_helper.h" #include "calibration_routines.h" +#include "calibration_messages.h" #include #include @@ -59,26 +61,20 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "mag"; + int do_mag_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait."); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); /* 45 seconds */ uint64_t calibration_interval = 45 * 1000 * 1000; - /* maximum 2000 values */ + /* maximum 500 values */ const unsigned int calibration_maxcount = 500; unsigned int calibration_counter = 0; - /* 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, O_RDONLY); - - /* erase old calibration */ struct mag_scale mscale_null = { 0.0f, 1.0f, @@ -88,97 +84,92 @@ int do_mag_calibration(int mavlink_fd) 1.0f, }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } + int res = OK; - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } + /* erase old calibration */ + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); - close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + } - mavlink_log_info(mavlink_fd, "mag cal progress <20> percent"); + if (res == OK) { + /* calibrate range */ + res = ioctl(fd, MAGIOCCALIBRATE, fd); - /* calibrate offsets */ + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale"); + } + } - // uint64_t calibration_start = hrt_absolute_time(); + close(fd); - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + float *x; + float *y; + float *z; - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; + if (res == OK) { + /* allocate memory */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + x = (float *)malloc(sizeof(float) * calibration_maxcount); + y = (float *)malloc(sizeof(float) * calibration_maxcount); + z = (float *)malloc(sizeof(float) * calibration_maxcount); - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return ERROR; + if (x == NULL || y == NULL || z == NULL) { + mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); + res = ERROR; + } } - mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting.."); + if (res == OK) { + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; - unsigned poll_errcount = 0; + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { + /* calibrate offsets */ + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + unsigned poll_errcount = 0; - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; + mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { - axis_index++; + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; - mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); - tune_neutral(); + int poll_ret = poll(fds, 1, 1000); - axis_deadline += calibration_interval / 3; - } + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - if (!(axis_index < 3)) { - break; - } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; + calibration_counter++; - calibration_counter++; - if (calibration_counter % (calibration_maxcount / 20) == 0) - mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount); + if (calibration_counter % (calibration_maxcount / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } else { + poll_errcount++; + } - } else { - poll_errcount++; - } - - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); - close(sub_mag); - free(x); - free(y); - free(z); - return ERROR; + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } } - + close(sub_mag); } float sphere_x; @@ -186,93 +177,100 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; - mavlink_log_info(mavlink_fd, "mag cal progress <70> percent"); - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - mavlink_log_info(mavlink_fd, "mag cal progress <80> percent"); + if (res == OK) { + /* sphere fit */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80); - free(x); - free(y); - free(z); + if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) { + mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit"); + res = ERROR; + } + } - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + if (x != NULL) + free(x); - fd = open(MAG_DEVICE_PATH, 0); + if (y != NULL) + free(y); - struct mag_scale mscale; + if (z != NULL) + free(z); - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); + if (res == OK) { + /* apply calibration and set parameters */ + struct mag_scale mscale; - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; + fd = open(MAG_DEVICE_PATH, 0); + res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale); - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration"); + } - close(fd); + if (res == OK) { + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; - /* announce and set new offset */ + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale); - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } } - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } + close(fd); - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } + if (res == OK) { + /* set parameters */ + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) + res = ERROR; - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) + res = ERROR; - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) + res = ERROR; - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) + res = ERROR; - mavlink_log_info(mavlink_fd, "mag cal progress <90> percent"); + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) + res = ERROR; - /* auto-save to EEPROM */ - int save_ret = param_save_default(); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + } - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - close(sub_mag); - return ERROR; + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90); } - warnx("\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); + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); - 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); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + } + } - 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, "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, "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, "magnetometer calibration completed"); - mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - close(sub_mag); - return OK; - /* third beep by cal end routine */ + } else { + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + } - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - close(sub_mag); - return ERROR; + return res; } } -- cgit v1.2.3 From 495073935e428d99833135bb227f3085d2def1cf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Oct 2013 23:33:01 +0200 Subject: accelerometer_calibration: stability fix --- src/modules/commander/accelerometer_calibration.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 49a8d6b33..5eeca5a1a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -180,10 +180,13 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } - /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_T[3][3]; - res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + + if (res == OK) { + /* measure and calculate offsets & scales */ + res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + } if (res == OK) { /* measurements completed successfully, rotate calibration values */ -- cgit v1.2.3 From 7f0ced968e5d518776930296ed870a47cc8c1756 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 21 Oct 2013 21:28:26 -0400 Subject: Working on roboclaw driver. --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/roboclaw/RoboClaw.cpp | 250 +++++++++++++++++++++++++++++++++ src/drivers/roboclaw/RoboClaw.hpp | 187 ++++++++++++++++++++++++ src/drivers/roboclaw/module.mk | 41 ++++++ src/drivers/roboclaw/roboclaw_main.cpp | 181 ++++++++++++++++++++++++ 6 files changed, 661 insertions(+), 1 deletion(-) create mode 100644 src/drivers/roboclaw/RoboClaw.cpp create mode 100644 src/drivers/roboclaw/RoboClaw.hpp create mode 100644 src/drivers/roboclaw/module.mk create mode 100644 src/drivers/roboclaw/roboclaw_main.cpp diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 46640f3c5..3cccc8447 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -33,7 +33,7 @@ MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl -MODULES += drivers/md25 +MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bb589cb9f..a2027ded9 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -31,6 +31,7 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm +MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp new file mode 100644 index 000000000..88b22e72a --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -0,0 +1,250 @@ +/**************************************************************************** + * + * 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 RoboClaw.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include "RoboClaw.hpp" +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +uint8_t RoboClaw::checksum_mask = 0x7f; + +RoboClaw::RoboClaw(const char *deviceName, uint16_t address): + _address(address), + _uart(0), + _controlPoll(), + _actuators(NULL, ORB_ID(actuator_controls_0), 20), + _motor1Position(0), + _motor1Speed(0), + _motor2Position(0), + _motor2Speed(0) +{ + // setup control polling + _controlPoll.fd = _actuators.getHandle(); + _controlPoll.events = POLLIN; + + // start serial port + _uart = open(deviceName, O_RDWR | O_NOCTTY); + struct termios uart_config; + tcgetattr(_uart, &uart_config); + uart_config.c_oflag &= ~ONLCR; // no CR for every LF + cfsetispeed(&uart_config, B38400); + cfsetospeed(&uart_config, B38400); + tcsetattr(_uart, TCSANOW, &uart_config); + + // setup default settings, reset encoders + resetEncoders(); +} + +RoboClaw::~RoboClaw() +{ + setMotorDutyCycle(MOTOR_1, 0.0); + setMotorDutyCycle(MOTOR_2, 0.0); + close(_uart); +} + +int RoboClaw::readEncoder(e_motor motor) +{ + uint16_t sum = 0; + if (motor == MOTOR_1) { + _sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum); + } else if (motor == MOTOR_2) { + _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); + } + uint8_t rbuf[6]; + int ret = read(_uart, rbuf, 6); + uint32_t count = 0; + uint8_t * countBytes = (uint8_t *)(&count); + countBytes[3] = rbuf[0]; + countBytes[2] = rbuf[1]; + countBytes[1] = rbuf[2]; + countBytes[0] = rbuf[3]; + uint8_t status = rbuf[4]; + if ((sum + _sumBytes(rbuf,5)) & + checksum_mask == rbuf[5]) return -1; + int overFlow = 0; + if (status & STATUS_UNDERFLOW) { + overFlow = -1; + } else if (status & STATUS_OVERFLOW) { + overFlow = +1; + } + if (motor == MOTOR_1) { + _motor1Overflow += overFlow; + } else if (motor == MOTOR_2) { + _motor2Overflow += overFlow; + } + return ret; +} + +void RoboClaw::status(char *string, size_t n) +{ + snprintf(string, n, + "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n" + "motor 2 position:\t%10.2f\tspeed:\t%10.2f\n", + double(getMotorPosition(MOTOR_1)), + double(getMotorSpeed(MOTOR_1)), + double(getMotorPosition(MOTOR_2)), + double(getMotorSpeed(MOTOR_2))); +} + +float RoboClaw::getMotorPosition(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Position; + } else if (motor == MOTOR_2) { + return _motor2Position; + } +} + +float RoboClaw::getMotorSpeed(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Speed; + } else if (motor == MOTOR_2) { + return _motor2Speed; + } +} + +int RoboClaw::setMotorSpeed(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + uint8_t speed = fabs(value)*127; + // send command + if (motor == MOTOR_1) { + if (value > 0) { + _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + } else { + _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + } + } else if (motor == MOTOR_2) { + if (value > 0) { + _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + } else { + _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + } + } +} + +int RoboClaw::setMotorDutyCycle(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + int16_t duty = 1500*value; + // send command + if (motor == MOTOR_1) { + _sendCommand(CMD_SIGNED_DUTYCYCLE_1, + (uint8_t *)(&duty), 2, sum); + } else if (motor == MOTOR_2) { + _sendCommand(CMD_SIGNED_DUTYCYCLE_2, + (uint8_t *)(&duty), 2, sum); + } +} + +void RoboClaw::update() +{ + // wait for an actuator publication, + // check for exit condition every second + // note "::poll" is required to distinguish global + // poll from member function for driver + if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error + + // if new data, send to motors + if (_actuators.updated()) { + _actuators.update(); + setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); + setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + } +} + +uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) +{ + uint16_t sum = 0; + for (size_t i=0;i +#include +#include +#include +#include + +/** + * This is a driver for the RoboClaw motor controller + */ +class RoboClaw +{ +public: + + /** control channels */ + enum e_channel { + CH_VOLTAGE_LEFT = 0, + CH_VOLTAGE_RIGHT + }; + + /** motors */ + enum e_motor { + MOTOR_1 = 0, + MOTOR_2 + }; + + /** + * constructor + * @param deviceName the name of the + * serial port e.g. "/dev/ttyS2" + * @param address the adddress of the motor + * (selectable on roboclaw) + */ + RoboClaw(const char *deviceName, uint16_t address); + + /** + * deconstructor + */ + virtual ~RoboClaw(); + + /** + * @return position of a motor, rev + */ + float getMotorPosition(e_motor motor); + + /** + * @return speed of a motor, rev/sec + */ + float getMotorSpeed(e_motor motor); + + /** + * set the speed of a motor, rev/sec + */ + int setMotorSpeed(e_motor motor, float value); + + /** + * set the duty cycle of a motor, rev/sec + */ + int setMotorDutyCycle(e_motor motor, float value); + + /** + * reset the encoders + * @return status + */ + int resetEncoders(); + + /** + * main update loop that updates RoboClaw motor + * dutycycle based on actuator publication + */ + void update(); + + /** + * read data from serial + */ + int readEncoder(e_motor motor); + + /** + * print status + */ + void status(char *string, size_t n); + +private: + + // Quadrature status flags + enum e_quadrature_status_flags { + STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/ + STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/ + STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/ + }; + + // commands + // We just list the commands we want from the manual here. + enum e_command { + + // simple + CMD_DRIVE_FWD_1 = 0, + CMD_DRIVE_REV_1 = 1, + CMD_DRIVE_FWD_2 = 4, + CMD_DRIVE_REV_2 = 5, + + // encoder commands + CMD_READ_ENCODER_1 = 16, + CMD_READ_ENCODER_2 = 17, + CMD_RESET_ENCODERS = 20, + + // advanced motor control + CMD_READ_SPEED_HIRES_1 = 30, + CMD_READ_SPEED_HIRES_2 = 31, + CMD_SIGNED_DUTYCYCLE_1 = 32, + CMD_SIGNED_DUTYCYCLE_2 = 33, + }; + + static uint8_t checksum_mask; + + uint16_t _address; + + int _uart; + + /** poll structure for control packets */ + struct pollfd _controlPoll; + + /** actuator controls subscription */ + control::UOrbSubscription _actuators; + + // private data + float _motor1Position; + float _motor1Speed; + int16_t _motor1Overflow; + + float _motor2Position; + float _motor2Speed; + int16_t _motor2Overflow; + + // private methods + uint16_t _sumBytes(uint8_t * buf, size_t n); + int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum); +}; + +// unit testing +int roboclawTest(const char *deviceName, uint8_t address); + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk new file mode 100644 index 000000000..1abecf198 --- /dev/null +++ b/src/drivers/roboclaw/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# RoboClaw Motor Controller +# + +MODULE_COMMAND = roboclaw + +SRCS = roboclaw_main.cpp \ + RoboClaw.cpp diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp new file mode 100644 index 000000000..a61c646fc --- /dev/null +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -0,0 +1,181 @@ +/**************************************************************************** + * + * 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 roboclaw_main.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include "RoboClaw.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int roboclaw_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(); + +static void usage() +{ + fprintf(stderr, "usage: roboclaw " + "{start|stop|status|test}\n\n"); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int roboclaw_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage(); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("roboclaw already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn_cmd("roboclaw", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + roboclaw_thread_main, + (const char **)argv); + exit(0); + + } else if (!strcmp(argv[1], "test")) { + + if (argc < 4) { + printf("usage: roboclaw test device address\n"); + exit(-1); + } + + const char *deviceName = argv[1]; + uint8_t address = strtoul(argv[2], nullptr, 0); + + roboclawTest(deviceName, address); + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "stop")) { + + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "status")) { + + if (thread_running) { + printf("\troboclaw app is running\n"); + + } else { + printf("\troboclaw app not started\n"); + } + exit(0); + } + + usage(); + exit(1); +} + +int roboclaw_thread_main(int argc, char *argv[]) +{ + printf("[roboclaw] starting\n"); + + // skip parent process args + argc -=2; + argv +=2; + + if (argc < 3) { + printf("usage: roboclaw start device address\n"); + return -1; + } + + const char *deviceName = argv[1]; + uint8_t address = strtoul(argv[2], nullptr, 0); + + // start + RoboClaw roboclaw(deviceName, address); + + thread_running = true; + + // loop + while (!thread_should_exit) { + roboclaw.update(); + } + + // exit + printf("[roboclaw] exiting.\n"); + thread_running = false; + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 -- cgit v1.2.3 From ce68f93867abfd3ea528809144f7c045d9bce544 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 21 Oct 2013 23:40:36 -0400 Subject: Debugging roboclaw comm. --- src/drivers/roboclaw/RoboClaw.cpp | 52 +++++++++++++++++++++++++++++----- src/drivers/roboclaw/RoboClaw.hpp | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 8 ++++-- 3 files changed, 52 insertions(+), 10 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 88b22e72a..aecc511ae 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -75,12 +75,18 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address): // start serial port _uart = open(deviceName, O_RDWR | O_NOCTTY); + if (_uart < 0) err(1, "could not open %s", deviceName); + int ret = 0; struct termios uart_config; - tcgetattr(_uart, &uart_config); + ret = tcgetattr(_uart, &uart_config); + if (ret < 0) err (1, "failed to get attr"); uart_config.c_oflag &= ~ONLCR; // no CR for every LF - cfsetispeed(&uart_config, B38400); - cfsetospeed(&uart_config, B38400); - tcsetattr(_uart, TCSANOW, &uart_config); + ret = cfsetispeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set input speed"); + ret = cfsetospeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set output speed"); + ret = tcsetattr(_uart, TCSANOW, &uart_config); + if (ret < 0) err (1, "failed to set attr"); // setup default settings, reset encoders resetEncoders(); @@ -110,23 +116,30 @@ int RoboClaw::readEncoder(e_motor motor) countBytes[1] = rbuf[2]; countBytes[0] = rbuf[3]; uint8_t status = rbuf[4]; - if ((sum + _sumBytes(rbuf,5)) & - checksum_mask == rbuf[5]) return -1; + if (((sum + _sumBytes(rbuf,5)) & checksum_mask) + == rbuf[5]) return -1; int overFlow = 0; if (status & STATUS_UNDERFLOW) { + printf("roboclaw: underflow\n"); overFlow = -1; } else if (status & STATUS_OVERFLOW) { + printf("roboclaw: overflow\n"); overFlow = +1; } + static int64_t overflowAmount = 0x100000000LL; if (motor == MOTOR_1) { _motor1Overflow += overFlow; + _motor1Position = count + + _motor1Overflow*_motor1Position; } else if (motor == MOTOR_2) { _motor2Overflow += overFlow; + _motor2Position = count + + _motor2Overflow*_motor2Position; } return ret; } -void RoboClaw::status(char *string, size_t n) +void RoboClaw::printStatus(char *string, size_t n) { snprintf(string, n, "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n" @@ -195,6 +208,13 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) } } +int RoboClaw::resetEncoders() +{ + uint16_t sum = 0; + return _sendCommand(CMD_RESET_ENCODERS, + nullptr, 0, sum); +} + void RoboClaw::update() { // wait for an actuator publication, @@ -214,9 +234,13 @@ void RoboClaw::update() uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) { uint16_t sum = 0; + printf("sum\n"); for (size_t i=0;i Date: Tue, 22 Oct 2013 05:04:13 -0400 Subject: Roboclaw encoders/ dutycycledrive complete. --- ROMFS/px4fmu_common/init.d/40_io_segway | 16 ++-- ROMFS/px4fmu_common/init.d/rcS | 6 ++ src/drivers/roboclaw/RoboClaw.cpp | 131 ++++++++++++++++++++++---------- src/drivers/roboclaw/RoboClaw.hpp | 11 ++- src/drivers/roboclaw/roboclaw_main.cpp | 28 ++++--- 5 files changed, 130 insertions(+), 62 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 4b7ed5286..ffe7f695b 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -21,8 +21,8 @@ param set MAV_TYPE 10 # # Start MAVLink (depends on orb) # -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 +#mavlink start -d /dev/ttyS1 -b 57600 +#usleep 5000 # # Start and configure PX4IO interface @@ -32,25 +32,25 @@ sh /etc/init.d/rc.io # # Start the commander (depends on orb, mavlink) # -commander start +#commander start # # Start the sensors (depends on orb, px4io) # -sh /etc/init.d/rc.sensors +#sh /etc/init.d/rc.sensors # # Start GPS interface (depends on orb) # -gps start +#gps start # # Start the attitude estimator (depends on orb) # -attitude_estimator_ekf start +#attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -md25 start 3 0x58 -segway start +roboclaw test /dev/ttyS2 128 +#segway start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index fd588017f..3a458286e 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -272,6 +272,12 @@ then sh /etc/init.d/30_io_camflyer set MODE custom fi + + if param compare SYS_AUTOSTART 40 + then + sh /etc/init.d/40_io_segway + set MODE custom + fi if param compare SYS_AUTOSTART 31 then diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index aecc511ae..3bbd7f48f 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -59,15 +59,19 @@ uint8_t RoboClaw::checksum_mask = 0x7f; -RoboClaw::RoboClaw(const char *deviceName, uint16_t address): +RoboClaw::RoboClaw(const char *deviceName, uint16_t address, + uint16_t pulsesPerRev): _address(address), + _pulsesPerRev(pulsesPerRev), _uart(0), _controlPoll(), _actuators(NULL, ORB_ID(actuator_controls_0), 20), _motor1Position(0), _motor1Speed(0), + _motor1Overflow(0), _motor2Position(0), - _motor2Speed(0) + _motor2Speed(0), + _motor2Overflow(0) { // setup control polling _controlPoll.fd = _actuators.getHandle(); @@ -90,6 +94,13 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address): // setup default settings, reset encoders resetEncoders(); + + // XXX roboclaw gives 128 as first several csums + // have to read a couple of messages first + for (int i=0;i<5;i++) { + if (readEncoder(MOTOR_1) > 0) break; + usleep(3000); + } } RoboClaw::~RoboClaw() @@ -107,8 +118,18 @@ int RoboClaw::readEncoder(e_motor motor) } else if (motor == MOTOR_2) { _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); } - uint8_t rbuf[6]; - int ret = read(_uart, rbuf, 6); + uint8_t rbuf[50]; + usleep(5000); + int nread = read(_uart, rbuf, 50); + if (nread < 6) { + printf("failed to read\n"); + return -1; + } + //printf("received: \n"); + //for (int i=0;i 0) { - _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); } else { - _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); } } else if (motor == MOTOR_2) { if (value > 0) { - _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); } else { - _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); } } + return -1; } int RoboClaw::setMotorDutyCycle(e_motor motor, float value) @@ -200,12 +234,13 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) int16_t duty = 1500*value; // send command if (motor == MOTOR_1) { - _sendCommand(CMD_SIGNED_DUTYCYCLE_1, + return _sendCommand(CMD_SIGNED_DUTYCYCLE_1, (uint8_t *)(&duty), 2, sum); } else if (motor == MOTOR_2) { - _sendCommand(CMD_SIGNED_DUTYCYCLE_2, + return _sendCommand(CMD_SIGNED_DUTYCYCLE_2, (uint8_t *)(&duty), 2, sum); } + return -1; } int RoboClaw::resetEncoders() @@ -215,13 +250,13 @@ int RoboClaw::resetEncoders() nullptr, 0, sum); } -void RoboClaw::update() +int RoboClaw::update() { // wait for an actuator publication, // check for exit condition every second // note "::poll" is required to distinguish global // poll from member function for driver - if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error + if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error // if new data, send to motors if (_actuators.updated()) { @@ -229,56 +264,68 @@ void RoboClaw::update() setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); } + return 0; } uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) { uint16_t sum = 0; - printf("sum\n"); + //printf("sum\n"); for (size_t i=0;i Date: Tue, 22 Oct 2013 05:08:20 -0400 Subject: Updated script. --- ROMFS/px4fmu_common/init.d/40_io_segway | 16 ++++++++-------- ROMFS/px4fmu_common/init.d/rcS | 12 ++++++------ 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index ffe7f695b..fb9dec043 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -21,8 +21,8 @@ param set MAV_TYPE 10 # # Start MAVLink (depends on orb) # -#mavlink start -d /dev/ttyS1 -b 57600 -#usleep 5000 +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 # # Start and configure PX4IO interface @@ -32,25 +32,25 @@ sh /etc/init.d/rc.io # # Start the commander (depends on orb, mavlink) # -#commander start +commander start # # Start the sensors (depends on orb, px4io) # -#sh /etc/init.d/rc.sensors +sh /etc/init.d/rc.sensors # # Start GPS interface (depends on orb) # -#gps start +gps start # # Start the attitude estimator (depends on orb) # -#attitude_estimator_ekf start +attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -roboclaw test /dev/ttyS2 128 -#segway start +roboclaw start /dev/ttyS2 128 1200 +segway start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3a458286e..424787d54 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -273,18 +273,18 @@ then set MODE custom fi - if param compare SYS_AUTOSTART 40 - then - sh /etc/init.d/40_io_segway - set MODE custom - fi - if param compare SYS_AUTOSTART 31 then sh /etc/init.d/31_io_phantom set MODE custom fi + if param compare SYS_AUTOSTART 40 + then + sh /etc/init.d/40_io_segway + set MODE custom + fi + if param compare SYS_AUTOSTART 100 then sh /etc/init.d/100_mpx_easystar -- cgit v1.2.3 From 108d723a4902086f883136c7de6a75d65256500b Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:10:26 -0400 Subject: Removed old timing hack. --- src/drivers/roboclaw/RoboClaw.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 3bbd7f48f..73bd5dada 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -94,13 +94,6 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, // setup default settings, reset encoders resetEncoders(); - - // XXX roboclaw gives 128 as first several csums - // have to read a couple of messages first - for (int i=0;i<5;i++) { - if (readEncoder(MOTOR_1) > 0) break; - usleep(3000); - } } RoboClaw::~RoboClaw() -- cgit v1.2.3 From d143e827dcd774548bca6d6b4cb6fb69ef35a826 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:43:10 -0400 Subject: Updated segway controller for new state machine. --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- src/modules/segway/BlockSegwayController.cpp | 13 +++++++------ 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 3cccc8447..85ac3f546 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/segway # XXX needs state machine update +MODULES += modules/segway MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a2027ded9..c0972be9e 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -78,7 +78,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/segway # XXX needs state machine update +MODULES += modules/segway MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index b1dc39445..96a443c6e 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -26,7 +26,7 @@ void BlockSegwayController::update() { _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.main_state == MAIN_STATE_AUTO) { // update guidance } @@ -34,17 +34,18 @@ void BlockSegwayController::update() { float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed); // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { + if (_status.main_state == MAIN_STATE_AUTO || + _status.main_state == MAIN_STATE_SEATBELT || + _status.main_state == MAIN_STATE_EASY) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + } else if (_status.main_state == MAIN_STATE_MANUAL) { + if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { _actuators.control[CH_LEFT] = _manual.throttle; _actuators.control[CH_RIGHT] = _manual.pitch; - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; } -- cgit v1.2.3 From c4a1a338ff4378c908ec34c5a5f408c1b96d0735 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:43:27 -0400 Subject: Changed driver to control motor duty cycle. --- src/drivers/roboclaw/RoboClaw.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 73bd5dada..d65a9be36 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -254,8 +254,8 @@ int RoboClaw::update() // if new data, send to motors if (_actuators.updated()) { _actuators.update(); - setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); - setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); + setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); } return 0; } -- cgit v1.2.3 From 28b4e978534e164d08125a9b0cf1fe428d9ad122 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 22 Oct 2013 21:01:30 +0200 Subject: Fixed bug with fd leak in rc_calibration_check --- src/modules/commander/commander.cpp | 4 ++-- src/modules/systemlib/rc_check.c | 4 +--- src/modules/systemlib/rc_check.h | 2 +- src/systemcmds/preflight_check/preflight_check.c | 4 ++-- 4 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ef509980..db758c386 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -687,7 +687,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - bool rc_calibration_ok = (OK == rc_calibration_check()); + bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -802,7 +802,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; /* re-check RC calibration */ - rc_calibration_ok = (OK == rc_calibration_check()); + rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 60d6473b8..b4350cc24 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -47,14 +47,12 @@ #include #include -int rc_calibration_check(void) { +int rc_calibration_check(int mavlink_fd) { 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 */ diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index e2238d151..e70b83cce 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -47,6 +47,6 @@ * @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); +__EXPORT int rc_calibration_check(int mavlink_fd); __END_DECLS diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index e9c5f1a2c..1c58a2db6 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -140,7 +140,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - bool rc_ok = (OK == rc_calibration_check()); + bool rc_ok = (OK == rc_calibration_check(mavlink_fd)); /* warn */ if (!rc_ok) @@ -227,4 +227,4 @@ static int led_off(int leds, int led) static int led_on(int leds, int led) { return ioctl(leds, LED_ON, led); -} \ No newline at end of file +} -- cgit v1.2.3 From 2f66a8894f1f8035bfc076306aa0d83197be108a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 22 Oct 2013 21:02:29 +0200 Subject: param_save_default() rewritten: don't try 10 times to do every operation but do it safe using temp file --- src/modules/systemlib/param/param.c | 85 ++++++++++++++++++------------------- 1 file changed, 42 insertions(+), 43 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index ccdb2ea38..398657dd7 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -508,64 +508,63 @@ param_get_default_file(void) int param_save_default(void) { - int result; - unsigned retries = 0; - - /* delete the file in case it exists */ - struct stat buffer; - if (stat(param_get_default_file(), &buffer) == 0) { - - do { - result = unlink(param_get_default_file()); - if (result != 0) { - retries++; - usleep(1000 * retries); - } - } while (result != OK && retries < 10); + int res; + int fd; - if (result != OK) - warnx("unlinking file %s failed.", param_get_default_file()); - } + const char *filename = param_get_default_file(); + const char *filename_tmp = malloc(strlen(filename) + 5); + sprintf(filename_tmp, "%s.tmp", filename); - /* create the file */ - int fd; + /* delete temp file if exist */ + res = unlink(filename_tmp); + + if (res != OK && errno == ENOENT) + res = OK; + + if (res != OK) + warn("failed to delete temp file: %s", filename_tmp); + + if (res == OK) { + /* write parameters to temp file */ + fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL); - do { - /* do another attempt in case the unlink call is not synced yet */ - fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); if (fd < 0) { - retries++; - usleep(1000 * retries); + warn("failed to open temp file: %s", filename_tmp); + res = ERROR; } - } while (fd < 0 && retries < 10); + if (res == OK) { + res = param_export(fd, false); - if (fd < 0) { - - warn("opening '%s' for writing failed", param_get_default_file()); - return fd; - } + if (res != OK) + warnx("failed to write parameters to file: %s", filename_tmp); + } - do { - result = param_export(fd, false); + close(fd); + } - if (result != OK) { - retries++; - usleep(1000 * retries); - } + if (res == OK) { + /* delete parameters file */ + res = unlink(filename); - } while (result != 0 && retries < 10); + if (res != OK && errno == ENOENT) + res = OK; + if (res != OK) + warn("failed to delete parameters file: %s", filename); + } - close(fd); + if (res == OK) { + /* rename temp file to parameters */ + res = rename(filename_tmp, filename); - if (result != OK) { - warn("error exporting parameters to '%s'", param_get_default_file()); - (void)unlink(param_get_default_file()); - return result; + if (res != OK) + warn("failed to rename %s to %s", filename_tmp, filename); } - return 0; + free(filename_tmp); + + return res; } /** -- cgit v1.2.3 From 3c6f43869178719abef90e5ef73e02dee952b1ce Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Oct 2013 18:57:06 +0200 Subject: sdlog2: parameters logging implemented (APM-compatible) --- src/modules/sdlog2/sdlog2.c | 122 +++++++++++++++++++++++++---------- src/modules/sdlog2/sdlog2_format.h | 8 +-- src/modules/sdlog2/sdlog2_messages.h | 27 +++++--- 3 files changed, 110 insertions(+), 47 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 61a54170a..f94875d5b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -84,6 +85,7 @@ #include #include +#include #include @@ -181,12 +183,17 @@ static void sdlog2_stop_log(void); /** * Write a header to log file: list of message formats. */ -static void write_formats(int fd); +static int write_formats(int fd); /** * Write version message to log file. */ -static void write_version(int fd); +static int write_version(int fd); + +/** + * Write parameters to log file. + */ +static int write_parameters(int fd); static bool file_exist(const char *filename); @@ -242,11 +249,11 @@ int sdlog2_main(int argc, char *argv[]) main_thread_should_exit = false; deamon_task = task_spawn_cmd("sdlog2", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 3000, - sdlog2_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 3000, + sdlog2_thread_main, + (const char **)argv); exit(0); } @@ -359,13 +366,13 @@ static void *logwriter_thread(void *arg) struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; - int log_file = open_logfile(); + int log_fd = open_logfile(); - /* write log messages formats */ - write_formats(log_file); - - /* write version */ - write_version(log_file); + /* write log messages formats, version and parameters */ + log_bytes_written += write_formats(log_fd); + log_bytes_written += write_version(log_fd); + log_bytes_written += write_parameters(log_fd); + fsync(log_fd); int poll_count = 0; @@ -404,7 +411,7 @@ static void *logwriter_thread(void *arg) n = available; } - n = write(log_file, read_ptr, n); + n = write(log_fd, read_ptr, n); should_wait = (n == available) && !is_part; @@ -419,21 +426,23 @@ static void *logwriter_thread(void *arg) } else { n = 0; + /* exit only with empty buffer */ if (main_thread_should_exit || logwriter_should_exit) { break; } + should_wait = true; } if (++poll_count == 10) { - fsync(log_file); + fsync(log_fd); poll_count = 0; } } - fsync(log_file); - close(log_file); + fsync(log_fd); + close(log_fd); return OK; } @@ -487,15 +496,17 @@ void sdlog2_stop_log() /* wait for write thread to return */ int ret; + if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) { warnx("error joining logwriter thread: %i", ret); } + logwriter_pthread = 0; sdlog2_status(); } -void write_formats(int fd) +int write_formats(int fd) { /* construct message format packet */ struct { @@ -505,35 +516,72 @@ void write_formats(int fd) LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), }; - /* fill message format packet for each format and write to log */ - int i; + int written = 0; - for (i = 0; i < log_formats_num; i++) { + /* fill message format packet for each format and write it */ + for (int i = 0; i < log_formats_num; i++) { log_msg_format.body = log_formats[i]; - log_bytes_written += write(fd, &log_msg_format, sizeof(log_msg_format)); + written += write(fd, &log_msg_format, sizeof(log_msg_format)); } - fsync(fd); + return written; } -void write_version(int fd) +int write_version(int fd) { /* construct version message */ struct { LOG_PACKET_HEADER; struct log_VER_s body; } log_msg_VER = { - LOG_PACKET_HEADER_INIT(127), + LOG_PACKET_HEADER_INIT(LOG_VER_MSG), }; - /* fill message format packet for each format and write to log */ - int i; - + /* fill version message and write it */ strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git)); strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); - log_bytes_written += write(fd, &log_msg_VER, sizeof(log_msg_VER)); + return write(fd, &log_msg_VER, sizeof(log_msg_VER)); +} + +int write_parameters(int fd) +{ + /* construct parameter message */ + struct { + LOG_PACKET_HEADER; + struct log_PARM_s body; + } log_msg_PARM = { + LOG_PACKET_HEADER_INIT(LOG_PARM_MSG), + }; + + int written = 0; + param_t params_cnt = param_count(); + + for (param_t param = 0; param < params_cnt; param++) { + /* fill parameter message and write it */ + strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name)); + float value = NAN; - fsync(fd); + switch (param_type(param)) { + case PARAM_TYPE_INT32: { + int32_t i; + param_get(param, &i); + value = i; // cast integer to float + break; + } + + case PARAM_TYPE_FLOAT: + param_get(param, &value); + break; + + default: + break; + } + + log_msg_PARM.body.value = value; + written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM)); + } + + return written; } int sdlog2_thread_main(int argc, char *argv[]) @@ -611,12 +659,13 @@ int sdlog2_thread_main(int argc, char *argv[]) } const char *converter_in = "/etc/logging/conv.zip"; - char* converter_out = malloc(120); + char *converter_out = malloc(120); sprintf(converter_out, "%s/conv.zip", folder_path); if (file_copy(converter_in, converter_out)) { errx(1, "unable to copy conversion scripts, exiting."); } + free(converter_out); /* only print logging path, important to find log file later */ @@ -630,6 +679,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } struct vehicle_status_s buf_status; + memset(&buf_status, 0, sizeof(buf_status)); /* warning! using union here to save memory, elements should be used separately! */ @@ -655,6 +705,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; } buf; + memset(&buf, 0, sizeof(buf)); struct { @@ -825,7 +876,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); fds[fdsc_count].fd = subs.airspeed_sub; fds[fdsc_count].events = POLLIN; - fdsc_count++; + fdsc_count++; /* --- ESCs --- */ subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); @@ -913,7 +964,7 @@ int sdlog2_thread_main(int argc, char *argv[]) continue; } - ifds = 1; // Begin from fds[1] again + ifds = 1; // begin from fds[1] again pthread_mutex_lock(&logbuffer_mutex); @@ -1172,8 +1223,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ESCs --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc); - for (uint8_t i=0; iarming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR; + if (armed != flag_system_armed) { flag_system_armed = armed; diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index 5c175ef7e..dc5e6c8bd 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -85,10 +85,10 @@ struct log_format_s { #define LOG_FORMAT(_name, _format, _labels) { \ .type = LOG_##_name##_MSG, \ - .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ - .name = #_name, \ - .format = _format, \ - .labels = _labels \ + .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ } #define LOG_FORMAT_MSG 0x80 diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9f709e459..90093a407 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -48,12 +48,6 @@ /* define message formats */ #pragma pack(push, 1) -/* --- TIME - TIME STAMP --- */ -#define LOG_TIME_MSG 1 -struct log_TIME_s { - uint64_t t; -}; - /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 struct log_ATT_s { @@ -253,18 +247,31 @@ struct log_GVSP_s { float vz; }; +/* --- TIME - TIME STAMP --- */ +#define LOG_TIME_MSG 129 +struct log_TIME_s { + uint64_t t; +}; + /* --- VER - VERSION --- */ -#define LOG_VER_MSG 127 +#define LOG_VER_MSG 130 struct log_VER_s { char arch[16]; char fw_git[64]; }; +/* --- PARM - PARAMETER --- */ +#define LOG_PARM_MSG 131 +struct log_PARM_s { + char name[16]; + float value; +}; + #pragma pack(pop) /* construct list of all message formats */ static const struct log_format_s log_formats[] = { - LOG_FORMAT(TIME, "Q", "StartTime"), + /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), @@ -283,7 +290,11 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), + /* system-level messages, ID >= 0x80 */ + // FMT: don't write format of format message, it's useless + LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), + LOG_FORMAT(PARM, "Nf", "Name,Value"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); -- cgit v1.2.3 From bb8a2c3631a518f822d1e7e3d40c28a281901a3f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Oct 2013 18:57:44 +0200 Subject: sdlog2_dump.py: C strings parsing fixed --- Tools/sdlog2_dump.py | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index 7fefc5908..5b1e55e22 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -25,8 +25,12 @@ import struct, sys if sys.hexversion >= 0x030000F0: runningPython3 = True + def _parseCString(cstr): + return str(cstr, 'ascii').split('\0')[0] else: runningPython3 = False + def _parseCString(cstr): + return str(cstr).split('\0')[0] class SDLog2Parser: BLOCK_SIZE = 8192 @@ -175,9 +179,9 @@ class SDLog2Parser: self.__csv_columns.append(full_label) self.__csv_data[full_label] = None if self.__file != None: - print(self.__csv_delim.join(self.__csv_columns), file=self.__file) + print(self.__csv_delim.join(self.__csv_columns), file=self.__file) else: - print(self.__csv_delim.join(self.__csv_columns)) + print(self.__csv_delim.join(self.__csv_columns)) def __printCSVRow(self): s = [] @@ -190,9 +194,9 @@ class SDLog2Parser: s.append(v) if self.__file != None: - print(self.__csv_delim.join(s), file=self.__file) + print(self.__csv_delim.join(s), file=self.__file) else: - print(self.__csv_delim.join(s)) + print(self.__csv_delim.join(s)) def __parseMsgDescr(self): if runningPython3: @@ -202,14 +206,9 @@ class SDLog2Parser: msg_type = data[0] if msg_type != self.MSG_TYPE_FORMAT: msg_length = data[1] - if runningPython3: - msg_name = str(data[2], 'ascii').strip("\0") - msg_format = str(data[3], 'ascii').strip("\0") - msg_labels = str(data[4], 'ascii').strip("\0").split(",") - else: - msg_name = str(data[2]).strip("\0") - msg_format = str(data[3]).strip("\0") - msg_labels = str(data[4]).strip("\0").split(",") + msg_name = _parseCString(data[2]) + msg_format = _parseCString(data[3]) + msg_labels = _parseCString(data[4]).split(",") # Convert msg_format to struct.unpack format string msg_struct = "" msg_mults = [] @@ -243,7 +242,7 @@ class SDLog2Parser: data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))) for i in range(len(data)): if type(data[i]) is str: - data[i] = data[i].strip("\0") + data[i] = _parseCString(data[i]) m = msg_mults[i] if m != None: data[i] = data[i] * m -- cgit v1.2.3 From 28845c4846e4f225e686f6643b5d2e0851392b2b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 09:45:34 +0200 Subject: Rascal (AERT) hil startup script --- ROMFS/px4fmu_common/init.d/1000_rc_fw.hil | 92 ------------------ ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 92 ++++++++++++++++++ .../px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 103 +++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 8 +- 4 files changed, 202 insertions(+), 93 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/1000_rc_fw.hil create mode 100644 ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil create mode 100644 ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil deleted file mode 100644 index 5e4028bbb..000000000 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil +++ /dev/null @@ -1,92 +0,0 @@ -#!nsh -# -# USB HIL start -# - -echo "[HIL] HILStar starting.." - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" - diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil new file mode 100644 index 000000000..5e4028bbb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -0,0 +1,92 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix +fw_pos_control_l1 start +fw_att_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil new file mode 100644 index 000000000..8c5e4b6a8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -0,0 +1,103 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +set MODE autostart +mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix +else + echo "Using /etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +fi + + +fw_pos_control_l1 start +fw_att_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 424787d54..94c01419e 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -108,7 +108,7 @@ then if param compare SYS_AUTOSTART 1000 then - sh /etc/init.d/1000_rc_fw.hil + sh /etc/init.d/1000_rc_fw_easystar.hil set MODE custom fi @@ -129,6 +129,12 @@ then sh /etc/init.d/1003_rc_quad_+.hil set MODE custom fi + + if param compare SYS_AUTOSTART 1004 + then + sh /etc/init.d/1004_rc_fw_Rascal110.hil + set MODE custom + fi if [ $MODE != custom ] then -- cgit v1.2.3