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

Merge branch 'master' into vector_control2

This commit is contained in:
Anton Babushkin 2013-12-13 21:15:21 +04:00
parent 6f316b352d 0b9b68f0d9
commit deac4eefc6
19 changed files with 516 additions and 511 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
View File

@ -97,9 +97,9 @@ fi
# #
if [ $MKBLCTRL_FRAME == x ] if [ $MKBLCTRL_FRAME == x ]
then then
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix
else else
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix
fi fi
# #

3
Tools/px_uploader.py
View File

@ -370,6 +370,7 @@ class uploader(object):
self.port.close() self.port.close()
def send_reboot(self): def send_reboot(self):
try:
# try reboot via NSH first # try reboot via NSH first
self.__send(uploader.NSH_INIT) self.__send(uploader.NSH_INIT)
self.__send(uploader.NSH_REBOOT_BL) self.__send(uploader.NSH_REBOOT_BL)
@ -378,6 +379,8 @@ class uploader(object):
# then try MAVLINK command # then try MAVLINK command
self.__send(uploader.MAVLINK_REBOOT_ID1) self.__send(uploader.MAVLINK_REBOOT_ID1)
self.__send(uploader.MAVLINK_REBOOT_ID0) self.__send(uploader.MAVLINK_REBOOT_ID0)
except:
return

3
makefiles/config_px4fmu-v1_backside.mk
View File

@ -69,12 +69,13 @@ MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led MODULES += modules/gpio_led
# #
# Estimation modules (EKF / other filters) # Estimation modules (EKF/ SO3 / other filters)
# #
#MODULES += modules/attitude_estimator_ekf #MODULES += modules/attitude_estimator_ekf
MODULES += modules/att_pos_estimator_ekf MODULES += modules/att_pos_estimator_ekf
#MODULES += modules/position_estimator_inav #MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator MODULES += examples/flow_position_estimator
MODULES += modules/attitude_estimator_so3
# #
# Vehicle Control # Vehicle Control

3
makefiles/config_px4fmu-v1_default.mk
View File

@ -69,9 +69,10 @@ MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led MODULES += modules/gpio_led
# #
# Estimation modules (EKF / other filters) # Estimation modules (EKF/ SO3 / other filters)
# #
MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3
MODULES += modules/att_pos_estimator_ekf MODULES += modules/att_pos_estimator_ekf
MODULES += modules/position_estimator_inav MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator MODULES += examples/flow_position_estimator

3
makefiles/config_px4fmu-v2_default.mk
View File

@ -68,9 +68,10 @@ MODULES += modules/mavlink
MODULES += modules/mavlink_onboard MODULES += modules/mavlink_onboard
# #
# Estimation modules (EKF / other filters) # Estimation modules (EKF/ SO3 / other filters)
# #
MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3
MODULES += modules/att_pos_estimator_ekf MODULES += modules/att_pos_estimator_ekf
MODULES += modules/position_estimator_inav MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator MODULES += examples/flow_position_estimator

166
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -1,6 +1,7 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
* Author: Marco Bauer <marco@wtns.de>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -35,7 +36,8 @@
* @file mkblctrl.cpp * @file mkblctrl.cpp
* *
* Driver/configurator for the Mikrokopter BL-Ctrl. * Driver/configurator for the Mikrokopter BL-Ctrl.
* Marco Bauer *
* @author Marco Bauer <marco@wtns.de>
* *
*/ */
@ -88,8 +90,8 @@
#define BLCTRL_MIN_VALUE -0.920F #define BLCTRL_MIN_VALUE -0.920F
#define MOTOR_STATE_PRESENT_MASK 0x80 #define MOTOR_STATE_PRESENT_MASK 0x80
#define MOTOR_STATE_ERROR_MASK 0x7F #define MOTOR_STATE_ERROR_MASK 0x7F
#define MOTOR_SPINUP_COUNTER 2500 #define MOTOR_SPINUP_COUNTER 30
#define ESC_UORB_PUBLISH_DELAY 200 #define ESC_UORB_PUBLISH_DELAY 500000
class MK : public device::I2C class MK : public device::I2C
{ {
@ -111,7 +113,7 @@ public:
FRAME_X, FRAME_X,
}; };
MK(int bus); MK(int bus, const char *_device_path);
~MK(); ~MK();
virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual int ioctl(file *filp, int cmd, unsigned long arg);
@ -125,7 +127,7 @@ public:
int set_overrideSecurityChecks(bool overrideSecurityChecks); int set_overrideSecurityChecks(bool overrideSecurityChecks);
int set_px4mode(int px4mode); int set_px4mode(int px4mode);
int set_frametype(int frametype); int set_frametype(int frametype);
unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
private: private:
static const unsigned _max_actuators = MAX_MOTORS; static const unsigned _max_actuators = MAX_MOTORS;
@ -140,6 +142,7 @@ private:
unsigned int _motor; unsigned int _motor;
int _px4mode; int _px4mode;
int _frametype; int _frametype;
char _device[20]; ///< device
orb_advert_t _t_outputs; orb_advert_t _t_outputs;
orb_advert_t _t_esc_status; orb_advert_t _t_esc_status;
@ -242,7 +245,7 @@ MK *g_mk;
} // namespace } // namespace
MK::MK(int bus) : MK::MK(int bus, const char *_device_path) :
I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED), I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
_mode(MODE_NONE), _mode(MODE_NONE),
_update_rate(50), _update_rate(50),
@ -262,6 +265,10 @@ MK::MK(int bus) :
_armed(false), _armed(false),
_mixers(nullptr) _mixers(nullptr)
{ {
strncpy(_device, _device_path, sizeof(_device));
/* enforce null termination */
_device[sizeof(_device) - 1] = '\0';
_debug_enabled = true; _debug_enabled = true;
} }
@ -288,7 +295,7 @@ MK::~MK()
/* clean up the alternate device node */ /* clean up the alternate device node */
if (_primary_pwm_device) if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH); unregister_driver(_device);
g_mk = nullptr; g_mk = nullptr;
} }
@ -310,14 +317,16 @@ MK::init(unsigned motors)
usleep(500000); usleep(500000);
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */ if (sizeof(_device) > 0) {
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); ret = register_driver(_device, &fops, 0666, (void *)this);
if (ret == OK) { if (ret == OK) {
log("default PWM output device"); log("creating alternate output device");
_primary_pwm_device = true; _primary_pwm_device = true;
} }
}
/* reset GPIOs */ /* reset GPIOs */
gpio_reset(); gpio_reset();
@ -620,10 +629,7 @@ MK::task_main()
} }
/* output to BLCtrl's */ /* output to BLCtrl's */
if (_motortest == true) { if (_motortest != true) {
mk_servo_test(i);
} else {
//mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine //mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine
// 11 Bit // 11 Bit
Motor[i].SetPoint_PX4 = outputs.output[i]; Motor[i].SetPoint_PX4 = outputs.output[i];
@ -655,7 +661,7 @@ MK::task_main()
* Only update esc topic every half second. * Only update esc topic every half second.
*/ */
if (hrt_absolute_time() - esc.timestamp > 500000) { if (hrt_absolute_time() - esc.timestamp > ESC_UORB_PUBLISH_DELAY) {
esc.counter++; esc.counter++;
esc.timestamp = hrt_absolute_time(); esc.timestamp = hrt_absolute_time();
esc.esc_count = (uint8_t) _num_outputs; esc.esc_count = (uint8_t) _num_outputs;
@ -679,14 +685,21 @@ MK::task_main()
esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
esc.esc[i].esc_state = (uint16_t) Motor[i].State; esc.esc[i].esc_state = (uint16_t) Motor[i].State;
esc.esc[i].esc_errorcount = (uint16_t) 0; esc.esc[i].esc_errorcount = (uint16_t) 0;
// if motortest is requested - do it...
if (_motortest == true) {
mk_servo_test(i);
}
} }
orb_publish(ORB_ID(esc_status), _t_esc_status, &esc); orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
} }
} }
//::close(_t_esc_status); ::close(_t_esc_status);
::close(_t_actuators); ::close(_t_actuators);
::close(_t_actuator_armed); ::close(_t_actuator_armed);
@ -713,8 +726,12 @@ MK::mk_servo_arm(bool status)
unsigned int unsigned int
MK::mk_check_for_blctrl(unsigned int count, bool showOutput) MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
{ {
if(initI2C) {
I2C::init();
}
_retries = 50; _retries = 50;
uint8_t foundMotorCount = 0; uint8_t foundMotorCount = 0;
@ -938,6 +955,7 @@ MK::mk_servo_test(unsigned int chan)
if (_motor >= _num_outputs) { if (_motor >= _num_outputs) {
_motor = -1; _motor = -1;
_motortest = false; _motortest = false;
fprintf(stderr, "[mkblctrl] Motortest finished...\n");
} }
} }
@ -1353,7 +1371,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
/* count used motors */ /* count used motors */
do { do {
if (g_mk->mk_check_for_blctrl(8, false) != 0) { if (g_mk->mk_check_for_blctrl(8, false, false) != 0) {
shouldStop = 4; shouldStop = 4;
} else { } else {
@ -1363,7 +1381,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
sleep(1); sleep(1);
} while (shouldStop < 3); } while (shouldStop < 3);
g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true)); g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false));
/* (re)set the PWM output mode */ /* (re)set the PWM output mode */
g_mk->set_mode(servo_mode); g_mk->set_mode(servo_mode);
@ -1376,13 +1394,13 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
} }
int int
mk_start(unsigned bus, unsigned motors) mk_start(unsigned bus, unsigned motors, char *device_path)
{ {
int ret = OK; int ret = OK;
if (g_mk == nullptr) { if (g_mk == nullptr) {
g_mk = new MK(bus); g_mk = new MK(bus, device_path);
if (g_mk == nullptr) { if (g_mk == nullptr) {
ret = -ENOMEM; ret = -ENOMEM;
@ -1401,6 +1419,52 @@ mk_start(unsigned bus, unsigned motors)
} }
int
mk_check_for_i2c_esc_bus(char *device_path, int motors)
{
int ret;
if (g_mk == nullptr) {
g_mk = new MK(3, device_path);
if (g_mk == nullptr) {
return -1;
} else {
ret = g_mk->mk_check_for_blctrl(8, false, true);
delete g_mk;
g_mk = nullptr;
if (ret > 0) {
return 3;
}
}
g_mk = new MK(1, device_path);
if (g_mk == nullptr) {
return -1;
} else {
ret = g_mk->mk_check_for_blctrl(8, false, true);
delete g_mk;
g_mk = nullptr;
if (ret > 0) {
return 1;
}
}
}
return -1;
}
} // namespace } // namespace
extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
@ -1411,13 +1475,14 @@ mkblctrl_main(int argc, char *argv[])
PortMode port_mode = PORT_FULL_PWM; PortMode port_mode = PORT_FULL_PWM;
int pwm_update_rate_in_hz = UPDATE_RATE; int pwm_update_rate_in_hz = UPDATE_RATE;
int motorcount = 8; int motorcount = 8;
int bus = 1; int bus = -1;
int px4mode = MAPPING_PX4; int px4mode = MAPPING_PX4;
int frametype = FRAME_PLUS; // + plus is default int frametype = FRAME_PLUS; // + plus is default
bool motortest = false; bool motortest = false;
bool overrideSecurityChecks = false; bool overrideSecurityChecks = false;
bool showHelp = false; bool showHelp = false;
bool newMode = false; bool newMode = false;
char *devicepath = "";
/* /*
* optional parameters * optional parameters
@ -1477,28 +1542,49 @@ mkblctrl_main(int argc, char *argv[])
newMode = true; newMode = true;
} }
/* look for the optional device parameter */
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
if (argc > i + 1) {
devicepath = argv[i + 1];
newMode = true;
} else {
errx(1, "missing the devicename (-d)");
return 1;
}
}
} }
if (showHelp) { if (showHelp) {
fprintf(stderr, "mkblctrl: help:\n"); fprintf(stderr, "mkblctrl: help:\n");
fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n"); fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n");
fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n"); fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n");
fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n");
fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
fprintf(stderr, "\n");
fprintf(stderr, "Motortest:\n");
fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n");
fprintf(stderr, "mkblctrl -t\n");
fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
exit(1); exit(1);
} }
if (!motortest) {
if (g_mk == nullptr) { if (g_mk == nullptr) {
if (mk_start(bus, motorcount) != OK) { if (bus == -1) {
bus = mk_check_for_i2c_esc_bus(devicepath, motorcount);
}
if (bus != -1) {
if (mk_start(bus, motorcount, devicepath) != OK) {
errx(1, "failed to start the MK-BLCtrl driver"); errx(1, "failed to start the MK-BLCtrl driver");
}
} else { } else {
newMode = true; errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)");
} }
}
/* parameter set ? */ /* parameter set ? */
if (newMode) { if (newMode) {
@ -1506,7 +1592,19 @@ mkblctrl_main(int argc, char *argv[])
return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
} }
/* test, etc. here g*/ exit(0);
} else {
exit(1); errx(1, "MK-BLCtrl driver already running");
}
} else {
if (g_mk == nullptr) {
errx(1, "MK-BLCtrl driver not running. You have to start it first.");
} else {
g_mk->set_motor_test(motortest);
exit(0);
}
}
} }

2
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
// Rate limit PD + FF throttle // Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time // Calculate the throttle increment from the specified slew time
if (fabsf(_throttle_slewrate) < 0.01f) { if (fabsf(_throttle_slewrate) > 0.01f) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
_throttle_dem = constrain(_throttle_dem, _throttle_dem = constrain(_throttle_dem,

3
src/modules/attitude_estimator_so3/README Normal file
View File

@ -0,0 +1,3 @@
Synopsis
nsh> attitude_estimator_so3_comp start

421
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp → src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
View File

@ -1,7 +1,40 @@
/* /****************************************************************************
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
* *
* @file attitude_estimator_so3_comp_main.c * Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* 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 attitude_estimator_so3_main.cpp
* *
* Implementation of nonlinear complementary filters on the SO(3). * Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
@ -46,83 +79,80 @@
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
#include "attitude_estimator_so3_comp_params.h" #include "attitude_estimator_so3_params.h"
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif
extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]); extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */ static bool thread_running = false; /**< Deamon status flag */
static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */ static int attitude_estimator_so3_task; /**< Handle of deamon task / thread */
//! Auxiliary variables to reduce number of repeated operations
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */ static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
static bool bFilterInit = false;
//! Auxiliary variables to reduce number of repeated operations
static float q0q0, q0q1, q0q2, q0q3; static float q0q0, q0q1, q0q2, q0q3;
static float q1q1, q1q2, q1q3; static float q1q1, q1q2, q1q3;
static float q2q2, q2q3; static float q2q2, q2q3;
static float q3q3; static float q3q3;
static bool bFilterInit = false;
//! Serial packet related
static int uart;
static int baudrate;
/** /**
* Mainloop of attitude_estimator_so3_comp. * Mainloop of attitude_estimator_so3.
*/ */
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]); int attitude_estimator_so3_thread_main(int argc, char *argv[]);
/** /**
* Print the correct usage. * Print the correct usage.
*/ */
static void usage(const char *reason); static void usage(const char *reason);
/* Function prototypes */
float invSqrt(float number);
void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz);
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt);
static void static void
usage(const char *reason) usage(const char *reason)
{ {
if (reason) if (reason)
fprintf(stderr, "%s\n", reason); fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d <devicename>] [-b <baud rate>]\n" fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n");
"-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n"
"ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n");
exit(1); exit(1);
} }
/** /**
* The attitude_estimator_so3_comp app only briefly exists to start * The attitude_estimator_so3 app only briefly exists to start
* the background job. The stack size assigned in the * the background job. The stack size assigned in the
* Makefile does only apply to this management task. * Makefile does only apply to this management task.
* *
* The actual stack size should be set in the call * The actual stack size should be set in the call
* to task_create(). * to task_create().
*/ */
int attitude_estimator_so3_comp_main(int argc, char *argv[]) int attitude_estimator_so3_main(int argc, char *argv[])
{ {
if (argc < 1) if (argc < 1)
usage("missing command"); usage("missing command");
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (thread_running) { if (thread_running) {
printf("attitude_estimator_so3_comp already running\n"); warnx("already running\n");
/* this is not an error */ /* this is not an error */
exit(0); exit(0);
} }
thread_should_exit = false; thread_should_exit = false;
attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp", attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
12400, 14000,
attitude_estimator_so3_comp_thread_main, attitude_estimator_so3_thread_main,
(const char **)argv); (argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0); exit(0);
} }
@ -131,9 +161,9 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
while (thread_running){ while (thread_running){
usleep(200000); usleep(200000);
printf(".");
} }
printf("terminated.");
warnx("stopped");
exit(0); exit(0);
} }
@ -157,7 +187,8 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
//--------------------------------------------------------------------------------------------------- //---------------------------------------------------------------------------------------------------
// Fast inverse square-root // Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float number) { float invSqrt(float number)
{
volatile long i; volatile long i;
volatile float x, y; volatile float x, y;
volatile const float f = 1.5F; volatile const float f = 1.5F;
@ -221,22 +252,21 @@ void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, floa
q3q3 = q3 * q3; q3q3 = q3 * q3;
} }
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) { void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt)
{
float recipNorm; float recipNorm;
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
//! Make filter converge to initial solution faster // Make filter converge to initial solution faster
//! This function assumes you are in static position. // This function assumes you are in static position.
//! WARNING : in case air reboot, this can cause problem. But this is very // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.
//! unlikely happen. if(bFilterInit == false) {
if(bFilterInit == false)
{
NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz); NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
bFilterInit = true; bFilterInit = true;
} }
//! If magnetometer measurement is available, use it. //! If magnetometer measurement is available, use it.
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
float hx, hy, hz, bx, bz; float hx, hy, hz, bx, bz;
float halfwx, halfwy, halfwz; float halfwx, halfwy, halfwz;
@ -250,7 +280,7 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
// Reference direction of Earth's magnetic field // Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2); hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
bx = sqrt(hx * hx + hy * hy); bx = sqrt(hx * hx + hy * hy);
bz = hz; bz = hz;
@ -293,7 +323,9 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki
gyro_bias[1] += twoKi * halfey * dt; gyro_bias[1] += twoKi * halfey * dt;
gyro_bias[2] += twoKi * halfez * dt; gyro_bias[2] += twoKi * halfez * dt;
gx += gyro_bias[0]; // apply integral feedback
// apply integral feedback
gx += gyro_bias[0];
gy += gyro_bias[1]; gy += gyro_bias[1];
gz += gyro_bias[2]; gz += gyro_bias[2];
} }
@ -349,196 +381,31 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
q3q3 = q3 * q3; q3q3 = q3 * q3;
} }
void send_uart_byte(char c)
{
write(uart,&c,1);
}
void send_uart_bytes(uint8_t *data, int length)
{
write(uart,data,(size_t)(sizeof(uint8_t)*length));
}
void send_uart_float(float f) {
uint8_t * b = (uint8_t *) &f;
//! Assume float is 4-bytes
for(int i=0; i<4; i++) {
uint8_t b1 = (b[i] >> 4) & 0x0f;
uint8_t b2 = (b[i] & 0x0f);
uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
send_uart_bytes(&c1,1);
send_uart_bytes(&c2,1);
}
}
void send_uart_float_arr(float *arr, int length)
{
for(int i=0;i<length;++i)
{
send_uart_float(arr[i]);
send_uart_byte(',');
}
}
int open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
{
int speed;
switch (baud) {
case 0: speed = B0; break;
case 50: speed = B50; break;
case 75: speed = B75; break;
case 110: speed = B110; break;
case 134: speed = B134; break;
case 150: speed = B150; break;
case 200: speed = B200; break;
case 300: speed = B300; break;
case 600: speed = B600; break;
case 1200: speed = B1200; break;
case 1800: speed = B1800; break;
case 2400: speed = B2400; break;
case 4800: speed = B4800; break;
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
case 460800: speed = B460800; break;
case 921600: speed = B921600; break;
default:
printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
return -EINVAL;
}
printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud);
uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
*is_usb = false;
/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
} else {
*is_usb = true;
}
return uart;
}
/* /*
* [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) * Nonliner complementary filter on SO(3), attitude estimator main function.
*/
/*
* EKF Attitude Estimator main function.
* *
* Estimates the attitude recursively once started. * Estimates the attitude once started.
* *
* @param argc number of commandline arguments (plus command name) * @param argc number of commandline arguments (plus command name)
* @param argv strings containing the arguments * @param argv strings containing the arguments
*/ */
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]) int attitude_estimator_so3_thread_main(int argc, char *argv[])
{ {
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
//! Serial debug related
int ch;
struct termios uart_config_original;
bool usb_uart;
bool debug_mode = false;
char *device_name = "/dev/ttyS2";
baudrate = 115200;
//! Time constant //! Time constant
float dt = 0.005f; float dt = 0.005f;
/* output euler angles */ /* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f}; float euler[3] = {0.0f, 0.0f, 0.0f};
float Rot_matrix[9] = {1.f, 0, 0, /* Initialization */
0, 1.f, 0, float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */
0, 0, 1.f
}; /**< init: identity matrix */
float acc[3] = {0.0f, 0.0f, 0.0f}; float acc[3] = {0.0f, 0.0f, 0.0f};
float gyro[3] = {0.0f, 0.0f, 0.0f}; float gyro[3] = {0.0f, 0.0f, 0.0f};
float mag[3] = {0.0f, 0.0f, 0.0f}; float mag[3] = {0.0f, 0.0f, 0.0f};
/* work around some stupidity in task_create's argv handling */ warnx("main thread started");
argc -= 2;
argv += 2;
//! -d <device_name>, default : /dev/ttyS2
//! -b <baud_rate>, default : 115200
while ((ch = getopt(argc,argv,"d:b:")) != EOF){
switch(ch){
case 'b':
baudrate = strtoul(optarg, NULL, 10);
if(baudrate == 0)
printf("invalid baud rate '%s'",optarg);
break;
case 'd':
device_name = optarg;
debug_mode = true;
break;
default:
usage("invalid argument");
}
}
if(debug_mode){
printf("Opening debugging port for 3D visualization\n");
uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0)
printf("could not open %s", device_name);
else
printf("Open port success\n");
}
// print text
printf("Nonlinear SO3 Attitude Estimator initialized..\n\n");
fflush(stdout);
int overloadcounter = 19;
/* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time();
struct sensor_combined_s raw; struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw)); memset(&raw, 0, sizeof(raw));
@ -555,8 +422,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to raw data */ /* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* rate-limit raw data updates to 200Hz */ /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
orb_set_interval(sub_raw, 4); orb_set_interval(sub_raw, 3);
/* subscribe to param changes */ /* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update)); int sub_params = orb_subscribe(ORB_ID(parameter_update));
@ -565,17 +432,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */ /* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); //orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
//orb_advert_t att_pub = -1;
orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0; int loopcounter = 0;
int printcounter = 0; int printcounter = 0;
thread_running = true; thread_running = true;
/* advertise debug value */
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
// orb_advert_t pub_dbg = -1;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs // XXX write this out to perf regs
@ -583,20 +448,22 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
uint32_t sensor_last_count[3] = {0, 0, 0}; uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_so3_comp_params so3_comp_params; struct attitude_estimator_so3_params so3_comp_params;
struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles; struct attitude_estimator_so3_param_handles so3_comp_param_handles;
/* initialize parameter handles */ /* initialize parameter handles */
parameters_init(&so3_comp_param_handles); parameters_init(&so3_comp_param_handles);
parameters_update(&so3_comp_param_handles, &so3_comp_params);
uint64_t start_time = hrt_absolute_time(); uint64_t start_time = hrt_absolute_time();
bool initialized = false; bool initialized = false;
bool state_initialized = false;
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
unsigned offset_count = 0; unsigned offset_count = 0;
/* register the perf counter */ /* register the perf counter */
perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp"); perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3");
/* Main loop*/ /* Main loop*/
while (!thread_should_exit) { while (!thread_should_exit) {
@ -615,12 +482,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
if (!control_mode.flag_system_hil_enabled) { if (!control_mode.flag_system_hil_enabled) {
fprintf(stderr, warnx("WARNING: Not getting sensors - sensor app running?");
"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
} }
} else { } else {
/* only update parameters if they changed */ /* only update parameters if they changed */
if (fds[1].revents & POLLIN) { if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */ /* read from param to clear updated flag */
@ -644,11 +508,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
gyro_offsets[2] += raw.gyro_rad_s[2]; gyro_offsets[2] += raw.gyro_rad_s[2];
offset_count++; offset_count++;
if (hrt_absolute_time() - start_time > 3000000LL) { if (hrt_absolute_time() > start_time + 3000000l) {
initialized = true; initialized = true;
gyro_offsets[0] /= offset_count; gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count; gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count; gyro_offsets[2] /= offset_count;
warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]);
} }
} else { } else {
@ -696,31 +561,14 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
mag[1] = raw.magnetometer_ga[1]; mag[1] = raw.magnetometer_ga[1];
mag[2] = raw.magnetometer_ga[2]; mag[2] = raw.magnetometer_ga[2];
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
// if (overloadcounter == 20) {
// printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
// overloadcounter = 0;
// }
overloadcounter++;
}
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */ /* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05f && dt > 0.005f) { if (!state_initialized && dt < 0.05f && dt > 0.001f) {
dt = 0.005f; state_initialized = true;
parameters_update(&so3_comp_param_handles, &so3_comp_params); warnx("state initialized");
const_initialized = true;
} }
/* do not execute the filter if not initialized */ /* do not execute the filter if not initialized */
if (!const_initialized) { if (!state_initialized) {
continue; continue;
} }
@ -728,17 +576,22 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// NOTE : Accelerometer is reversed. // NOTE : Accelerometer is reversed.
// Because proper mount of PX4 will give you a reversed accelerometer readings. // Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],
-acc[0], -acc[1], -acc[2],
mag[0], mag[1], mag[2],
so3_comp_params.Kp,
so3_comp_params.Ki,
dt);
// Convert q->R, This R converts inertial frame to body frame. // Convert q->R, This R converts inertial frame to body frame.
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13
Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3); // 21 Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1); // 23 Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23
Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2); // 31 Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31
Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1); // 32 Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
//1-2-3 Representation. //1-2-3 Representation.
@ -751,25 +604,38 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* swap values for next iteration, check for fatal inputs */ /* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
/* Do something */ // Publish only finite euler angles
att.roll = euler[0] - so3_comp_params.roll_off;
att.pitch = euler[1] - so3_comp_params.pitch_off;
att.yaw = euler[2] - so3_comp_params.yaw_off;
} else { } else {
/* due to inputs or numerical failure the output is invalid, skip it */ /* due to inputs or numerical failure the output is invalid, skip it */
// Due to inputs or numerical failure the output is invalid
warnx("infinite euler angles, rotation matrix:");
warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]);
warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]);
warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]);
// Don't publish anything
continue; continue;
} }
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data); if (last_data > 0 && raw.timestamp > last_data + 12000) {
warnx("sensor data missed");
}
last_data = raw.timestamp; last_data = raw.timestamp;
/* send out */ /* send out */
att.timestamp = raw.timestamp; att.timestamp = raw.timestamp;
// XXX Apply the same transformation to the rotation matrix // Quaternion
att.roll = euler[0] - so3_comp_params.roll_off; att.q[0] = q0;
att.pitch = euler[1] - so3_comp_params.pitch_off; att.q[1] = q1;
att.yaw = euler[2] - so3_comp_params.yaw_off; att.q[2] = q2;
att.q[3] = q3;
att.q_valid = true;
//! Euler angle rate. But it needs to be investigated again. // Euler angle rate. But it needs to be investigated again.
/* /*
att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3);
att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3);
@ -783,13 +649,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
att.pitchacc = 0; att.pitchacc = 0;
att.yawacc = 0; att.yawacc = 0;
//! Quaternion
att.q[0] = q0;
att.q[1] = q1;
att.q[2] = q2;
att.q[3] = q3;
att.q_valid = true;
/* TODO: Bias estimation required */ /* TODO: Bias estimation required */
memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
@ -797,39 +656,23 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
memcpy(&att.R, Rot_matrix, sizeof(float)*9); memcpy(&att.R, Rot_matrix, sizeof(float)*9);
att.R_valid = true; att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Publish
// Broadcast if (att_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
} else { } else {
warnx("NaN in roll/pitch/yaw estimate!"); warnx("NaN in roll/pitch/yaw estimate!");
orb_advertise(ORB_ID(vehicle_attitude), &att);
} }
perf_end(so3_comp_loop_perf); perf_end(so3_comp_loop_perf);
//! This will print out debug packet to visualization software
if(debug_mode)
{
float quat[4];
quat[0] = q0;
quat[1] = q1;
quat[2] = q2;
quat[3] = q3;
send_uart_float_arr(quat,4);
send_uart_byte('\n');
}
} }
} }
} }
loopcounter++; loopcounter++;
}// while }
thread_running = false; thread_running = false;
/* Reset the UART flags to original state */
if (!usb_uart)
tcsetattr(uart, TCSANOW, &uart_config_original);
return 0; return 0;
} }

86
src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c Executable file
View File

@ -0,0 +1,86 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* 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 attitude_estimator_so3_params.c
*
* Parameters for nonlinear complementary filters on the SO(3).
*/
#include "attitude_estimator_so3_params.h"
/* This is filter gain for nonlinear SO3 complementary filter */
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
will compensate gyro bias which depends on temperature and vibration of your vehicle */
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
//! You can set this gain higher if you want more fast response.
//! But note that higher gain will give you also higher overshoot.
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
//! This gain is depend on your vehicle status.
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_so3_param_handles *h)
{
/* Filter gain parameters */
h->Kp = param_find("SO3_COMP_KP");
h->Ki = param_find("SO3_COMP_KI");
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
h->roll_off = param_find("SO3_ROLL_OFFS");
h->pitch_off = param_find("SO3_PITCH_OFFS");
h->yaw_off = param_find("SO3_YAW_OFFS");
return OK;
}
int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p)
{
/* Update filter gain */
param_get(h->Kp, &(p->Kp));
param_get(h->Ki, &(p->Ki));
/* Update attitude offset */
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}

67
src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h Executable file
View File

@ -0,0 +1,67 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* 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 attitude_estimator_so3_params.h
*
* Parameters for nonlinear complementary filters on the SO(3).
*/
#include <systemlib/param/param.h>
struct attitude_estimator_so3_params {
float Kp;
float Ki;
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_so3_param_handles {
param_t Kp, Ki;
param_t roll_off, pitch_off, yaw_off;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_so3_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p);

8
src/modules/attitude_estimator_so3/module.mk Normal file
View File

@ -0,0 +1,8 @@
#
# Attitude estimator (Nonlinear SO(3) complementary Filter)
#
MODULE_COMMAND = attitude_estimator_so3
SRCS = attitude_estimator_so3_main.cpp \
attitude_estimator_so3_params.c

5
src/modules/attitude_estimator_so3_comp/README
View File

@ -1,5 +0,0 @@
Synopsis
nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200
Option -d is for debugging packet. See code for detailed packet structure.

63
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
View File

@ -1,63 +0,0 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.c
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include "attitude_estimator_so3_comp_params.h"
/* This is filter gain for nonlinear SO3 complementary filter */
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
will compensate gyro bias which depends on temperature and vibration of your vehicle */
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
//! You can set this gain higher if you want more fast response.
//! But note that higher gain will give you also higher overshoot.
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
//! This gain is depend on your vehicle status.
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h)
{
/* Filter gain parameters */
h->Kp = param_find("SO3_COMP_KP");
h->Ki = param_find("SO3_COMP_KI");
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
h->roll_off = param_find("ATT_ROLL_OFFS");
h->pitch_off = param_find("ATT_PITCH_OFFS");
h->yaw_off = param_find("ATT_YAW_OFFS");
return OK;
}
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p)
{
/* Update filter gain */
param_get(h->Kp, &(p->Kp));
param_get(h->Ki, &(p->Ki));
/* Update attitude offset */
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}

44
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h
View File

@ -1,44 +0,0 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.h
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include <systemlib/param/param.h>
struct attitude_estimator_so3_comp_params {
float Kp;
float Ki;
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_so3_comp_param_handles {
param_t Kp, Ki;
param_t roll_off, pitch_off, yaw_off;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p);

8
src/modules/attitude_estimator_so3_comp/module.mk
View File

@ -1,8 +0,0 @@
#
# Attitude estimator (Nonlinear SO3 complementary Filter)
#
MODULE_COMMAND = attitude_estimator_so3_comp
SRCS = attitude_estimator_so3_comp_main.cpp \
attitude_estimator_so3_comp_params.c

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

@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[])
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
if (status.condition_local_altitude_valid) { if (status.is_rotary_wing && status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) { if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed; status.condition_landed = local_position.landed;
status_changed = true; status_changed = true;
@ -1539,7 +1539,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
// TODO AUTO_LAND handling // TODO AUTO_LAND handling
if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* don't switch to other states until takeoff not completed */ /* don't switch to other states until takeoff not completed */
if (local_pos->z > -takeoff_alt || status->condition_landed) { // XXX: only respect the condition_landed when the local position is actually valid
if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
return TRANSITION_NOT_CHANGED; return TRANSITION_NOT_CHANGED;
} }
} }
@ -1549,7 +1550,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
/* possibly on ground, switch to TAKEOFF if needed */ /* possibly on ground, switch to TAKEOFF if needed */
if (local_pos->z > -takeoff_alt || status->condition_landed) { if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
return res; return res;
} }

4
src/modules/fw_att_control/fw_att_control_params.c
View File

@ -33,9 +33,9 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file fw_pos_control_l1_params.c * @file fw_att_control_params.c
* *
* Parameters defined by the L1 position control task * Parameters defined by the fixed-wing attitude control task
* *
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
*/ */

13
src/modules/mavlink/orb_listener.c
View File

@ -250,6 +250,19 @@ l_vehicle_attitude(const struct listener *l)
float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
} }
/* send quaternion values if it exists */
if(att.q_valid) {
mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
att.q[0],
att.q[1],
att.q[2],
att.q[3],
att.rollspeed,
att.pitchspeed,
att.yawspeed);
}
} }
attitude_counter++; attitude_counter++;