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 (limited to 'src/modules') 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 3dcd5dbd0e25432049e5ba6971851931764ae043 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 08:39:57 +0200 Subject: Piping through manual control channels --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++++ .../multirotor_att_control_main.c | 6 ++++++ src/modules/sensors/sensors.cpp | 22 ++++++++++++++++++++++ 3 files changed, 32 insertions(+) (limited to 'src/modules') diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ae9aaa2da..eb67874db 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -683,6 +683,10 @@ FixedwingAttitudeControl::task_main() _actuators.control[4] = _manual.flaps; } + _actuators.control[5] = _manual.aux1; + _actuators.control[6] = _manual.aux2; + _actuators.control[7] = _manual.aux3; + /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 44f8f664b..60a211817 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -368,6 +368,12 @@ mc_thread_main(int argc, char *argv[]) actuators.control[3] = 0.0f; } + /* fill in manual control values */ + actuators.control[4] = manual.flaps; + actuators.control[5] = manual.aux1; + actuators.control[6] = manual.aux2; + actuators.control[7] = manual.aux3; + actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fcacaf8f..630131e1e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -264,6 +265,7 @@ private: orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ + orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ @@ -519,6 +521,7 @@ Sensors::Sensors() : /* publications */ _sensor_pub(-1), _manual_control_pub(-1), + _actuator_group_3_pub(-1), _rc_pub(-1), _battery_pub(-1), _airspeed_pub(-1), @@ -1318,6 +1321,7 @@ Sensors::rc_poll() orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); struct manual_control_setpoint_s manual_control; + struct actuator_controls_s actuator_group_3; /* initialize to default values */ manual_control.roll = NAN; @@ -1485,6 +1489,16 @@ Sensors::rc_poll() manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); } + /* copy from mapped manual control to control group 3 */ + actuator_group_3.control[0] = manual_control.roll; + actuator_group_3.control[1] = manual_control.pitch; + actuator_group_3.control[2] = manual_control.yaw; + actuator_group_3.control[3] = manual_control.throttle; + actuator_group_3.control[4] = manual_control.flaps; + actuator_group_3.control[5] = manual_control.aux1; + actuator_group_3.control[6] = manual_control.aux2; + actuator_group_3.control[7] = manual_control.aux3; + /* check if ready for publishing */ if (_rc_pub > 0) { orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); @@ -1501,6 +1515,14 @@ Sensors::rc_poll() } else { _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); } + + /* check if ready for publishing */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + } } } -- cgit v1.2.3 From 99068e864beaabf3b2dfabfb2e2f1c6cb7df7095 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Oct 2013 09:10:40 +0200 Subject: Enable payload channels as direct pass-through from manual control --- ROMFS/px4fmu_common/init.d/rcS | 6 +++--- ROMFS/px4fmu_common/mixers/FMU_AERT.mix | 20 ++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_AET.mix | 20 ++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_Q.mix | 18 ++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_RET.mix | 20 ++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_X5.mix | 19 +++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_delta.mix | 20 ++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_hex_+.mix | 11 +++++++++++ ROMFS/px4fmu_common/mixers/FMU_hex_x.mix | 11 +++++++++++ ROMFS/px4fmu_common/mixers/FMU_quad_+.mix | 20 ++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_quad_v.mix | 19 +++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_quad_w.mix | 19 +++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_quad_x.mix | 19 +++++++++++++++++++ src/modules/sensors/sensor_params.c | 5 +++-- 14 files changed, 222 insertions(+), 5 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 93e76184d..e0813b031 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -166,11 +166,11 @@ then set MODE custom fi - if param compare SYS_AUTOSTART 10 - then + #if param compare SYS_AUTOSTART 10 + #then sh /etc/init.d/10_dji_f330 set MODE custom - fi + #fi if param compare SYS_AUTOSTART 11 then diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix index 75e82bb00..0ec663e35 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -62,3 +62,23 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/FMU_AET.mix index 20cb88b91..c73cb2a62 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AET.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AET.mix @@ -58,3 +58,23 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix index ebcb66b24..17ff71151 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_Q.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix @@ -50,3 +50,21 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/FMU_RET.mix index 95beb8927..f07c34ac8 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_RET.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_RET.mix @@ -51,3 +51,23 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix index 9f81e1dc3..610868354 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_X5.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix @@ -48,3 +48,22 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/FMU_delta.mix index 981466704..f0aa6650d 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_delta.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_delta.mix @@ -48,3 +48,23 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix index b5e38ce9e..f8f9f0e4d 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix @@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the + configuration. All co are mixed 100%. R: 6+ 10000 10000 10000 0 + +Gimbal / payload mixer for last two channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix index 8e8d122ad..26b40b9e9 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix @@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the X configuration. All c are mixed 100%. R: 6x 10000 10000 10000 0 + +Gimbal / payload mixer for last two channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix index dfdf1d58e..cd9a9cfab 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix @@ -5,3 +5,23 @@ This file defines a single mixer for a quadrotor in the + configuration. All con are mixed 100%. R: 4+ 10000 10000 10000 0 + + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix index 2a4a0f341..520aba635 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix @@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the V configuration. All co are mixed 100%. R: 4v 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix index 81b4af30b..58e6af74b 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix @@ -4,3 +4,22 @@ Multirotor mixer for PX4FMU This file defines a single mixer for a quadrotor with a wide configuration. All controls are mixed 100%. R: 4w 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix index 12a3bee20..fa21c8012 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix @@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the X configuration. All co are mixed 100%. R: 4x 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 4d719e0e1..682cb3d81 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -193,8 +193,9 @@ PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ -PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); -- cgit v1.2.3 From 71ac33596836519a341001bb48a8835b8af75cd3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Oct 2013 21:43:11 +0200 Subject: Small improvements to autoland, ensure that throttle can be shut down close to touch down. Depends on accurate land WP altitude --- src/lib/external_lgpl/tecs/tecs.h | 5 ++++ .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 32 +++++++++++++++++----- .../fw_pos_control_l1/fw_pos_control_l1_params.c | 1 + 3 files changed, 31 insertions(+), 7 deletions(-) (limited to 'src/modules') diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 2ae6b28bb..4a98c8e97 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -94,6 +94,11 @@ public: // Rate of change of velocity along X body axis in m/s^2 float get_VXdot(void) { return _vel_dot; } + + float get_speed_weight() { + return _spdWeight; + } + // log data on internal state of the controller. Called at 10Hz // void log_data(DataFlash_Class &dataflash, uint8_t msgid); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index cd4a0d58e..3697af225 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -196,6 +196,8 @@ private: float throttle_max; float throttle_cruise; + float throttle_land_max; + float loiter_hold_radius; } _parameters; /**< local copies of interesting parameters */ @@ -227,6 +229,8 @@ private: param_t throttle_max; param_t throttle_cruise; + param_t throttle_land_max; + param_t loiter_hold_radius; } _parameter_handles; /**< handles for interesting parameters */ @@ -342,6 +346,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_min = param_find("FW_THR_MIN"); _parameter_handles.throttle_max = param_find("FW_THR_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); + _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -404,6 +409,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); + param_get(_parameter_handles.time_const, &(_parameters.time_const)); param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); @@ -625,6 +632,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); float altitude_error = _global_triplet.current.altitude - _global_pos.alt; + /* no throttle limit as default */ + float throttle_max = 1.0f; + /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -634,11 +644,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + /* execute navigation once we have a setpoint */ if (_setpoint_valid) { - float altitude_error = _global_triplet.current.altitude - _global_pos.alt; - /* current waypoint (the one currently heading for) */ math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); @@ -712,16 +723,23 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - if (altitude_error > -20.0f) { - float flare_angle_rad = math::radians(15.0f);//math::radians(global_triplet.current.param1) + if (altitude_error > -10.0f) { + + float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + + /* set the speed weight to 0.0 to push the system to control altitude with pitch */ + _tecs.set_speed_weight(0.0f); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, flare_angle_rad, + false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* kill the throttle if param requests it */ + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f)); @@ -785,7 +803,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _loiter_hold = true; } - float altitude_error = _loiter_hold_alt - _global_pos.alt; + altitude_error = _loiter_hold_alt - _global_pos.alt; math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); @@ -862,7 +880,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } _att_sp.pitch_body = _tecs.get_pitch_demand(); - _att_sp.thrust = _tecs.get_throttle_demand(); + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); return setpoint; } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index d210ec712..9b64cb047 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -72,6 +72,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 0.0f); PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); -- cgit v1.2.3 From 013579cffd70f46788a356043563340731beabae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Oct 2013 07:54:04 +0200 Subject: More improvements on landing --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 72 +++++++++++++++++++--- .../fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 2 files changed, 64 insertions(+), 10 deletions(-) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3697af225..348a17ba6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -727,15 +727,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (altitude_error > -10.0f) { float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - /* set the speed weight to 0.0 to push the system to control altitude with pitch */ - _tecs.set_speed_weight(0.0f); + // /* set the speed weight to 0.0 to push the system to control altitude with pitch */ + // _tecs.set_speed_weight(0.0f); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, _parameters.airspeed_min, _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + 0.0f, _parameters.throttle_max, throttle_land, + land_pitch_min, math::radians(_parameters.pitch_limit_max)); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); @@ -814,7 +816,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.yaw_body = _l1_control.nav_bearing(); /* climb with full throttle if the altitude error is bigger than 5 meters */ - bool climb_out = (altitude_error > 5); + bool climb_out = (altitude_error > 3); float min_pitch; @@ -842,6 +844,43 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.roll_reset_integral = true; } + } else if (0/* easy mode enabled */) { + + /** EASY FLIGHT **/ + + if (0/* switched from another mode to easy */) { + _seatbelt_hold_heading = _att.yaw; + } + + if (0/* easy on and manual control yaw non-zero */) { + _seatbelt_hold_heading = _att.yaw + _manual.yaw; + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + + /* if in seatbelt mode, set airspeed based on manual control */ + + // XXX check if ground speed undershoot should be applied here + float seatbelt_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.throttle; + + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + seatbelt_airspeed, + _airspeed.indicated_airspeed_m_s, eas2tas, + false, _parameters.pitch_limit_min, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _parameters.pitch_limit_min, _parameters.pitch_limit_max); + } else if (0/* seatbelt mode enabled */) { /** SEATBELT FLIGHT **/ @@ -861,13 +900,28 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; + /* user switched off throttle */ + if (_manual.throttle < 0.1f) { + throttle_max = 0.0f; + /* switch to pure pitch based altitude control, give up speed */ + _tecs.set_speed_weight(0.0f); + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + _att_sp.roll_body = _manual.roll; + _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, - false, _parameters.pitch_limit_min, + climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 9b64cb047..c39df9ae3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); -PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 0.0f); +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); -- cgit v1.2.3 From 95aba0d70eae6a9aba55d31f223b852df1f82dcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Oct 2013 09:36:20 +0200 Subject: Almost perfect landing approach, needs touch-down fine tuning --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 47 +++++++++++++++++----- src/modules/mavlink/waypoints.c | 37 +++++++++++------ 2 files changed, 62 insertions(+), 22 deletions(-) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 348a17ba6..219c6ffa6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -635,6 +635,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* no throttle limit as default */ float throttle_max = 1.0f; + /* not in non-abort mode for landing yet */ + bool land_noreturn = false; + /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -717,23 +720,39 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + /* switch to heading hold for the last meters, continue heading hold after */ + + float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), next_wp.getX(), next_wp.getY()); + + if (wp_distance < 5.0f || land_noreturn) { + + /* heading hold, along the line connecting this and the last waypoint */ + float target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + + if (altitude_error > -20.0f) + land_noreturn = true; + + } else { + + /* normal navigation */ + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + } + _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - if (altitude_error > -10.0f) { - - float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) - float land_pitch_min = math::radians(5.0f); - float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float airspeed_land = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - // /* set the speed weight to 0.0 to push the system to control altitude with pitch */ - // _tecs.set_speed_weight(0.0f); + if (altitude_error > -5.0f) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, _parameters.airspeed_min, + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, _parameters.throttle_max, throttle_land, @@ -743,7 +762,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio throttle_max = math::min(throttle_max, _parameters.throttle_land_max); /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f)); + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + + } else if (altitude_error > -20.0f) { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } else { diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index ddad5f0df..adaf81404 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -399,6 +399,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (time_elapsed) { if (cur_wp->autocontinue) { + + /* safeguard against invalid missions with last wp autocontinue on */ + if (wpm->current_active_wp_id == wpm->size - 1) { + /* stop handling missions here */ + cur_wp->autocontinue = false; + } + cur_wp->current = 0; float navigation_lat = -1.0f; @@ -419,13 +426,16 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ navigation_frame = MAV_FRAME_LOCAL_NED; } + /* guard against missions without final land waypoint */ /* only accept supported navigation waypoints, skip unknown ones */ do { + /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) { /* this is a navigation waypoint */ navigation_frame = cur_wp->frame; @@ -434,17 +444,20 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ navigation_alt = cur_wp->z; } - if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { - /* the last waypoint was reached, if auto continue is - * activated keep the system loitering there. - */ - cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; - cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius - cur_wp->frame = navigation_frame; - cur_wp->x = navigation_lat; - cur_wp->y = navigation_lon; - cur_wp->z = navigation_alt; - cur_wp->autocontinue = false; + if (wpm->current_active_wp_id == wpm->size - 1) { + + /* if we're not landing at the last nav waypoint, we're falling back to loiter */ + if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) { + /* the last waypoint was reached, if auto continue is + * activated AND it is NOT a land waypoint, keep the system loitering there. + */ + cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; + cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius + cur_wp->frame = navigation_frame; + cur_wp->x = navigation_lat; + cur_wp->y = navigation_lon; + cur_wp->z = navigation_alt; + } /* we risk an endless loop for missions without navigation waypoints, abort. */ break; -- cgit v1.2.3 From 40610c7d484d077dc10726628c3adbe01edd91df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Oct 2013 10:38:51 +0200 Subject: Fixes, but approach needs proper design --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 66 +++++++++++++++++----- src/modules/mavlink/waypoints.c | 12 ++-- 2 files changed, 57 insertions(+), 21 deletions(-) (limited to 'src/modules') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 219c6ffa6..9ed74f0ca 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -158,6 +158,12 @@ private: float _launch_alt; bool _launch_valid; + /* land states */ + /* not in non-abort mode for landing yet */ + bool land_noreturn; + /* heading hold */ + float target_bearing; + /* throttle and airspeed states */ float _airspeed_error; ///< airspeed error to setpoint in m/s bool _airspeed_valid; ///< flag if a valid airspeed estimate exists @@ -329,7 +335,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_error(0.0f), _airspeed_valid(false), _groundspeed_undershoot(0.0f), - _global_pos_valid(false) + _global_pos_valid(false), + land_noreturn(false) { _nav_capabilities.turn_distance = 0.0f; @@ -635,9 +642,6 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* no throttle limit as default */ float throttle_max = 1.0f; - /* not in non-abort mode for landing yet */ - bool land_noreturn = false; - /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -722,15 +726,26 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), next_wp.getX(), next_wp.getY()); - - if (wp_distance < 5.0f || land_noreturn) { + float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + if (wp_distance < 15.0f || land_noreturn) { /* heading hold, along the line connecting this and the last waypoint */ - float target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + + + // if (global_triplet.previous_valid) { + // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + // } else { + + if (!land_noreturn) + target_bearing = _att.yaw; + //} + + warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - if (altitude_error > -20.0f) + if (altitude_error > -5.0f) land_noreturn = true; } else { @@ -739,6 +754,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); } + /* do not go down too early */ + if (wp_distance > 50.0f) { + altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; + } + + _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -748,15 +769,21 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; + float airspeed_land = _parameters.airspeed_min; + float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - if (altitude_error > -5.0f) { + if (altitude_error > -4.0f) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + /* land with minimal speed */ + + /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ + _tecs.set_speed_weight(2.0f); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, _parameters.throttle_max, throttle_land, - land_pitch_min, math::radians(_parameters.pitch_limit_max)); + math::radians(-10.0f), math::radians(15.0f)); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); @@ -764,9 +791,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - } else if (altitude_error > -20.0f) { + } else if (wp_distance < 60.0f && altitude_error > -20.0f) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + /* minimize speed to approach speed */ + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -774,6 +803,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { + /* normal cruise speed */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), @@ -866,6 +897,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } + /* reset land state */ + if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { + land_noreturn = false; + } + if (was_circle_mode && !_l1_control.circle_mode()) { /* just kicked out of loiter, reset roll integrals */ _att_sp.roll_reset_integral = true; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index adaf81404..7e4a2688f 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -398,13 +398,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (time_elapsed) { - if (cur_wp->autocontinue) { + /* safeguard against invalid missions with last wp autocontinue on */ + if (wpm->current_active_wp_id == wpm->size - 1) { + /* stop handling missions here */ + cur_wp->autocontinue = false; + } - /* safeguard against invalid missions with last wp autocontinue on */ - if (wpm->current_active_wp_id == wpm->size - 1) { - /* stop handling missions here */ - cur_wp->autocontinue = false; - } + if (cur_wp->autocontinue) { cur_wp->current = 0; -- 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(-) (limited to 'src/modules') 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(-) (limited to 'src/modules') 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 (limited to 'src/modules') 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(-) (limited to 'src/modules') 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(-) (limited to 'src/modules') 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(-) (limited to 'src/modules') 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(-) (limited to 'src/modules') 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 (limited to 'src/modules') 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(-) (limited to 'src/modules') 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 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(-) (limited to 'src/modules') 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 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(-) (limited to 'src/modules') 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(-) (limited to 'src/modules') 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(-) (limited to 'src/modules') 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 1cb73687f7acd4ae3263c40940afe057b1b7d368 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 21:51:45 +0200 Subject: added parameter for maximal roll angle --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 2 +- src/lib/ecl/l1/ecl_l1_pos_controller.h | 11 +++++++++++ src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++++ src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 3 +++ 4 files changed, 20 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index daf136d49..196ded26c 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -43,7 +43,7 @@ float ECL_L1_Pos_Controller::nav_roll() { float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); - ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f); + ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad); return ret; } diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index 5a17346cb..7a3c42a92 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -222,6 +222,15 @@ public: _K_L1 = 4.0f * _L1_damping * _L1_damping; } + + /** + * Set the maximum roll angle output in radians + * + */ + void set_l1_roll_limit(float roll_lim_rad) { + _roll_lim_rad = roll_lim_rad; + } + private: float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2 @@ -238,6 +247,8 @@ private: float _K_L1; ///< L1 control gain for _L1_damping float _heading_omega; ///< Normalized frequency + float _roll_lim_rad; /// Date: Mon, 28 Oct 2013 15:49:26 +1100 Subject: px4io: fixed the io_reg_{set,get} errors this fixes the PX4IO state machine to avoid the io errors we were seeing. There are still some open questions with this code, but it now seems to give zero errors, which is an improvement! --- src/modules/px4iofirmware/i2c.c | 6 ------ src/modules/px4iofirmware/px4io.c | 10 ++-------- src/modules/px4iofirmware/serial.c | 20 +++----------------- 3 files changed, 5 insertions(+), 31 deletions(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 10aeb5c9f..79b6546b3 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -149,12 +149,6 @@ interface_init(void) #endif } -void -interface_tick() -{ -} - - /* reset the I2C bus used to recover from lockups diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e70b3fe88..e28106971 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -159,9 +159,6 @@ user_start(int argc, char *argv[]) /* start the FMU interface */ interface_init(); - /* add a performance counter for the interface */ - perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface"); - /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -203,11 +200,6 @@ user_start(int argc, char *argv[]) /* track the rate at which the loop is running */ perf_count(loop_perf); - /* kick the interface */ - perf_begin(interface_perf); - interface_tick(); - perf_end(interface_perf); - /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); @@ -218,6 +210,7 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); +#if 0 /* check for debug activity */ show_debug_messages(); @@ -234,6 +227,7 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } +#endif } } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 94d7407df..e9adc8cd6 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -74,9 +74,6 @@ static DMA_HANDLE rx_dma; static int serial_interrupt(int irq, void *context); static void dma_reset(void); -/* if we spend this many ticks idle, reset the DMA */ -static unsigned idle_ticks; - static struct IOPacket dma_packet; /* serial register accessors */ @@ -150,16 +147,6 @@ interface_init(void) debug("serial init"); } -void -interface_tick() -{ - /* XXX look for stuck/damaged DMA and reset? */ - if (idle_ticks++ > 100) { - dma_reset(); - idle_ticks = 0; - } -} - static void rx_handle_packet(void) { @@ -230,9 +217,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); - /* reset the idle counter */ - idle_ticks = 0; - /* handle the received packet */ rx_handle_packet(); @@ -308,6 +292,7 @@ serial_interrupt(int irq, void *context) /* it was too short - possibly truncated */ perf_count(pc_badidle); + dma_reset(); return 0; } @@ -343,7 +328,8 @@ dma_reset(void) sizeof(dma_packet), DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS); + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIVERYHI); /* start receive DMA ready for the next packet */ stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); -- cgit v1.2.3 From 1336d625a83ba3f1afc207b64ec248dd1e15848a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Oct 2013 14:47:37 +0100 Subject: Hotfix: Announcing important messages via audio --- src/modules/commander/commander.cpp | 58 ++++++++++++++++++------------------- 1 file changed, 29 insertions(+), 29 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9814a7bcc..44a144296 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -379,7 +379,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } // TODO remove debug code - //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); + //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ arming_res = TRANSITION_NOT_CHANGED; @@ -496,11 +496,11 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); + mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } @@ -521,13 +521,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe tune_negative(); if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command); } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command); } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); } } @@ -874,10 +874,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); + mavlink_log_critical(mavlink_fd, "#audio: LANDED"); } else { - mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); + mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } } @@ -955,7 +955,7 @@ int commander_thread_main(int argc, char *argv[]) //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); + mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; battery_tune_played = false; @@ -967,7 +967,7 @@ int commander_thread_main(int argc, char *argv[]) /* critical battery voltage, this is rather an emergency, change state machine */ if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); + mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; battery_tune_played = false; @@ -1069,12 +1069,12 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained"); + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; } } @@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (res == TRANSITION_DENIED) { warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } /* fill current_status according to mode switches */ @@ -1159,12 +1159,12 @@ int commander_thread_main(int argc, char *argv[]) } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } @@ -1189,7 +1189,7 @@ int commander_thread_main(int argc, char *argv[]) if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); } /* check which state machines for changes, clear "changed" flag */ @@ -1506,7 +1506,7 @@ print_reject_mode(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; char s[80]; - sprintf(s, "[cmd] WARNING: reject %s", msg); + sprintf(s, "#audio: warning: reject %s", msg); mavlink_log_critical(mavlink_fd, s); tune_negative(); } @@ -1520,7 +1520,7 @@ print_reject_arm(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; char s[80]; - sprintf(s, "[cmd] %s", msg); + sprintf(s, "#audio: %s", msg); mavlink_log_critical(mavlink_fd, s); tune_negative(); } @@ -1617,14 +1617,14 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (res == TRANSITION_CHANGED) { if (control_mode->flag_control_position_enabled) { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD"); } else { if (status->condition_landed) { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)"); } else { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD"); } } } @@ -1660,22 +1660,22 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); tune_negative(); break; @@ -1814,14 +1814,14 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) - mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -1834,14 +1834,14 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) - mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } -- cgit v1.2.3