diff options
Diffstat (limited to 'src')
40 files changed, 2250 insertions, 797 deletions
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index eff5e7349..8a4f68428 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -66,7 +66,7 @@ struct accel_report { int16_t temperature_raw; }; -/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */ +/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ struct accel_scale { float x_offset; float x_scale; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 084671ae2..9d8ad084e 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -132,11 +132,8 @@ ETSAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - return ret; } - ret = OK; - return ret; } diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index ee45d46ac..b3003fc1b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -138,11 +138,8 @@ MEASAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - return ret; } - ret = OK; - return ret; } diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index ea87b37d9..727c86e02 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -172,7 +172,20 @@ RGBLED::probe() bool on, powersave; uint8_t r, g, b; - ret = get(on, powersave, r, g, b); + /** + this may look strange, but is needed. There is a serial + EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to + a bunch of I2C addresses, including the 0x55 used by this + LED device. So we need to do enough operations to be sure + we are talking to the right device. These 3 operations seem + to be enough, as the 3rd one consistently fails if no + RGBLED is on the bus. + */ + if ((ret=get(on, powersave, r, g, b)) != OK || + (ret=send_led_enable(false) != OK) || + (ret=send_led_enable(false) != OK)) { + return ret; + } return ret; } @@ -561,7 +574,7 @@ rgbled_main(int argc, char *argv[]) int ch; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) { + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { switch (ch) { case 'a': rgbledadr = strtol(optarg, NULL, 0); @@ -577,7 +590,12 @@ rgbled_main(int argc, char *argv[]) } } - const char *verb = argv[1]; + if (optind >= argc) { + rgbled_usage(); + exit(1); + } + + const char *verb = argv[optind]; int fd; int ret; @@ -598,6 +616,9 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + errx(1, "init failed"); + } i2cdevice = PX4_I2C_BUS_LED; } } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp new file mode 100644 index 000000000..d65a9be36 --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -0,0 +1,328 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file RoboClaw.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include "RoboClaw.hpp" +#include <poll.h> +#include <stdio.h> +#include <math.h> +#include <string.h> +#include <fcntl.h> +#include <termios.h> + +#include <systemlib/err.h> +#include <arch/board/board.h> +#include <mavlink/mavlink_log.h> + +#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/topics/debug_key_value.h> +#include <drivers/drv_hrt.h> + +uint8_t RoboClaw::checksum_mask = 0x7f; + +RoboClaw::RoboClaw(const char *deviceName, uint16_t address, + uint16_t pulsesPerRev): + _address(address), + _pulsesPerRev(pulsesPerRev), + _uart(0), + _controlPoll(), + _actuators(NULL, ORB_ID(actuator_controls_0), 20), + _motor1Position(0), + _motor1Speed(0), + _motor1Overflow(0), + _motor2Position(0), + _motor2Speed(0), + _motor2Overflow(0) +{ + // setup control polling + _controlPoll.fd = _actuators.getHandle(); + _controlPoll.events = POLLIN; + + // start serial port + _uart = open(deviceName, O_RDWR | O_NOCTTY); + if (_uart < 0) err(1, "could not open %s", deviceName); + int ret = 0; + struct termios uart_config; + ret = tcgetattr(_uart, &uart_config); + if (ret < 0) err (1, "failed to get attr"); + uart_config.c_oflag &= ~ONLCR; // no CR for every LF + ret = cfsetispeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set input speed"); + ret = cfsetospeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set output speed"); + ret = tcsetattr(_uart, TCSANOW, &uart_config); + if (ret < 0) err (1, "failed to set attr"); + + // setup default settings, reset encoders + resetEncoders(); +} + +RoboClaw::~RoboClaw() +{ + setMotorDutyCycle(MOTOR_1, 0.0); + setMotorDutyCycle(MOTOR_2, 0.0); + close(_uart); +} + +int RoboClaw::readEncoder(e_motor motor) +{ + uint16_t sum = 0; + if (motor == MOTOR_1) { + _sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum); + } else if (motor == MOTOR_2) { + _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); + } + uint8_t rbuf[50]; + usleep(5000); + int nread = read(_uart, rbuf, 50); + if (nread < 6) { + printf("failed to read\n"); + return -1; + } + //printf("received: \n"); + //for (int i=0;i<nread;i++) { + //printf("%d\t", rbuf[i]); + //} + //printf("\n"); + uint32_t count = 0; + uint8_t * countBytes = (uint8_t *)(&count); + countBytes[3] = rbuf[0]; + countBytes[2] = rbuf[1]; + countBytes[1] = rbuf[2]; + countBytes[0] = rbuf[3]; + uint8_t status = rbuf[4]; + uint8_t checksum = rbuf[5]; + uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & + checksum_mask; + // check if checksum is valid + if (checksum != checksum_computed) { + printf("checksum failed: expected %d got %d\n", + checksum, checksum_computed); + return -1; + } + int overFlow = 0; + + if (status & STATUS_REVERSE) { + //printf("roboclaw: reverse\n"); + } + + if (status & STATUS_UNDERFLOW) { + //printf("roboclaw: underflow\n"); + overFlow = -1; + } else if (status & STATUS_OVERFLOW) { + //printf("roboclaw: overflow\n"); + overFlow = +1; + } + + static int64_t overflowAmount = 0x100000000LL; + if (motor == MOTOR_1) { + _motor1Overflow += overFlow; + _motor1Position = float(int64_t(count) + + _motor1Overflow*overflowAmount)/_pulsesPerRev; + } else if (motor == MOTOR_2) { + _motor2Overflow += overFlow; + _motor2Position = float(int64_t(count) + + _motor2Overflow*overflowAmount)/_pulsesPerRev; + } + return 0; +} + +void RoboClaw::printStatus(char *string, size_t n) +{ + snprintf(string, n, + "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n", + double(getMotorPosition(MOTOR_1)), + double(getMotorSpeed(MOTOR_1)), + double(getMotorPosition(MOTOR_2)), + double(getMotorSpeed(MOTOR_2))); +} + +float RoboClaw::getMotorPosition(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Position; + } else if (motor == MOTOR_2) { + return _motor2Position; + } +} + +float RoboClaw::getMotorSpeed(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Speed; + } else if (motor == MOTOR_2) { + return _motor2Speed; + } +} + +int RoboClaw::setMotorSpeed(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + uint8_t speed = fabs(value)*127; + // send command + if (motor == MOTOR_1) { + if (value > 0) { + return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + } else { + return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + } + } else if (motor == MOTOR_2) { + if (value > 0) { + return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + } else { + return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + } + } + return -1; +} + +int RoboClaw::setMotorDutyCycle(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + int16_t duty = 1500*value; + // send command + if (motor == MOTOR_1) { + return _sendCommand(CMD_SIGNED_DUTYCYCLE_1, + (uint8_t *)(&duty), 2, sum); + } else if (motor == MOTOR_2) { + return _sendCommand(CMD_SIGNED_DUTYCYCLE_2, + (uint8_t *)(&duty), 2, sum); + } + return -1; +} + +int RoboClaw::resetEncoders() +{ + uint16_t sum = 0; + return _sendCommand(CMD_RESET_ENCODERS, + nullptr, 0, sum); +} + +int RoboClaw::update() +{ + // wait for an actuator publication, + // check for exit condition every second + // note "::poll" is required to distinguish global + // poll from member function for driver + if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error + + // if new data, send to motors + if (_actuators.updated()) { + _actuators.update(); + setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); + setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + } + return 0; +} + +uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) +{ + uint16_t sum = 0; + //printf("sum\n"); + for (size_t i=0;i<n;i++) { + sum += buf[i]; + //printf("%d\t", buf[i]); + } + //printf("total sum %d\n", sum); + return sum; +} + +int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, + size_t n_data, uint16_t & prev_sum) +{ + tcflush(_uart, TCIOFLUSH); // flush buffers + uint8_t buf[n_data + 3]; + buf[0] = _address; + buf[1] = cmd; + for (size_t i=0;i<n_data;i++) { + buf[i+2] = data[n_data - i - 1]; // MSB + } + uint16_t sum = _sumBytes(buf, n_data + 2); + prev_sum += sum; + buf[n_data + 2] = sum & checksum_mask; + //printf("\nmessage:\n"); + //for (size_t i=0;i<n_data+3;i++) { + //printf("%d\t", buf[i]); + //} + //printf("\n"); + return write(_uart, buf, n_data + 3); +} + +int roboclawTest(const char *deviceName, uint8_t address, + uint16_t pulsesPerRev) +{ + printf("roboclaw test: starting\n"); + + // setup + RoboClaw roboclaw(deviceName, address, pulsesPerRev); + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3); + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3); + char buf[200]; + for (int i=0; i<10; i++) { + usleep(100000); + roboclaw.readEncoder(RoboClaw::MOTOR_1); + roboclaw.readEncoder(RoboClaw::MOTOR_2); + roboclaw.printStatus(buf,200); + printf("%s", buf); + } + + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3); + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3); + for (int i=0; i<10; i++) { + usleep(100000); + roboclaw.readEncoder(RoboClaw::MOTOR_1); + roboclaw.readEncoder(RoboClaw::MOTOR_2); + roboclaw.printStatus(buf,200); + printf("%s", buf); + } + + printf("Test complete\n"); + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp new file mode 100644 index 000000000..e9f35cf95 --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * 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 RoboClas.hpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#pragma once + +#include <poll.h> +#include <stdio.h> +#include <controllib/uorb/UOrbSubscription.hpp> +#include <uORB/topics/actuator_controls.h> +#include <drivers/device/i2c.h> + +/** + * This is a driver for the RoboClaw motor controller + */ +class RoboClaw +{ +public: + + /** control channels */ + enum e_channel { + CH_VOLTAGE_LEFT = 0, + CH_VOLTAGE_RIGHT + }; + + /** motors */ + enum e_motor { + MOTOR_1 = 0, + MOTOR_2 + }; + + /** + * constructor + * @param deviceName the name of the + * serial port e.g. "/dev/ttyS2" + * @param address the adddress of the motor + * (selectable on roboclaw) + * @param pulsesPerRev # of encoder + * pulses per revolution of wheel + */ + RoboClaw(const char *deviceName, uint16_t address, + uint16_t pulsesPerRev); + + /** + * deconstructor + */ + virtual ~RoboClaw(); + + /** + * @return position of a motor, rev + */ + float getMotorPosition(e_motor motor); + + /** + * @return speed of a motor, rev/sec + */ + float getMotorSpeed(e_motor motor); + + /** + * set the speed of a motor, rev/sec + */ + int setMotorSpeed(e_motor motor, float value); + + /** + * set the duty cycle of a motor, rev/sec + */ + int setMotorDutyCycle(e_motor motor, float value); + + /** + * reset the encoders + * @return status + */ + int resetEncoders(); + + /** + * main update loop that updates RoboClaw motor + * dutycycle based on actuator publication + */ + int update(); + + /** + * read data from serial + */ + int readEncoder(e_motor motor); + + /** + * print status + */ + void printStatus(char *string, size_t n); + +private: + + // Quadrature status flags + enum e_quadrature_status_flags { + STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/ + STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/ + STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/ + }; + + // commands + // We just list the commands we want from the manual here. + enum e_command { + + // simple + CMD_DRIVE_FWD_1 = 0, + CMD_DRIVE_REV_1 = 1, + CMD_DRIVE_FWD_2 = 4, + CMD_DRIVE_REV_2 = 5, + + // encoder commands + CMD_READ_ENCODER_1 = 16, + CMD_READ_ENCODER_2 = 17, + CMD_RESET_ENCODERS = 20, + + // advanced motor control + CMD_READ_SPEED_HIRES_1 = 30, + CMD_READ_SPEED_HIRES_2 = 31, + CMD_SIGNED_DUTYCYCLE_1 = 32, + CMD_SIGNED_DUTYCYCLE_2 = 33, + }; + + static uint8_t checksum_mask; + + uint16_t _address; + uint16_t _pulsesPerRev; + + int _uart; + + /** poll structure for control packets */ + struct pollfd _controlPoll; + + /** actuator controls subscription */ + control::UOrbSubscription<actuator_controls_s> _actuators; + + // private data + float _motor1Position; + float _motor1Speed; + int16_t _motor1Overflow; + + float _motor2Position; + float _motor2Speed; + int16_t _motor2Overflow; + + // private methods + uint16_t _sumBytes(uint8_t * buf, size_t n); + int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum); +}; + +// unit testing +int roboclawTest(const char *deviceName, uint8_t address, + uint16_t pulsesPerRev); + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk new file mode 100644 index 000000000..1abecf198 --- /dev/null +++ b/src/drivers/roboclaw/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# RoboClaw Motor Controller +# + +MODULE_COMMAND = roboclaw + +SRCS = roboclaw_main.cpp \ + RoboClaw.cpp diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp new file mode 100644 index 000000000..44ed03254 --- /dev/null +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + + +/** + * @ file roboclaw_main.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> + +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> + +#include <arch/board/board.h> +#include "RoboClaw.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int roboclaw_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(); + +static void usage() +{ + fprintf(stderr, "usage: roboclaw " + "{start|stop|status|test}\n\n"); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int roboclaw_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage(); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("roboclaw already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn_cmd("roboclaw", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + roboclaw_thread_main, + (const char **)argv); + exit(0); + + } else if (!strcmp(argv[1], "test")) { + + const char * deviceName = "/dev/ttyS2"; + uint8_t address = 128; + uint16_t pulsesPerRev = 1200; + + if (argc == 2) { + printf("testing with default settings\n"); + } else if (argc != 4) { + printf("usage: roboclaw test device address pulses_per_rev\n"); + exit(-1); + } else { + deviceName = argv[2]; + address = strtoul(argv[3], nullptr, 0); + pulsesPerRev = strtoul(argv[4], nullptr, 0); + } + + printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n", + deviceName, address, pulsesPerRev); + + roboclawTest(deviceName, address, pulsesPerRev); + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "stop")) { + + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "status")) { + + if (thread_running) { + printf("\troboclaw app is running\n"); + + } else { + printf("\troboclaw app not started\n"); + } + exit(0); + } + + usage(); + exit(1); +} + +int roboclaw_thread_main(int argc, char *argv[]) +{ + printf("[roboclaw] starting\n"); + + // skip parent process args + argc -=2; + argv +=2; + + if (argc < 3) { + printf("usage: roboclaw start device address\n"); + return -1; + } + + const char *deviceName = argv[1]; + uint8_t address = strtoul(argv[2], nullptr, 0); + uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0); + + printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n", + deviceName, address, pulsesPerRev); + + // start + RoboClaw roboclaw(deviceName, address, pulsesPerRev); + + thread_running = true; + + // loop + while (!thread_should_exit) { + roboclaw.update(); + } + + // exit + printf("[roboclaw] exiting.\n"); + thread_running = false; + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk new file mode 100644 index 000000000..f5f59a2dc --- /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. +# +############################################################################ + +# +# Conversion library +# + +SRCS = rotation.cpp diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp new file mode 100644 index 000000000..b078562c2 --- /dev/null +++ b/src/lib/conversion/rotation.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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" +#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..85c63c0fc --- /dev/null +++ b/src/lib/conversion/rotation.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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_ +#define ROTATION_H_ + +#include <unistd.h> +#include <mathlib/mathlib.h> + +/** + * 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/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; ///<maximum roll angle + /** * Convert a 2D vector from WGS84 to planar coordinates. * 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/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index cfa7d9e8a..5eeca5a1a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -100,10 +100,29 @@ * 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 <anton.babushkin@me.com> */ #include "accelerometer_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include <unistd.h> @@ -112,11 +131,13 @@ #include <fcntl.h> #include <sys/prctl.h> #include <math.h> +#include <mathlib/mathlib.h> #include <string.h> #include <drivers/drv_hrt.h> #include <uORB/topics/sensor_combined.h> #include <drivers/drv_accel.h> #include <geo/geo.h> +#include <conversion/rotation.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <mavlink/mavlink_log.h> @@ -127,93 +148,122 @@ #endif static const int ERROR = -1; -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +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); 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) { - /* announce change */ - mavlink_log_info(mavlink_fd, "accel calibration started"); - mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); +int do_accel_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + + 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, CAL_FAILED_RESET_CAL_MSG); + } - /* 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]; 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]))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); + /* 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 */ + param_t board_rotation_h = param_find("SENS_BOARD_ROT"); + 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); + 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_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_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_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, CAL_FAILED_SET_PARAMS_MSG); + res = ERROR; } + } + if (res == OK) { + /* apply new scaling and offsets */ 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], - }; - - 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, CAL_FAILED_APPLY_CAL_MSG); + } + } + + 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, CAL_FAILED_SAVE_PARAMS_MSG); } + } + + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "accel calibration done"); - return OK; } else { - /* measurements error */ - mavlink_log_info(mavlink_fd, "accel calibration aborted"); - return ERROR; + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } - /* exit accel calibration mode */ + return res; } -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 }; 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; @@ -221,64 +271,63 @@ 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; } } - 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- " : ""); - if (old_done_count != done_count) - mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 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- " : "", + (!data_collected[2]) ? "y+ " : "", + (!data_collected[3]) ? "y- " : "", + (!data_collected[4]) ? "z+ " : "", + (!data_collected[5]) ? "z- " : ""); + 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]) { - 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; } 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 rotation+scale matrix */ - float accel_T[3][3]; - 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; - } + if (res == OK) { + /* calculate offsets and transform matrix */ + res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - /* 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]; + if (res != OK) { + mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); + } } - return OK; + return res; } /* @@ -287,14 +336,15 @@ 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 }; /* 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 */ @@ -318,30 +368,38 @@ 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]; + 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); + + if (d > still_thr2 * 8.0f) + d = still_thr2 * 8.0f; + 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) { @@ -349,62 +407,71 @@ 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 * 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; } } + } else if (poll_ret == 0) { poll_errcount++; } + if (t > t_timeout) { poll_errcount++; } if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor"); - return -1; + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + return ERROR; } } - 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"); + mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); - return -2; // Can't detect orientation + return ERROR; // Can't detect orientation } /* * 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; @@ -415,12 +482,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; @@ -437,10 +508,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 @@ -457,7 +530,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; @@ -466,6 +540,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]; @@ -475,6 +550,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; 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 <anton.babushkin@me.com> + * + * 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 <anton.babushkin@me.com> + */ + +#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/commander.cpp b/src/modules/commander/commander.cpp index f733e9483..bc363469b 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: @@ -687,7 +690,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 +805,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); @@ -941,7 +944,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); } @@ -971,6 +974,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); } @@ -1245,12 +1249,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); @@ -1265,7 +1271,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); @@ -1309,6 +1315,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); @@ -1333,6 +1340,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 { @@ -1706,11 +1714,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; @@ -1785,7 +1792,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) { @@ -1866,7 +1873,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 369e6da62..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 <stdio.h> @@ -56,21 +58,14 @@ #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 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; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); - 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,97 +74,100 @@ 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"); + int res = OK; + /* reset all offsets to zero and all scales to one */ + int fd = open(GYRO_DEVICE_PATH, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); - unsigned poll_errcount = 0; - - while (calibration_counter < calibration_count) { + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + } - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_sensor_combined; - fds[0].events = POLLIN; + 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]; + 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, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } + } - int poll_ret = poll(fds, 1, 1000); + close(sub_sensor_gyro); - 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]; - 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++; - } + gyro_scale.x_offset /= calibration_count; + gyro_scale.y_offset /= calibration_count; + gyro_scale.z_offset /= calibration_count; + } - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); - close(sub_sensor_combined); - 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; } } - 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!"); + 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: failed to set offset params"); + res = ERROR; } + } - /* 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); +#if 0 + /* beep on offset calibration end */ + mavlink_log_info(mavlink_fd, "gyro offset calibration done"); + tune_neutral(); - /* auto-save to EEPROM */ - int save_ret = param_save_default(); + /* scale calibration */ + /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ - 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; - } + 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."); - tune_neutral(); - /* third beep by cal end routine */ + /* apply new offsets */ + fd = open(GYRO_DEVICE_PATH, 0); - } else { - mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); - close(sub_sensor_combined); - return ERROR; - } + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to apply new offsets for gyro"); - mavlink_log_info(mavlink_fd, "offset calibration done."); + close(fd); -#if 0 - /*** --- SCALING --- ***/ - - 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."); unsigned rotations_count = 30; float gyro_integral = 0.0f; @@ -178,9 +176,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(); @@ -190,7 +190,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; @@ -218,14 +218,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; @@ -235,63 +238,68 @@ 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); -#else - float gyro_scales[] = { 1.0f, 1.0f, 1.0f }; -#endif - + 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; + } - if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { + /* beep on calibration end */ + mavlink_log_info(mavlink_fd, "gyro scale calibration done"); + tune_neutral(); - 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!"); +#endif + + 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: failed to set scale params"); + res = ERROR; } + } - /* set offsets to actual value */ + if (res == OK) { + /* apply new scaling and offsets */ 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], - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } + } + + 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, CAL_FAILED_SAVE_PARAMS_MSG); } + } - mavlink_log_info(mavlink_fd, "gyro calibration done"); + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - /* 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); - return ERROR; + 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 <stdio.h> #include <stdlib.h> @@ -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; } } 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/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/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..ffa7915a7 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 @@ -192,10 +198,13 @@ private: float pitch_limit_min; float pitch_limit_max; + float roll_limit; float throttle_min; float throttle_max; float throttle_cruise; + float throttle_land_max; + float loiter_hold_radius; } _parameters; /**< local copies of interesting parameters */ @@ -223,10 +232,13 @@ private: param_t pitch_limit_min; param_t pitch_limit_max; + param_t roll_limit; param_t throttle_min; param_t throttle_max; param_t throttle_cruise; + param_t throttle_land_max; + param_t loiter_hold_radius; } _parameter_handles; /**< handles for interesting parameters */ @@ -325,7 +337,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; @@ -339,9 +352,11 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN"); _parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX"); + _parameter_handles.roll_limit = param_find("FW_R_LIM"); _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"); @@ -400,10 +415,13 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min)); param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max)); + param_get(_parameter_handles.roll_limit, &(_parameters.roll_limit)); param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min)); 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)); @@ -419,6 +437,7 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); + _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); _tecs.set_time_const(_parameters.time_const); _tecs.set_min_sink_rate(_parameters.min_sink_rate); @@ -625,6 +644,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 +656,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); @@ -706,27 +729,87 @@ 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(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 */ + + + // 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 > -5.0f) + land_noreturn = true; + + } else { + + /* normal navigation */ + _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(); /* 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) + 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; + float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + if (altitude_error > -4.0f) { + + /* 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, - true, flare_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + false, flare_angle_rad, + 0.0f, _parameters.throttle_max, throttle_land, + 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); /* 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 (wp_distance < 60.0f && altitude_error > -20.0f) { + + /* 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, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } 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), @@ -785,7 +868,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); @@ -796,7 +879,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; @@ -819,11 +902,53 @@ 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; } + } 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 **/ @@ -843,13 +968,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); @@ -862,7 +1002,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..3bb872405 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 @@ -67,11 +67,15 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); +PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); + + PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); 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; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index ddad5f0df..7e4a2688f 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -398,7 +398,14 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (time_elapsed) { + /* 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; 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; 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/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index ddd2f62c0..8f4f9e080 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -71,6 +71,8 @@ #include "position_estimator_inav_params.h" #include "inertial_filter.h" +#define MIN_VALID_W 0.00001f + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ @@ -180,7 +182,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_cnt = 0; int baro_init_num = 200; - float baro_offset = 0.0f; // baro offset for reference altitude, initialized only once + float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate float alt_avg = 0.0f; @@ -192,9 +194,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) uint32_t accel_counter = 0; uint32_t baro_counter = 0; - bool ref_xy_inited = false; - hrt_abstime ref_xy_init_start = 0; - const hrt_abstime ref_xy_init_delay = 1000000; // wait for 1s after 3D fix + bool ref_inited = false; + hrt_abstime ref_init_start = 0; + const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix uint16_t accel_updates = 0; uint16_t baro_updates = 0; @@ -211,18 +213,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ - float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D - float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame - float baro_corr = 0.0f; // D - float gps_corr[2][2] = { + float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D + float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame + float corr_baro = 0.0f; // D + float corr_gps[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) + { 0.0f, 0.0f }, // D (pos, vel) }; - float sonar_corr = 0.0f; - float sonar_corr_filtered = 0.0f; + float w_gps_xy = 1.0f; + float w_gps_z = 1.0f; + float corr_sonar = 0.0f; + float corr_sonar_filtered = 0.0f; - float flow_corr[] = { 0.0f, 0.0f }; // X, Y - float flow_w = 0.0f; + float corr_flow[] = { 0.0f, 0.0f }; // N E + float w_flow = 0.0f; float sonar_prev = 0.0f; hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) @@ -307,13 +312,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("init ref: alt=%.3f", baro_offset); - mavlink_log_info(mavlink_fd, "[inav] init ref: alt=%.3f", baro_offset); - local_pos.ref_alt = baro_offset; - local_pos.ref_timestamp = hrt_absolute_time(); + corr_baro = -baro_offset; + warnx("baro offs: %.2f", baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; - local_pos.z_global = true; } } } @@ -370,10 +373,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (armed.armed && !flag_armed) { flag_armed = armed.armed; baro_offset -= z_est[0]; + corr_baro = -baro_offset; + local_pos.ref_alt -= z_est[0]; + local_pos.ref_timestamp = t; z_est[0] = 0.0f; alt_avg = 0.0f; - local_pos.ref_alt = baro_offset; - local_pos.ref_timestamp = t; } } @@ -386,9 +390,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (sensor.accelerometer_counter > accel_counter) { if (att.R_valid) { /* correct accel bias, now only for Z */ - sensor.accelerometer_m_s2[0] -= accel_bias[0]; - sensor.accelerometer_m_s2[1] -= accel_bias[1]; - sensor.accelerometer_m_s2[2] -= accel_bias[2]; + sensor.accelerometer_m_s2[0] -= acc_bias[0]; + sensor.accelerometer_m_s2[1] -= acc_bias[1]; + sensor.accelerometer_m_s2[2] -= acc_bias[2]; /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { @@ -399,12 +403,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - accel_corr[0] = accel_NED[0] - x_est[2]; - accel_corr[1] = accel_NED[1] - y_est[2]; - accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; + corr_acc[0] = accel_NED[0] - x_est[2]; + corr_acc[1] = accel_NED[1] - y_est[2]; + corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; } else { - memset(accel_corr, 0, sizeof(accel_corr)); + memset(corr_acc, 0, sizeof(corr_acc)); } accel_counter = sensor.accelerometer_counter; @@ -412,7 +416,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - baro_corr = baro_offset - sensor.baro_alt_meter - z_est[0]; + corr_baro = - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } @@ -427,22 +431,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; - sonar_corr = flow.ground_distance_m + surface_offset + z_est[0]; - sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; + corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt; - if (fabsf(sonar_corr) > params.sonar_err) { + if (fabsf(corr_sonar) > params.sonar_err) { /* correction is too large: spike or new ground level? */ - if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { + if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) { /* spike detected, ignore */ - sonar_corr = 0.0f; + corr_sonar = 0.0f; sonar_valid = false; } else { /* new ground level */ - surface_offset -= sonar_corr; + surface_offset -= corr_sonar; surface_offset_rate = 0.0f; - sonar_corr = 0.0f; - sonar_corr_filtered = 0.0f; + corr_sonar = 0.0f; + corr_sonar_filtered = 0.0f; sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; @@ -494,21 +498,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* velocity correction */ - flow_corr[0] = flow_v[0] - x_est[1]; - flow_corr[1] = flow_v[1] - y_est[1]; + corr_flow[0] = flow_v[0] - x_est[1]; + corr_flow[1] = flow_v[1] - y_est[1]; /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); - flow_w = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist); + w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist); /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) - flow_w *= 0.05f; + w_flow *= 0.05f; flow_valid = true; } else { - flow_w = 0.0f; + w_flow = 0.0f; flow_valid = false; } @@ -524,13 +528,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps.fix_type >= 3) { /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > 10.0f) { + if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < 5.0f) { + if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) { gps_valid = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } @@ -542,50 +546,58 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { /* initialize reference position if needed */ - if (!ref_xy_inited) { - if (ref_xy_init_start == 0) { - ref_xy_init_start = t; + if (!ref_inited) { + if (ref_init_start == 0) { + ref_init_start = t; - } else if (t > ref_xy_init_start + ref_xy_init_delay) { - ref_xy_inited = true; + } else if (t > ref_init_start + ref_init_delay) { + ref_inited = true; /* reference GPS position */ double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; + float alt = gps.alt * 1e-3; local_pos.ref_lat = gps.lat; local_pos.ref_lon = gps.lon; + local_pos.ref_alt = alt + z_est[0]; local_pos.ref_timestamp = t; /* initialize projection */ map_projection_init(lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f", lat, lon); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f", lat, lon); + warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); } } - if (ref_xy_inited) { + if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); /* calculate correction for position */ - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; + corr_gps[0][0] = gps_proj[0] - x_est[0]; + corr_gps[1][0] = gps_proj[1] - y_est[0]; + corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + corr_gps[0][1] = gps.vel_n_m_s - x_est[1]; + corr_gps[1][1] = gps.vel_e_m_s - y_est[1]; + corr_gps[2][1] = gps.vel_d_m_s - z_est[1]; } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; + corr_gps[0][1] = 0.0f; + corr_gps[1][1] = 0.0f; + corr_gps[2][1] = 0.0f; } + + w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m); + w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m); } } else { /* no GPS lock */ - memset(gps_corr, 0, sizeof(gps_corr)); - ref_xy_init_start = 0; + memset(corr_gps, 0, sizeof(corr_gps)); + ref_init_start = 0; } gps_updates++; @@ -609,7 +621,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* check for sonar measurement timeout */ if (sonar_valid && t > sonar_time + sonar_timeout) { - sonar_corr = 0.0f; + corr_sonar = 0.0f; sonar_valid = false; } @@ -617,12 +629,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) t_prev = t; /* use GPS if it's valid and reference position initialized */ - bool use_gps = ref_xy_inited && gps_valid && params.w_pos_gps_p > 0.00001f; + bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; + bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; /* use flow if it's valid and (accurate or no GPS available) */ - bool use_flow = flow_valid && (flow_accurate || !use_gps); + bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); /* try to estimate position during some time after position sources lost */ - if (use_gps || use_flow) { + if (use_gps_xy || use_flow) { xy_src_time = t; } @@ -636,36 +649,47 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* surface distance correction */ if (sonar_valid) { - surface_offset_rate -= sonar_corr * 0.5f * params.w_alt_sonar * params.w_alt_sonar * dt; - surface_offset -= sonar_corr * params.w_alt_sonar * dt; + surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt; + surface_offset -= corr_sonar * params.w_z_sonar * dt; } } - /* reduce GPS weight if optical flow is good */ - float w_pos_gps_p = params.w_pos_gps_p; - float w_pos_gps_v = params.w_pos_gps_v; + float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; + float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; + float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_z_gps_v = params.w_z_gps_v * w_gps_z; + /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { - w_pos_gps_p *= params.w_gps_flow; - w_pos_gps_v *= params.w_gps_flow; + w_xy_gps_p *= params.w_gps_flow; + w_xy_gps_v *= params.w_gps_flow; } + /* baro offset correction */ + if (use_gps_z) { + baro_offset += corr_gps[2][0] * w_z_gps_p * dt; + } + /* accelerometer bias correction */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; - if (use_gps) { - accel_bias_corr[0] -= gps_corr[0][0] * w_pos_gps_p * w_pos_gps_p; - accel_bias_corr[0] -= gps_corr[0][1] * w_pos_gps_v; - accel_bias_corr[1] -= gps_corr[1][0] * w_pos_gps_p * w_pos_gps_p; - accel_bias_corr[1] -= gps_corr[1][1] * w_pos_gps_v; + if (use_gps_xy) { + accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p; + accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v; + accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; + accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; + } + if (use_gps_z) { + accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } if (use_flow) { - accel_bias_corr[0] -= flow_corr[0] * params.w_pos_flow; - accel_bias_corr[1] -= flow_corr[1] * params.w_pos_flow; + accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; + accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; } - accel_bias_corr[2] -= baro_corr * params.w_alt_baro * params.w_alt_baro; + accel_bias_corr[2] -= (corr_baro + baro_offset) * params.w_z_baro * params.w_z_baro; /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { @@ -675,15 +699,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += att.R[j][i] * accel_bias_corr[j]; } - accel_bias[i] += c * params.w_acc_bias * dt; + acc_bias[i] += c * params.w_acc_bias * dt; } /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); /* inertial filter correction for altitude */ - inertial_filter_correct(baro_corr, dt, z_est, 0, params.w_alt_baro); - inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); + inertial_filter_correct(corr_baro + baro_offset, dt, z_est, 0, params.w_z_baro); + inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); + inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); if (can_estimate_xy) { /* inertial filter prediction for position */ @@ -696,27 +722,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* inertial filter correction for position */ - inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); - inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); + inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc); + inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc); if (use_flow) { - inertial_filter_correct(flow_corr[0], dt, x_est, 1, params.w_pos_flow * flow_w); - inertial_filter_correct(flow_corr[1], dt, y_est, 1, params.w_pos_flow * flow_w); + inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); + inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); } - if (use_gps) { - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, w_pos_gps_p); + if (use_gps_xy) { + inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); + inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_v); if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, w_pos_gps_v); + inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); + inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); } } if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { warnx("BAD ESTIMATE AFTER CORRECTION dt: %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]); - warnx("BAD ESTIMATE AFTER CORRECTION acc: %.3f %.3f gps x: %.3f %.3f gps y: %.3f %.3f", accel_corr[0], accel_corr[1], gps_corr[0][0], gps_corr[0][1], gps_corr[1][0], gps_corr[1][1]); + warnx("BAD ESTIMATE AFTER CORRECTION acc: %.3f %.3f gps x: %.3f %.3f gps y: %.3f %.3f", corr_acc[0], corr_acc[1], corr_gps[0][0], corr_gps[0][1], corr_gps[1][0], corr_gps[1][1]); thread_should_exit = true; } } @@ -775,9 +801,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t > pub_last + pub_interval) { pub_last = t; /* publish local position */ - local_pos.xy_valid = can_estimate_xy && use_gps; + local_pos.xy_valid = can_estimate_xy && use_gps_xy; local_pos.v_xy_valid = can_estimate_xy; - local_pos.xy_global = local_pos.xy_valid && use_gps; // will make sense when local position sources (e.g. vicon) will be implemented + local_pos.xy_global = local_pos.xy_valid && use_gps_xy; + local_pos.z_global = local_pos.z_valid && use_gps_z; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index b3f46fb99..77299bd71 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,13 +40,15 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f); -PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 20.0f); -PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.5f); +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); @@ -59,13 +61,15 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->w_alt_baro = param_find("INAV_W_ALT_BARO"); - h->w_alt_acc = param_find("INAV_W_ALT_ACC"); - h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); - h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); - h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); - h->w_pos_acc = param_find("INAV_W_POS_ACC"); - h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->w_z_baro = param_find("INAV_W_Z_BARO"); + h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_gps_v = param_find("INAV_W_Z_GPS_V"); + h->w_z_acc = param_find("INAV_W_Z_ACC"); + h->w_z_sonar = param_find("INAV_W_Z_SONAR"); + h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); + h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); + h->w_xy_acc = param_find("INAV_W_XY_ACC"); + h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); @@ -81,13 +85,15 @@ int parameters_init(struct position_estimator_inav_param_handles *h) int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->w_alt_baro, &(p->w_alt_baro)); - param_get(h->w_alt_acc, &(p->w_alt_acc)); - param_get(h->w_alt_sonar, &(p->w_alt_sonar)); - param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); - param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); - param_get(h->w_pos_acc, &(p->w_pos_acc)); - param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->w_z_baro, &(p->w_z_baro)); + param_get(h->w_z_gps_p, &(p->w_z_gps_p)); + param_get(h->w_z_gps_v, &(p->w_z_gps_v)); + param_get(h->w_z_acc, &(p->w_z_acc)); + param_get(h->w_z_sonar, &(p->w_z_sonar)); + param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); + param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); + param_get(h->w_xy_acc, &(p->w_xy_acc)); + param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index e394edfa4..f583f4b7d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -41,13 +41,15 @@ #include <systemlib/param/param.h> struct position_estimator_inav_params { - float w_alt_baro; - float w_alt_acc; - float w_alt_sonar; - float w_pos_gps_p; - float w_pos_gps_v; - float w_pos_acc; - float w_pos_flow; + float w_z_baro; + float w_z_gps_p; + float w_z_gps_v; + float w_z_acc; + float w_z_sonar; + float w_xy_gps_p; + float w_xy_gps_v; + float w_xy_acc; + float w_xy_flow; float w_gps_flow; float w_acc_bias; float flow_k; @@ -60,13 +62,15 @@ struct position_estimator_inav_params { }; struct position_estimator_inav_param_handles { - param_t w_alt_baro; - param_t w_alt_acc; - param_t w_alt_sonar; - param_t w_pos_gps_p; - param_t w_pos_gps_v; - param_t w_pos_acc; - param_t w_pos_flow; + param_t w_z_baro; + param_t w_z_gps_p; + param_t w_z_gps_v; + param_t w_z_acc; + param_t w_z_sonar; + param_t w_xy_gps_p; + param_t w_xy_gps_v; + param_t w_xy_acc; + param_t w_xy_flow; param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e2f412662..a144e1094 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -58,6 +58,7 @@ #include <systemlib/err.h> #include <unistd.h> #include <drivers/drv_hrt.h> +#include <math.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -84,12 +85,14 @@ #include <uORB/topics/esc_status.h> #include <systemlib/systemlib.h> +#include <systemlib/param/param.h> #include <mavlink/mavlink_log.h> #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++; \ @@ -180,8 +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 int write_version(int fd); + +/** + * Write parameters to log file. + */ +static int write_parameters(int fd); static bool file_exist(const char *filename); @@ -237,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); } @@ -354,10 +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 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; @@ -396,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; @@ -411,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; } @@ -479,34 +496,92 @@ 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 { LOG_PACKET_HEADER; struct log_format_s body; - } log_format_packet = { + } log_msg_format = { 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++) { - log_format_packet.body = log_formats[i]; - log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet)); + /* 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]; + written += write(fd, &log_msg_format, sizeof(log_msg_format)); } - fsync(fd); + return written; +} + +int write_version(int fd) +{ + /* construct version message */ + struct { + LOG_PACKET_HEADER; + struct log_VER_s body; + } log_msg_VER = { + LOG_PACKET_HEADER_INIT(LOG_VER_MSG), + }; + + /* 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)); + 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; + + 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[]) @@ -584,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 */ @@ -603,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! */ @@ -628,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 { @@ -799,7 +877,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)); @@ -890,7 +968,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); @@ -1160,8 +1238,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; i<buf.esc.esc_count; i++) - { + + for (uint8_t i = 0; i < buf.esc.esc_count; i++) { log_msg.msg_type = LOG_ESC_MSG; log_msg.body.log_ESC.counter = buf.esc.counter; log_msg.body.log_ESC.esc_count = buf.esc.esc_count; @@ -1309,6 +1387,7 @@ void handle_status(struct vehicle_status_s *status) { // TODO use flag from actuator_armed here? bool armed = status->arming_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 7e11940b7..ccc853eb9 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,38 +247,41 @@ struct log_GVSP_s { float vz; }; -/* --- FWRV - FIRMWARE REVISION --- */ -#define LOG_FWRV_MSG 20 -struct log_FWRV_s { - char fw_revision[64]; -}; - /* --- DIST - DISTANCE TO SURFACE --- */ -#define LOG_DIST_MSG 21 +#define LOG_DIST_MSG 20 struct log_DIST_s { float bottom; float bottom_rate; uint8_t flags; }; -#pragma pack(pop) +/********** SYSTEM MESSAGES, ID > 0x80 **********/ +/* --- TIME - TIME STAMP --- */ +#define LOG_TIME_MSG 129 +struct log_TIME_s { + uint64_t t; +}; -/* - 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) +/* --- VER - VERSION --- */ +#define LOG_VER_MSG 130 +struct log_VER_s { + char arch[16]; + char fw_git[64]; +}; -/* construct list of all message formats */ +/* --- 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"), @@ -303,8 +300,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"), - LOG_FORMAT(FWRV, "Z", FW_VERSION_STR), - LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), + /* 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); 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 <anton.babushkin@me.com> + * + * 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 <anton.babushkin@me.com> + */ + +#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_ */ 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; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 6f5c20d39..6f1741f69 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -44,8 +44,34 @@ #include <systemlib/param/param.h> +/** + * Gyro X offset FIXME + * + * This is an X-axis offset for the gyro. + * Adjust it according to the calibration data. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); + +/** + * Gyro Y offset FIXME with dot. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); + +/** + * Gyro Z offset FIXME + * + * @min -5.0 + * @max 5.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); @@ -194,8 +220,9 @@ PARAM_DEFINE_INT32(RC_MAP_DIST_B_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); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 94573d9c4..32cc25bc5 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -67,6 +67,7 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/perf_counter.h> +#include <conversion/rotation.h> #include <systemlib/airspeed.h> @@ -74,6 +75,7 @@ #include <uORB/topics/sensor_combined.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/battery_status.h> @@ -136,75 +138,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 * * @ingroup apps @@ -264,6 +197,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 */ @@ -387,11 +321,6 @@ private: int parameters_update(); /** - * Get the rotation matrices - */ - void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); - - /** * Do accel-related initialisation. */ void accel_init(); @@ -521,6 +450,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), @@ -812,24 +742,6 @@ Sensors::parameters_update() } 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() { int fd; @@ -1326,6 +1238,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; @@ -1502,6 +1415,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); @@ -1518,6 +1441,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); + } } } 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; } /** 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 <mavlink/mavlink_log.h> #include <uORB/topics/rc_channels.h> -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 +} |