diff options
Diffstat (limited to 'src')
99 files changed, 1728 insertions, 499 deletions
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 4a6f30a4f..187820372 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -115,7 +115,7 @@ int ardrone_interface_main(int argc, char *argv[]) } thread_should_exit = false; - ardrone_interface_task = task_spawn("ardrone_interface", + ardrone_interface_task = task_spawn_cmd("ardrone_interface", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 15, 2048, diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c index 69edc23ab..212a92cfa 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -58,7 +58,7 @@ #include <nuttx/mmcsd.h> #include <nuttx/analog/adc.h> -#include "stm32_internal.h" +#include "stm32.h" #include "px4fmu_internal.h" #include "stm32_uart.h" diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h index 671a58f8f..383a046ff 100644 --- a/src/drivers/boards/px4fmu/px4fmu_internal.h +++ b/src/drivers/boards/px4fmu/px4fmu_internal.h @@ -50,7 +50,7 @@ __BEGIN_DECLS /* these headers are not C++ safe */ -#include <stm32_internal.h> +#include <stm32.h> /**************************************************************************************************** diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu/px4fmu_led.c index 34fd194c3..31b25984e 100644 --- a/src/drivers/boards/px4fmu/px4fmu_led.c +++ b/src/drivers/boards/px4fmu/px4fmu_led.c @@ -41,7 +41,7 @@ #include <stdbool.h> -#include "stm32_internal.h" +#include "stm32.h" #include "px4fmu_internal.h" #include <arch/board/board.h> diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c index cb8918306..d85131dd8 100644 --- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c @@ -46,7 +46,7 @@ #include <arch/board/board.h> #include <drivers/drv_pwm_output.h> -#include <stm32_internal.h> +#include <stm32.h> #include <stm32_gpio.h> #include <stm32_tim.h> diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu/px4fmu_spi.c index b5d00eac0..e05ddecf3 100644 --- a/src/drivers/boards/px4fmu/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu/px4fmu_spi.c @@ -52,7 +52,7 @@ #include "up_arch.h" #include "chip.h" -#include "stm32_internal.h" +#include "stm32.h" #include "px4fmu_internal.h" /************************************************************************************ diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu/px4fmu_usb.c index b0b669fbe..0be981c1e 100644 --- a/src/drivers/boards/px4fmu/px4fmu_usb.c +++ b/src/drivers/boards/px4fmu/px4fmu_usb.c @@ -52,7 +52,7 @@ #include <nuttx/usb/usbdev_trace.h> #include "up_arch.h" -#include "stm32_internal.h" +#include "stm32.h" #include "px4fmu_internal.h" /************************************************************************************ diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io/px4io_init.c index d36353c6f..15c59e423 100644 --- a/src/drivers/boards/px4io/px4io_init.c +++ b/src/drivers/boards/px4io/px4io_init.c @@ -54,7 +54,7 @@ #include <nuttx/arch.h> -#include "stm32_internal.h" +#include "stm32.h" #include "px4io_internal.h" #include "stm32_uart.h" diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io/px4io_internal.h index eb2820bb7..6638e715e 100644 --- a/src/drivers/boards/px4io/px4io_internal.h +++ b/src/drivers/boards/px4io/px4io_internal.h @@ -47,7 +47,7 @@ #include <nuttx/compiler.h> #include <stdint.h> -#include <stm32_internal.h> +#include <stm32.h> /************************************************************************************ * Definitions diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io/px4io_pwm_servo.c index a2f73c429..6df470da6 100644 --- a/src/drivers/boards/px4io/px4io_pwm_servo.c +++ b/src/drivers/boards/px4io/px4io_pwm_servo.c @@ -46,7 +46,7 @@ #include <arch/board/board.h> #include <drivers/drv_pwm_output.h> -#include <stm32_internal.h> +#include <stm32.h> #include <stm32_gpio.h> #include <stm32_tim.h> diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 92d184326..7e3998fca 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 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 @@ -42,7 +42,7 @@ #include <sys/ioctl.h> -#ifdef CONFIG_ARCH_BOARD_PX4FMU +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* * PX4FMU GPIO numbers. * @@ -67,7 +67,7 @@ #endif -#ifdef CONFIG_ARCH_BOARD_PX4IO +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 /* * PX4IO GPIO numbers. * diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index c39da98d7..b34d3fa5d 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -84,8 +84,9 @@ /** * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. + * You can set this value to 12 if you want a zero reading below 15km/h. */ -#define MIN_ACCURATE_DIFF_PRES_PA 12 +#define MIN_ACCURATE_DIFF_PRES_PA 0 /* Measurement rate is 100Hz */ #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ @@ -463,8 +464,8 @@ ETSAirspeed::collect() uint16_t diff_pres_pa = val[1] << 8 | val[0]; + // XXX move the parameter read out of the driver. param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); - if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { diff_pres_pa = 0; diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index e851d8a52..15fbf8c88 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -225,7 +225,7 @@ HIL::init() // gpio_reset(); /* start the HIL interface task */ - _task = task_spawn("fmuhil", + _task = task_spawn_cmd("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c index 1d2bdd92e..4699ce5bf 100644 --- a/src/drivers/hott_telemetry/hott_telemetry_main.c +++ b/src/drivers/hott_telemetry/hott_telemetry_main.c @@ -271,7 +271,7 @@ hott_telemetry_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn(daemon_name, + deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 2048, diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c index d2634ef41..0ce56acef 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott_telemetry/messages.c @@ -108,7 +108,7 @@ build_eam_response(uint8_t *buffer, size_t *size) memset(&airspeed, 0, sizeof(airspeed)); orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); - uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s); + uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f); msg.speed_L = (uint8_t)speed & 0xff; msg.speed_H = (uint8_t)(speed >> 8) & 0xff; diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index 80850e708..e62c46b0d 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -109,7 +109,7 @@ int md25_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("md25", + deamon_task = task_spawn_cmd("md25", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 093b53d42..f187f22a6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -314,7 +314,7 @@ MK::init(unsigned motors) gpio_reset(); /* start the IO interface task */ - _task = task_spawn("mkblctrl", + _task = task_spawn_cmd("mkblctrl", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index db4c87ddc..05951f74c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -240,7 +240,7 @@ PX4FMU::init() gpio_reset(); /* start the IO interface task */ - _task = task_spawn("fmuservo", + _task = task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp index 15524c3ae..9e3f041e3 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/uploader.cpp @@ -49,6 +49,7 @@ #include <unistd.h> #include <fcntl.h> #include <poll.h> +#include <termios.h> #include <sys/stat.h> #include "uploader.h" @@ -121,6 +122,12 @@ PX4IO_Uploader::upload(const char *filenames[]) return -errno; } + /* adjust line speed to match bootloader */ + struct termios t; + tcgetattr(_io_fd, &t); + cfsetspeed(&t, 115200); + tcsetattr(_io_fd, TCSANOW, &t); + /* look for the bootloader */ ret = sync(); @@ -251,7 +258,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) int ret = ::poll(&fds[0], 1, timeout); if (ret < 1) { - log("poll timeout %d", ret); + //log("poll timeout %d", ret); return -ETIMEDOUT; } diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 911def943..1020eb946 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -58,7 +58,7 @@ #include <drivers/drv_adc.h> #include <arch/stm32/chip.h> -#include <stm32_internal.h> +#include <stm32.h> #include <stm32_gpio.h> #include <systemlib/err.h> diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index fd63681e3..7ef3db970 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -66,7 +66,7 @@ #include "up_internal.h" #include "up_arch.h" -#include "stm32_internal.h" +#include "stm32.h" #include "stm32_gpio.h" #include "stm32_tim.h" @@ -278,8 +278,10 @@ static void hrt_call_invoke(void); #ifdef CONFIG_HRT_PPM /* * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. + * + * Note that we assume that M3 means STM32F1 (since we don't really care about the F2). */ -# ifndef GTIM_CCER_CC1NP +# ifdef CONFIG_ARCH_CORTEXM3 # define GTIM_CCER_CC1NP 0 # define GTIM_CCER_CC2NP 0 # define GTIM_CCER_CC3NP 0 diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index c1efb8515..7b060412c 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -64,7 +64,7 @@ #include <up_internal.h> #include <up_arch.h> -#include <stm32_internal.h> +#include <stm32.h> #include <stm32_gpio.h> #include <stm32_tim.h> diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index e8c6fd542..e1409a815 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -111,7 +111,7 @@ #include <up_internal.h> #include <up_arch.h> -#include <stm32_internal.h> +#include <stm32.h> #include <stm32_gpio.h> #include <stm32_tim.h> diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 6f52c60bb..ae33a3702 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -531,7 +531,7 @@ int ex_fixedwing_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("ex_fixedwing_control", + deamon_task = task_spawn_cmd("ex_fixedwing_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/examples/flow_position_control/flow_position_control_params.c b/src/examples/flow_position_control/flow_position_control_params.c index 4f3598a57..eb1473647 100644 --- a/src/examples/flow_position_control/flow_position_control_params.c +++ b/src/examples/flow_position_control/flow_position_control_params.c @@ -40,14 +40,25 @@ #include "flow_position_control_params.h" /* controller parameters */ + +// Position control P gain PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f); +// Position control D / damping gain PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f); +// Altitude control P gain PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f); +// Altitude control I (integrator) gain PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f); +// Altitude control D gain PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f); +// Altitude control rate limiter PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f); +// Altitude control minimum altitude PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f); +// Altitude control maximum altitude (higher than 1.5m is untested) PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f); +// Altitude control feed forward throttle - adjust to the +// throttle position (0..1) where the copter hovers in manual flight PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f); PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f); diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index c568aaadc..53f1b4a9a 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -95,7 +95,7 @@ int px4_daemon_app_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn("daemon", + daemon_task = task_spawn_cmd("daemon", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 4096, diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index c3836bdfa..97d7fdd75 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 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 @@ -40,6 +40,7 @@ #include <poll.h> #include "KalmanNav.hpp" +#include <systemlib/err.h> // constants // Titterton pg. 52 @@ -58,14 +59,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : P0(9, 9), V(6, 6), // attitude measurement ekf matrices - HAtt(6, 9), - RAtt(6, 6), + HAtt(4, 9), + RAtt(4, 4), // position measurement ekf matrices HPos(6, 9), RPos(6, 6), // attitude representations C_nb(), q(), + _accel_sub(-1), + _gyro_sub(-1), + _mag_sub(-1), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz @@ -123,8 +127,19 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : lon = 0.0f; alt = 0.0f; - // initialize quaternions - q = Quaternion(EulerAngles(phi, theta, psi)); + // gyro, accel and mag subscriptions + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + + struct accel_report accel; + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel); + + struct mag_report mag; + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag); + + // initialize rotation quaternion with a single raw sensor measurement + q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z); // initialize dcm C_nb = Dcm(q); @@ -141,6 +156,45 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : updateParams(); } +math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz) +{ + float initialRoll, initialPitch; + float cosRoll, sinRoll, cosPitch, sinPitch; + float magX, magY; + float initialHdg, cosHeading, sinHeading; + + initialRoll = atan2(-ay, -az); + initialPitch = atan2(ax, -az); + + cosRoll = cosf(initialRoll); + sinRoll = sinf(initialRoll); + cosPitch = cosf(initialPitch); + sinPitch = sinf(initialPitch); + + magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; + + magY = my * cosRoll - mz * sinRoll; + + initialHdg = atan2f(-magY, magX); + + cosRoll = cosf(initialRoll * 0.5f); + sinRoll = sinf(initialRoll * 0.5f); + + cosPitch = cosf(initialPitch * 0.5f); + sinPitch = sinf(initialPitch * 0.5f); + + cosHeading = cosf(initialHdg * 0.5f); + sinHeading = sinf(initialHdg * 0.5f); + + float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + return math::Quaternion(q0, q1, q2, q3); + +} + void KalmanNav::update() { using namespace math; @@ -181,8 +235,8 @@ void KalmanNav::update() if (correctAtt() == ret_ok) _attitudeInitCounter++; if (_attitudeInitCounter > 100) { - printf("[kalman_demo] initialized EKF attitude\n"); - printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + warnx("initialized EKF attitude\n"); + warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", double(phi), double(theta), double(psi)); _attitudeInitialized = true; } @@ -202,13 +256,13 @@ void KalmanNav::update() setLonDegE7(_gps.lon); setAltE3(_gps.alt); _positionInitialized = true; - printf("[kalman_demo] initialized EKF state with GPS\n"); - printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + warnx("initialized EKF state with GPS\n"); + warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", double(vN), double(vE), double(vD), lat, lon, alt); } - // prediciton step + // prediction step // using sensors timestamp so we can account for packet lag float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; //printf("dt: %15.10f\n", double(dt)); @@ -233,7 +287,7 @@ void KalmanNav::update() // attitude correction step if (_attitudeInitialized // initialized && sensorsUpdate // new data - && _sensors.timestamp - _attTimeStamp > 1e6 / 20 // 20 Hz + && _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz ) { _attTimeStamp = _sensors.timestamp; correctAtt(); @@ -480,26 +534,32 @@ int KalmanNav::correctAtt() // trig float cosPhi = cosf(phi); float cosTheta = cosf(theta); - float cosPsi = cosf(psi); + // float cosPsi = cosf(psi); float sinPhi = sinf(phi); float sinTheta = sinf(theta); - float sinPsi = sinf(psi); - - // mag measurement - Vector3 zMag(_sensors.magnetometer_ga); - //float magNorm = zMag.norm(); - zMag = zMag.unit(); + // float sinPsi = sinf(psi); // mag predicted measurement // choosing some typical magnetic field properties, // TODO dip/dec depend on lat/ lon/ time - float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level + //float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north - float bN = cosf(dip) * cosf(dec); - float bE = cosf(dip) * sinf(dec); - float bD = sinf(dip); - Vector3 bNav(bN, bE, bD); - Vector3 zMagHat = (C_nb.transpose() * bNav).unit(); + + // compensate roll and pitch, but not yaw + // XXX take the vectors out of the C_nb matrix to avoid singularities + math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose(); + + // mag measurement + Vector3 magBody(_sensors.magnetometer_ga); + + // transform to earth frame + Vector3 magNav = C_rp * magBody; + + // calculate error between estimate and measurement + // apply declination correction for true heading as well. + float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec; + if (yMag > M_PI_F) yMag -= 2*M_PI_F; + if (yMag < -M_PI_F) yMag += 2*M_PI_F; // accel measurement Vector3 zAccel(_sensors.accelerometer_m_s2); @@ -512,9 +572,9 @@ int KalmanNav::correctAtt() bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; if (ignoreAccel) { + RAttAdjust(1, 1) = 1.0e10; + RAttAdjust(2, 2) = 1.0e10; RAttAdjust(3, 3) = 1.0e10; - RAttAdjust(4, 4) = 1.0e10; - RAttAdjust(5, 5) = 1.0e10; } else { //printf("correcting attitude with accel\n"); @@ -523,58 +583,25 @@ int KalmanNav::correctAtt() // accel predicted measurement Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); - // combined measurement - Vector zAtt(6); - Vector zAttHat(6); + // calculate residual + Vector y(4); + y(0) = yMag; + y(1) = zAccel(0) - zAccelHat(0); + y(2) = zAccel(1) - zAccelHat(1); + y(3) = zAccel(2) - zAccelHat(2); - for (int i = 0; i < 3; i++) { - zAtt(i) = zMag(i); - zAtt(i + 3) = zAccel(i); - zAttHat(i) = zMagHat(i); - zAttHat(i + 3) = zAccelHat(i); - } + // HMag + HAtt(0, 2) = 1; - // HMag , HAtt (0-2,:) - float tmp1 = - cosPsi * cosTheta * bN + - sinPsi * cosTheta * bE - - sinTheta * bD; - HAtt(0, 1) = -( - cosPsi * sinTheta * bN + - sinPsi * sinTheta * bE + - cosTheta * bD - ); - HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE); - HAtt(1, 0) = - (cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN + - (cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE + - cosPhi * cosTheta * bD; - HAtt(1, 1) = sinPhi * tmp1; - HAtt(1, 2) = -( - (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN - - (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE - ); - HAtt(2, 0) = -( - (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN + - (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE + - (sinPhi * cosTheta) * bD - ); - HAtt(2, 1) = cosPhi * tmp1; - HAtt(2, 2) = -( - (cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN - - (cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE - ); - - // HAccel , HAtt (3-5,:) - HAtt(3, 1) = cosTheta; - HAtt(4, 0) = -cosPhi * cosTheta; - HAtt(4, 1) = sinPhi * sinTheta; - HAtt(5, 0) = sinPhi * cosTheta; - HAtt(5, 1) = cosPhi * sinTheta; + // HAccel + HAtt(1, 1) = cosTheta; + HAtt(2, 0) = -cosPhi * cosTheta; + HAtt(2, 1) = sinPhi * sinTheta; + HAtt(3, 0) = sinPhi * cosTheta; + HAtt(3, 1) = cosPhi * sinTheta; // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Vector y = zAtt - zAttHat; // residual Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance Matrix K = P * HAtt.transpose() * S.inverse(); Vector xCorrect = K * y; @@ -585,7 +612,7 @@ int KalmanNav::correctAtt() if (isnan(val) || isinf(val)) { // abort correction and return - printf("[kalman_demo] numerical failure in att correction\n"); + warnx("numerical failure in att correction\n"); // reset P matrix to P0 P = P0; return ret_error; @@ -615,11 +642,8 @@ int KalmanNav::correctAtt() float beta = y.dot(S.inverse() * y); if (beta > _faultAtt.get()) { - printf("fault in attitude: beta = %8.4f\n", (double)beta); - printf("y:\n"); y.print(); - printf("zMagHat:\n"); zMagHat.print(); - printf("zMag:\n"); zMag.print(); - printf("bNav:\n"); bNav.print(); + warnx("fault in attitude: beta = %8.4f", (double)beta); + warnx("y:\n"); y.print(); } // update quaternions from euler @@ -652,9 +676,9 @@ int KalmanNav::correctPos() for (size_t i = 0; i < xCorrect.getRows(); i++) { float val = xCorrect(i); - if (isnan(val) || isinf(val)) { + if (!isfinite(val)) { // abort correction and return - printf("[kalman_demo] numerical failure in gps correction\n"); + warnx("numerical failure in gps correction\n"); // fallback to GPS vN = _gps.vel_n_m_s; vE = _gps.vel_e_m_s; @@ -685,8 +709,8 @@ int KalmanNav::correctPos() static int counter = 0; if (beta > _faultPos.get() && (counter % 10 == 0)) { - printf("fault in gps: beta = %8.4f\n", (double)beta); - printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + warnx("fault in gps: beta = %8.4f", (double)beta); + warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", double(y(0) / sqrtf(RPos(0, 0))), double(y(1) / sqrtf(RPos(1, 1))), double(y(2) / sqrtf(RPos(2, 2))), @@ -722,8 +746,6 @@ void KalmanNav::updateParams() if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; RAtt(0, 0) = noiseMagSq; // normalized direction - RAtt(1, 1) = noiseMagSq; - RAtt(2, 2) = noiseMagSq; // accelerometer noise float noiseAccelSq = _rAccel.get() * _rAccel.get(); @@ -731,9 +753,9 @@ void KalmanNav::updateParams() // bound noise to prevent singularities if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; - RAtt(3, 3) = noiseAccelSq; // normalized direction - RAtt(4, 4) = noiseAccelSq; - RAtt(5, 5) = noiseAccelSq; + RAtt(1, 1) = noiseAccelSq; // normalized direction + RAtt(2, 2) = noiseAccelSq; + RAtt(3, 3) = noiseAccelSq; // gps noise float R = R0 + float(alt); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index c2bf18115..49d0d157d 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 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 @@ -56,6 +56,10 @@ #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/parameter_update.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> + #include <drivers/drv_hrt.h> #include <poll.h> #include <unistd.h> @@ -78,6 +82,9 @@ public: */ virtual ~KalmanNav() {}; + + math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz); + /** * The main callback function for the class */ @@ -136,6 +143,11 @@ protected: // publications control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */ control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */ + + int _accel_sub; /**< Accelerometer subscription */ + int _gyro_sub; /**< Gyroscope subscription */ + int _mag_sub; /**< Magnetometer subscription */ + // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */ @@ -151,7 +163,8 @@ protected: enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ float phi, theta, psi; /**< 3-2-1 euler angles */ float vN, vE, vD; /**< navigation velocity, m/s */ - double lat, lon, alt; /**< lat, lon, alt, radians */ + double lat, lon; /**< lat, lon radians */ + float alt; /**< altitude, meters */ // parameters control::BlockParam<float> _vGyro; /**< gyro process noise */ control::BlockParam<float> _vAccel; /**< accelerometer process noise */ diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 10592ec7c..4befdc879 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User <mail@example.com> + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: James Goppert * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,8 +33,10 @@ ****************************************************************************/ /** - * @file kalman_demo.cpp - * Demonstration of control library + * @file kalman_main.cpp + * Combined attitude / position estimator. + * + * @author James Goppert */ #include <nuttx/config.h> @@ -51,7 +53,7 @@ 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 */ +static int daemon_task; /**< Handle of deamon task / thread */ /** * Deamon management function. @@ -101,9 +103,10 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("att_pos_estimator_ekf", + + daemon_task = task_spawn_cmd("att_pos_estimator_ekf", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_MAX - 30, 4096, kalman_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -133,7 +136,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) int kalman_demo_thread_main(int argc, char *argv[]) { - warnx("starting\n"); + warnx("starting"); using namespace math; @@ -145,7 +148,7 @@ int kalman_demo_thread_main(int argc, char *argv[]) nav.update(); } - printf("exiting.\n"); + warnx("exiting."); thread_running = false; diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk index 21b7c9166..8d4a40d95 100644 --- a/src/modules/att_pos_estimator_ekf/module.mk +++ b/src/modules/att_pos_estimator_ekf/module.mk @@ -37,9 +37,6 @@ MODULE_COMMAND = att_pos_estimator_ekf -# XXX this might be intended for the spawned deamon, validate -MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" - SRCS = kalman_main.cpp \ KalmanNav.cpp \ params.c diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c index 50642f067..4af5edead 100644 --- a/src/modules/att_pos_estimator_ekf/params.c +++ b/src/modules/att_pos_estimator_ekf/params.c @@ -1,12 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 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. + * + ****************************************************************************/ + #include <systemlib/param/param.h> /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f); +PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f); +PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f); +PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f); +PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f); PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f); PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 16d5ad626..d8b40ac3b 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -123,7 +123,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", + attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 14000, diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 3cbc62ea1..3ca50fb39 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -117,7 +117,7 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_so3_comp_task = task_spawn("attitude_estimator_so3_comp", + attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 12400, diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 7bae8ad40..50234a45e 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -1,12 +1,45 @@ -/* - * accelerometer_calibration.c +/**************************************************************************** + * + * 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: * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * 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. * - * Transform acceleration vector to true orientation and scale + * 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 accelerometer_calibration.c * - * * * * Model * * * + * Implementation of accelerometer calibration. + * + * Transform acceleration vector to true orientation, scale and offset + * + * ===== Model ===== * accel_corr = accel_T * (accel_raw - accel_offs) * * accel_corr[3] - fully corrected acceleration vector in body frame @@ -14,7 +47,7 @@ * accel_raw[3] - raw acceleration vector * accel_offs[3] - acceleration offset vector * - * * * * Calibration * * * + * ===== Calibration ===== * * Reference vectors * accel_corr_ref[6][3] = [ g 0 0 ] // nose up @@ -34,7 +67,6 @@ * * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 * - * * Find accel_T * * 9 unknown constants @@ -67,6 +99,8 @@ * * accel_T = A^-1 * g * g = 9.80665 + * + * @author Anton Babushkin <anton.babushkin@me.com> */ #include "accelerometer_calibration.h" diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 4175a25f4..3275d9461 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -1,8 +1,43 @@ -/* - * accelerometer_calibration.h +/**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * 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 accelerometer_calibration.h + * + * Definition of accelerometer calibration. + * + * @author Anton Babushkin <anton.babushkin@me.com> */ #ifndef ACCELEROMETER_CALIBRATION_H_ diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 41baa34d5..9d52d4f1c 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -659,7 +659,7 @@ int commander_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn("commander", + daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 3000, diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/block/UOrbPublication.hpp index a36f4429f..0a8ae2ff7 100644 --- a/src/modules/controllib/block/UOrbPublication.hpp +++ b/src/modules/controllib/block/UOrbPublication.hpp @@ -60,11 +60,15 @@ public: List<UOrbPublicationBase *> * list, const struct orb_metadata *meta) : _meta(meta), - _handle() { + _handle(-1) { if (list != NULL) list->add(this); } void update() { - orb_publish(getMeta(), getHandle(), getDataVoidPtr()); + if (_handle > 0) { + orb_publish(getMeta(), getHandle(), getDataVoidPtr()); + } else { + setHandle(orb_advertise(getMeta(), getDataVoidPtr())); + } } virtual void *getDataVoidPtr() = 0; virtual ~UOrbPublicationBase() { @@ -99,10 +103,6 @@ public: const struct orb_metadata *meta) : T(), // initialize data structure to zero UOrbPublicationBase(list, meta) { - // It is important that we call T() - // before we publish the data, so we - // call this here instead of the base class - setHandle(orb_advertise(getMeta(), getDataVoidPtr())); } virtual ~UOrbPublication() {} /* diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp index 8b0ea2fac..9435959e9 100644 --- a/src/modules/controllib/fixedwing.cpp +++ b/src/modules/controllib/fixedwing.cpp @@ -131,7 +131,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20), + _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz @@ -215,7 +215,7 @@ void BlockMultiModeBacksideAutopilot::update() // only update guidance in auto mode if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // update guidance - _guide.update(_pos, _att, _posCmd, _lastPosCmd); + _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); } // XXX handle STABILIZED (loiter on spot) as well @@ -228,7 +228,7 @@ void BlockMultiModeBacksideAutopilot::update() _status.navigation_state == NAVIGATION_STATE_SEATBELT) { // update guidance - _guide.update(_pos, _att, _posCmd, _lastPosCmd); + _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between @@ -242,7 +242,7 @@ void BlockMultiModeBacksideAutopilot::update() float vCmd = _vLimit.update(_vCmd.get()); // altitude hold - float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt); + float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt); // heading hold float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); @@ -332,7 +332,7 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); - // currenlty using manual throttle + // currently using manual throttle // XXX if you enable this watch out, vz might be very noisy //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); _actuators.control[CH_THR] = _manual.throttle; diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/controllib/fixedwing.hpp index 53d0cf893..e4028c40d 100644 --- a/src/modules/controllib/fixedwing.hpp +++ b/src/modules/controllib/fixedwing.hpp @@ -43,7 +43,7 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/vehicle_global_position_set_triplet.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> @@ -280,7 +280,7 @@ protected: UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; UOrbSubscription<vehicle_global_position_s> _pos; - UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd; + UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd; UOrbSubscription<manual_control_setpoint_s> _manual; UOrbSubscription<vehicle_status_s> _status; UOrbSubscription<parameter_update_s> _param_update; @@ -328,7 +328,7 @@ private: BlockParam<float> _crMax; struct pollfd _attPoll; - vehicle_global_position_setpoint_s _lastPosCmd; + vehicle_global_position_set_triplet_s _lastPosCmd; enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; uint64_t _timeStamp; public: diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index 7009356b7..68c176459 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller - pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller + pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller initialized = true; } @@ -137,8 +137,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0); - pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0); + pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim); + pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); } /* Roll (P) */ diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c index 758c97d78..b96fc3e5a 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c @@ -337,7 +337,7 @@ int fixedwing_att_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("fixedwing_att_control", + deamon_task = task_spawn_cmd("fixedwing_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 991d5ec5d..66854592a 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher initialized = true; } @@ -189,9 +189,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0); + pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); + pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); } diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index c3d57a85a..4803a526e 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -108,7 +108,8 @@ int fixedwing_backside_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("fixedwing_backside", + + deamon_task = task_spawn_cmd("fixedwing_backside", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 9c19c6786..87a942ffb 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) parameters_init(&h); parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, 0.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit - pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, 0.0f, PID_MODE_DERIVATIV_NONE); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, 0.0f, PID_MODE_DERIVATIV_NONE); - pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD_F, 0.0f, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value /* error and performance monitoring */ perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); @@ -268,10 +268,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f, 0.0f); //arbitrary high limit - pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim, 0.0f); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim, 0.0f); - pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F, 0.0f); //TODO: remove hardcoded value + pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit + pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); + pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F); //TODO: remove hardcoded value } /* only run controller if attitude changed */ @@ -448,7 +448,7 @@ int fixedwing_pos_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("fixedwing_pos_control", + deamon_task = task_spawn_cmd("fixedwing_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp index c3742e288..f509f7081 100644 --- a/src/modules/mathlib/math/Dcm.cpp +++ b/src/modules/mathlib/math/Dcm.cpp @@ -69,6 +69,15 @@ Dcm::Dcm(float c00, float c01, float c02, dcm(2, 2) = c22; } +Dcm::Dcm(const float data[3][3]) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + /* set rotation matrix */ + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + dcm(i, j) = data[i][j]; +} + Dcm::Dcm(const float *data) : Matrix(3, 3, data) { diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/modules/mathlib/math/Dcm.hpp index 28d840b10..df8970d3a 100644 --- a/src/modules/mathlib/math/Dcm.hpp +++ b/src/modules/mathlib/math/Dcm.hpp @@ -77,6 +77,11 @@ public: Dcm(const float *data); /** + * array ctor + */ + Dcm(const float data[3][3]); + + /** * quaternion ctor */ Dcm(const Quaternion &q); diff --git a/src/modules/mathlib/math/Limits.cpp b/src/modules/mathlib/math/Limits.cpp new file mode 100644 index 000000000..d4c892d8a --- /dev/null +++ b/src/modules/mathlib/math/Limits.cpp @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * 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 Limits.cpp + * + * Limiting / constrain helper functions + */ + + +#include <math.h> +#include <stdint.h> + +#include "Limits.hpp" + + +namespace math { + + +float __EXPORT min(float val1, float val2) +{ + return (val1 < val2) ? val1 : val2; +} + +int __EXPORT min(int val1, int val2) +{ + return (val1 < val2) ? val1 : val2; +} + +unsigned __EXPORT min(unsigned val1, unsigned val2) +{ + return (val1 < val2) ? val1 : val2; +} + +uint64_t __EXPORT min(uint64_t val1, uint64_t val2) +{ + return (val1 < val2) ? val1 : val2; +} + +double __EXPORT min(double val1, double val2) +{ + return (val1 < val2) ? val1 : val2; +} + +float __EXPORT max(float val1, float val2) +{ + return (val1 > val2) ? val1 : val2; +} + +int __EXPORT max(int val1, int val2) +{ + return (val1 > val2) ? val1 : val2; +} + +unsigned __EXPORT max(unsigned val1, unsigned val2) +{ + return (val1 > val2) ? val1 : val2; +} + +uint64_t __EXPORT max(uint64_t val1, uint64_t val2) +{ + return (val1 > val2) ? val1 : val2; +} + +double __EXPORT max(double val1, double val2) +{ + return (val1 > val2) ? val1 : val2; +} + + +float __EXPORT constrain(float val, float min, float max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +int __EXPORT constrain(int val, int min, int max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +double __EXPORT constrain(double val, double min, double max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +float __EXPORT radians(float degrees) +{ + return (degrees / 180.0f) * M_PI_F; +} + +double __EXPORT radians(double degrees) +{ + return (degrees / 180.0) * M_PI; +} + +float __EXPORT degrees(float radians) +{ + return (radians / M_PI_F) * 180.0f; +} + +double __EXPORT degrees(double radians) +{ + return (radians / M_PI) * 180.0; +} + +}
\ No newline at end of file diff --git a/src/modules/mathlib/math/Limits.hpp b/src/modules/mathlib/math/Limits.hpp new file mode 100644 index 000000000..fb778dd66 --- /dev/null +++ b/src/modules/mathlib/math/Limits.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 Limits.hpp + * + * Limiting / constrain helper functions + */ + +#pragma once + +#include <nuttx/config.h> +#include <stdint.h> + +namespace math { + + +float __EXPORT min(float val1, float val2); + +int __EXPORT min(int val1, int val2); + +unsigned __EXPORT min(unsigned val1, unsigned val2); + +uint64_t __EXPORT min(uint64_t val1, uint64_t val2); + +double __EXPORT min(double val1, double val2); + +float __EXPORT max(float val1, float val2); + +int __EXPORT max(int val1, int val2); + +unsigned __EXPORT max(unsigned val1, unsigned val2); + +uint64_t __EXPORT max(uint64_t val1, uint64_t val2); + +double __EXPORT max(double val1, double val2); + + +float __EXPORT constrain(float val, float min, float max); + +int __EXPORT constrain(int val, int min, int max); + +unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max); + +uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max); + +double __EXPORT constrain(double val, double min, double max); + +float __EXPORT radians(float degrees); + +double __EXPORT radians(double degrees); + +float __EXPORT degrees(float radians); + +double __EXPORT degrees(double radians); + +}
\ No newline at end of file diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/modules/mathlib/math/Quaternion.hpp index 4b4e959d8..048a55d33 100644 --- a/src/modules/mathlib/math/Quaternion.hpp +++ b/src/modules/mathlib/math/Quaternion.hpp @@ -37,7 +37,7 @@ * math quaternion lib */ -//#pragma once +#pragma once #include "Vector.hpp" #include "Matrix.hpp" diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/modules/mathlib/math/Vector2f.cpp new file mode 100644 index 000000000..68e741817 --- /dev/null +++ b/src/modules/mathlib/math/Vector2f.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 Vector2f.cpp + * + * math vector + */ + +#include "test/test.hpp" + +#include "Vector2f.hpp" + +namespace math +{ + +Vector2f::Vector2f() : + Vector(2) +{ +} + +Vector2f::Vector2f(const Vector &right) : + Vector(right) +{ +#ifdef VECTOR_ASSERT + ASSERT(right.getRows() == 2); +#endif +} + +Vector2f::Vector2f(float x, float y) : + Vector(2) +{ + setX(x); + setY(y); +} + +Vector2f::Vector2f(const float *data) : + Vector(2, data) +{ +} + +Vector2f::~Vector2f() +{ +} + +float Vector2f::cross(const Vector2f &b) const +{ + const Vector2f &a = *this; + return a(0)*b(1) - a(1)*b(0); +} + +float Vector2f::operator %(const Vector2f &v) const +{ + return cross(v); +} + +float Vector2f::operator *(const Vector2f &v) const +{ + return dot(v); +} + +int __EXPORT vector2fTest() +{ + printf("Test Vector2f\t\t: "); + // test float ctor + Vector2f v(1, 2); + ASSERT(equal(v(0), 1)); + ASSERT(equal(v(1), 2)); + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/modules/mathlib/math/Vector2f.hpp new file mode 100644 index 000000000..ecd62e81c --- /dev/null +++ b/src/modules/mathlib/math/Vector2f.hpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 Vector2f.hpp + * + * math 3 vector + */ + +#pragma once + +#include "Vector.hpp" + +namespace math +{ + +class __EXPORT Vector2f : + public Vector +{ +public: + Vector2f(); + Vector2f(const Vector &right); + Vector2f(float x, float y); + Vector2f(const float *data); + virtual ~Vector2f(); + float cross(const Vector2f &b) const; + float operator %(const Vector2f &v) const; + float operator *(const Vector2f &v) const; + inline Vector2f operator*(const float &right) const { + return Vector::operator*(right); + } + + /** + * accessors + */ + void setX(float x) { (*this)(0) = x; } + void setY(float y) { (*this)(1) = y; } + const float &getX() const { return (*this)(0); } + const float &getY() const { return (*this)(1); } +}; + +class __EXPORT Vector2 : + public Vector2f +{ +}; + +int __EXPORT vector2fTest(); +} // math + diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/modules/mathlib/math/Vector3.cpp index 61fcc442f..dcb85600e 100644 --- a/src/modules/mathlib/math/Vector3.cpp +++ b/src/modules/mathlib/math/Vector3.cpp @@ -74,9 +74,9 @@ Vector3::~Vector3() { } -Vector3 Vector3::cross(const Vector3 &b) +Vector3 Vector3::cross(const Vector3 &b) const { - Vector3 &a = *this; + const Vector3 &a = *this; Vector3 result; result(0) = a(1) * b(2) - a(2) * b(1); result(1) = a(2) * b(0) - a(0) * b(2); diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp index 8c36ac134..568d9669a 100644 --- a/src/modules/mathlib/math/Vector3.hpp +++ b/src/modules/mathlib/math/Vector3.hpp @@ -53,7 +53,7 @@ public: Vector3(float x, float y, float z); Vector3(const float *data); virtual ~Vector3(); - Vector3 cross(const Vector3 &b); + Vector3 cross(const Vector3 &b) const; /** * accessors @@ -65,6 +65,11 @@ public: const float &getY() const { return (*this)(1); } const float &getZ() const { return (*this)(2); } }; + +class __EXPORT Vector3f : + public Vector3 +{ +}; int __EXPORT vector3Test(); } // math diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp index 58d51107d..4155800e8 100644 --- a/src/modules/mathlib/math/arm/Vector.hpp +++ b/src/modules/mathlib/math/arm/Vector.hpp @@ -178,8 +178,15 @@ public: getRows()); return result; } + inline Vector operator-(void) const { + Vector result(getRows()); + arm_negate_f32((float *)getData(), + result.getData(), + getRows()); + return result; + } // other functions - inline float dot(const Vector &right) { + inline float dot(const Vector &right) const { float result = 0; arm_dot_prod_f32((float *)getData(), (float *)right.getData(), @@ -187,12 +194,21 @@ public: &result); return result; } - inline float norm() { + inline float norm() const { return sqrtf(dot(*this)); } - inline Vector unit() { + inline float length() const { + return norm(); + } + inline Vector unit() const { return (*this) / norm(); } + inline Vector normalized() const { + return unit(); + } + inline void normalize() { + (*this) = (*this) / norm(); + } inline static Vector zero(size_t rows) { Vector result(rows); // calloc returns zeroed memory diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/modules/mathlib/math/generic/Vector.hpp index 1a7363779..8cfdc676d 100644 --- a/src/modules/mathlib/math/generic/Vector.hpp +++ b/src/modules/mathlib/math/generic/Vector.hpp @@ -184,8 +184,17 @@ public: return result; } + inline Vector operator-(void) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = -((*this)(i)); + } + + return result; + } // other functions - inline float dot(const Vector &right) { + inline float dot(const Vector &right) const { float result = 0; for (size_t i = 0; i < getRows(); i++) { @@ -194,12 +203,21 @@ public: return result; } - inline float norm() { + inline float norm() const { return sqrtf(dot(*this)); } - inline Vector unit() { + inline float length() const { + return norm(); + } + inline Vector unit() const { return (*this) / norm(); } + inline Vector normalized() const { + return unit(); + } + inline void normalize() { + (*this) = (*this) / norm(); + } inline static Vector zero(size_t rows) { Vector result(rows); // calloc returns zeroed memory diff --git a/src/modules/mathlib/mathlib.h b/src/modules/mathlib/mathlib.h index b919d53db..40ffb22bc 100644 --- a/src/modules/mathlib/mathlib.h +++ b/src/modules/mathlib/mathlib.h @@ -39,12 +39,16 @@ #ifdef __cplusplus +#pragma once + #include "math/Dcm.hpp" #include "math/EulerAngles.hpp" #include "math/Matrix.hpp" #include "math/Quaternion.hpp" #include "math/Vector.hpp" #include "math/Vector3.hpp" +#include "math/Vector2f.hpp" +#include "math/Limits.hpp" #endif diff --git a/src/modules/mathlib/module.mk b/src/modules/mathlib/module.mk index bcdb2afe5..2146a1413 100644 --- a/src/modules/mathlib/module.mk +++ b/src/modules/mathlib/module.mk @@ -36,11 +36,13 @@ # SRCS = math/test/test.cpp \ math/Vector.cpp \ + math/Vector2f.cpp \ math/Vector3.cpp \ math/EulerAngles.cpp \ math/Quaternion.cpp \ math/Dcm.cpp \ - math/Matrix.cpp + math/Matrix.cpp \ + math/Limits.cpp # # In order to include .config we first have to save off the diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 15ba8860d..f6c371c7c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -144,14 +144,6 @@ set_hil_on_off(bool hil_enabled) /* Enable HIL */ if (hil_enabled && !mavlink_hil_enabled) { - /* Advertise topics */ - pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); - pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - - /* sensore level hil */ - pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); - pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); - mavlink_hil_enabled = true; /* ramp up some HIL-related subscriptions */ @@ -708,6 +700,8 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + /* sleep quarter the time */ usleep(25000); @@ -719,10 +713,13 @@ int mavlink_thread_main(int argc, char *argv[]) /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); /* sleep quarter the time */ usleep(25000); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + if (baudrate > 57600) { mavlink_pm_queued_send(); } @@ -781,7 +778,7 @@ int mavlink_main(int argc, char *argv[]) errx(0, "mavlink already running\n"); thread_should_exit = false; - mavlink_task = task_spawn("mavlink", + mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/modules/mavlink/mavlink_hil.h b/src/modules/mavlink/mavlink_hil.h index 8c7a5b514..744ed7d94 100644 --- a/src/modules/mavlink/mavlink_hil.h +++ b/src/modules/mavlink/mavlink_hil.h @@ -41,15 +41,6 @@ extern bool mavlink_hil_enabled; -extern struct vehicle_global_position_s hil_global_pos; -extern struct vehicle_attitude_s hil_attitude; -extern struct sensor_combined_s hil_sensors; -extern struct vehicle_gps_position_s hil_gps; -extern orb_advert_t pub_hil_global_pos; -extern orb_advert_t pub_hil_attitude; -extern orb_advert_t pub_hil_sensors; -extern orb_advert_t pub_hil_gps; - /** * Enable / disable Hardware in the Loop simulation mode. * diff --git a/src/modules/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.cpp index 940d030b2..33ac14860 100644 --- a/src/modules/mavlink/mavlink_receiver.c +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -49,7 +49,6 @@ #include <fcntl.h> #include <mqueue.h> #include <string.h> -#include "mavlink_bridge_header.h" #include <v1.0/common/mavlink.h> #include <drivers/drv_hrt.h> #include <time.h> @@ -62,10 +61,17 @@ #include <stdlib.h> #include <poll.h> +#include <mathlib/mathlib.h> + #include <systemlib/param/param.h> #include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <systemlib/airspeed.h> #include <mavlink/mavlink_log.h> +__BEGIN_DECLS + +#include "mavlink_bridge_header.h" #include "waypoints.h" #include "orb_topics.h" #include "missionlib.h" @@ -73,8 +79,12 @@ #include "mavlink_parameters.h" #include "util.h" +extern bool gcs_link; + +__END_DECLS + /* XXX should be in a header somewhere */ -pthread_t receive_start(int uart); +extern "C" pthread_t receive_start(int uart); static void handle_message(mavlink_message_t *msg); static void *receive_thread(void *arg); @@ -88,18 +98,18 @@ struct vehicle_global_position_s hil_global_pos; struct vehicle_attitude_s hil_attitude; struct vehicle_gps_position_s hil_gps; struct sensor_combined_s hil_sensors; -orb_advert_t pub_hil_global_pos = -1; -orb_advert_t pub_hil_attitude = -1; -orb_advert_t pub_hil_gps = -1; -orb_advert_t pub_hil_sensors = -1; +static orb_advert_t pub_hil_global_pos = -1; +static orb_advert_t pub_hil_attitude = -1; +static orb_advert_t pub_hil_gps = -1; +static orb_advert_t pub_hil_sensors = -1; +static orb_advert_t pub_hil_airspeed = -1; static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t offboard_control_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; - -extern bool gcs_link; +static orb_advert_t telemetry_status_pub = -1; static void handle_message(mavlink_message_t *msg) @@ -141,10 +151,10 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } - - /* publish */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } } } @@ -281,7 +291,7 @@ handle_message(mavlink_message_t *msg) } offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = ml_mode; + offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode); offboard_control_sp.timestamp = hrt_absolute_time(); @@ -296,6 +306,33 @@ handle_message(mavlink_message_t *msg) } } + /* handle status updates of the radio */ + if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + + struct telemetry_status_s tstatus; + + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + /* publish telemetry status topic */ + tstatus.timestamp = hrt_absolute_time(); + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.rssi = rstatus.rssi; + tstatus.remote_rssi = rstatus.remrssi; + tstatus.txbuf = rstatus.txbuf; + tstatus.noise = rstatus.noise; + tstatus.remote_noise = rstatus.remnoise; + tstatus.rxerrors = rstatus.rxerrors; + tstatus.fixed = rstatus.fixed; + + if (telemetry_status_pub == 0) { + telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + + } else { + orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus); + } + } + /* * Only decode hil messages in HIL mode. * @@ -308,10 +345,10 @@ handle_message(mavlink_message_t *msg) uint64_t timestamp = hrt_absolute_time(); - if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) { + if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) { - mavlink_highres_imu_t imu; - mavlink_msg_highres_imu_decode(msg, &imu); + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); /* packet counter */ static uint16_t hil_counter = 0; @@ -370,8 +407,34 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_counter = hil_counter; hil_sensors.accelerometer_counter = hil_counter; + /* differential pressure */ + hil_sensors.differential_pressure_pa = imu.diff_pressure; + hil_sensors.differential_pressure_counter = hil_counter; + + /* airspeed from differential pressure, ambient pressure and temp */ + struct airspeed_s airspeed; + airspeed.timestamp = hrt_absolute_time(); + + float ias = calc_indicated_airspeed(imu.diff_pressure); + // XXX need to fix this + float tas = ias; + + airspeed.indicated_airspeed_m_s = ias; + airspeed.true_airspeed_m_s = tas; + + if (pub_hil_airspeed < 0) { + pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { + orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); + } + //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); + /* publish */ - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + if (pub_hil_sensors > 0) { + orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + } else { + pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + } // increment counters hil_counter++; @@ -379,21 +442,16 @@ handle_message(mavlink_message_t *msg) // output if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil imu at %d hz\n", hil_frames/10); + printf("receiving hil sensor at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } } - if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) { + if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) { - mavlink_gps_raw_int_t gps; - mavlink_msg_gps_raw_int_decode(msg, &gps); - - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; + mavlink_hil_gps_t gps; + mavlink_msg_hil_gps_decode(msg, &gps); /* gps */ hil_gps.timestamp_position = gps.time_usec; @@ -412,54 +470,40 @@ handle_message(mavlink_message_t *msg) /* go back to -PI..PI */ if (heading_rad > M_PI_F) heading_rad -= 2.0f * M_PI_F; - hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad); - hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad); - hil_gps.vel_d_m_s = 0.0f; + hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m + hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m + hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m hil_gps.vel_ned_valid = true; - /* COG (course over ground) is speced as -PI..+PI */ + /* COG (course over ground) is spec'ed as -PI..+PI */ hil_gps.cog_rad = heading_rad; hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; - /* publish */ - orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); - - // increment counters - hil_counter += 1 ; - hil_frames += 1 ; + /* publish GPS measurement data */ + if (pub_hil_gps > 0) { + orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); + } else { + pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + } - // output - // if ((timestamp - old_timestamp) > 10000000) { - // printf("receiving hil gps at %d hz\n", hil_frames/10); - // old_timestamp = timestamp; - // hil_frames = 0; - // } } - if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { + if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) { - mavlink_hil_state_t hil_state; - mavlink_msg_hil_state_decode(msg, &hil_state); + mavlink_hil_state_quaternion_t hil_state; + mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); - /* Calculate Rotation Matrix */ - //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode - - if (mavlink_system.type == MAV_TYPE_FIXED_WING) { - //TODO: assuming low pitch and roll values for now - hil_attitude.R[0][0] = cosf(hil_state.yaw); - hil_attitude.R[0][1] = sinf(hil_state.yaw); - hil_attitude.R[0][2] = 0.0f; - - hil_attitude.R[1][0] = -sinf(hil_state.yaw); - hil_attitude.R[1][1] = cosf(hil_state.yaw); - hil_attitude.R[1][2] = 0.0f; - - hil_attitude.R[2][0] = 0.0f; - hil_attitude.R[2][1] = 0.0f; - hil_attitude.R[2][2] = 1.0f; + struct airspeed_s airspeed; + airspeed.timestamp = hrt_absolute_time(); + airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; + airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - hil_attitude.R_valid = true; + if (pub_hil_airspeed < 0) { + pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { + orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } + warnx("IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; @@ -468,21 +512,48 @@ handle_message(mavlink_message_t *msg) hil_global_pos.vy = hil_state.vy / 100.0f; hil_global_pos.vz = hil_state.vz / 100.0f; - /* set timestamp and notify processes (broadcast) */ hil_global_pos.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); - hil_attitude.roll = hil_state.roll; - hil_attitude.pitch = hil_state.pitch; - hil_attitude.yaw = hil_state.yaw; + if (pub_hil_global_pos > 0) { + orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + } else { + pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + } + + /* Calculate Rotation Matrix */ + math::Quaternion q(hil_state.attitude_quaternion); + math::Dcm C_nb(q); + math::EulerAngles euler(C_nb); + + /* set rotation matrix */ + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + hil_attitude.R[i][j] = C_nb(i, j); + + hil_attitude.R_valid = true; + + /* set quaternion */ + hil_attitude.q[0] = q(0); + hil_attitude.q[1] = q(1); + hil_attitude.q[2] = q(2); + hil_attitude.q[3] = q(3); + hil_attitude.q_valid = true; + + hil_attitude.roll = euler.getPhi(); + hil_attitude.pitch = euler.getTheta(); + hil_attitude.yaw = euler.getPsi(); hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; /* set timestamp and notify processes (broadcast) */ hil_attitude.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + + if (pub_hil_attitude > 0) { + orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } else { + pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + } } if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { @@ -553,7 +624,9 @@ receive_thread(void *arg) while (!thread_should_exit) { - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = uart_fd; + fds[0].events = POLLIN; if (poll(fds, 1, timeout) > 0) { /* non-blocking read. read may return negative values */ @@ -592,7 +665,7 @@ receive_start(int uart) param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 2048); + pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index d369e05ff..4b010dd59 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -60,6 +60,7 @@ #include <stdlib.h> #include <poll.h> +#include <systemlib/err.h> #include <systemlib/param/param.h> #include <systemlib/systemlib.h> #include <mavlink/mavlink_log.h> @@ -73,6 +74,10 @@ #include "mavlink_parameters.h" static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; +static uint64_t loiter_start_time; + +static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command, + struct vehicle_global_position_setpoint_s *sp); int mavlink_missionlib_send_message(mavlink_message_t *msg) @@ -123,6 +128,52 @@ uint64_t mavlink_missionlib_get_system_timestamp() } /** + * Set special vehicle setpoint fields based on current mission item. + * + * @return true if the mission item could be interpreted + * successfully, it return false on failure. + */ +bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command, + struct vehicle_global_position_setpoint_s *sp) +{ + switch (command) { + case MAV_CMD_NAV_LOITER_UNLIM: + sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + break; + case MAV_CMD_NAV_LOITER_TIME: + sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + loiter_start_time = hrt_absolute_time(); + break; + // case MAV_CMD_NAV_LOITER_TURNS: + // sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT; + // break; + case MAV_CMD_NAV_WAYPOINT: + sp->nav_cmd = NAV_CMD_WAYPOINT; + break; + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; + break; + case MAV_CMD_NAV_LAND: + sp->nav_cmd = NAV_CMD_LAND; + break; + case MAV_CMD_NAV_TAKEOFF: + sp->nav_cmd = NAV_CMD_TAKEOFF; + break; + default: + /* abort */ + return false; + } + + sp->loiter_radius = param3; + sp->loiter_direction = (param3 >= 0) ? 1 : -1; + + sp->param1 = param1; + sp->param1 = param2; + sp->param1 = param3; + sp->param1 = param4; +} + +/** * This callback is executed each time a waypoint changes. * * It publishes the vehicle_global_position_setpoint_s or the @@ -133,9 +184,13 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) { static orb_advert_t global_position_setpoint_pub = -1; + static orb_advert_t global_position_set_triplet_pub = -1; static orb_advert_t local_position_setpoint_pub = -1; + static unsigned last_waypoint_index = -1; char buf[50] = {0}; + // XXX include check if WP is supported, jump to next if not + /* Update controller setpoints */ if (frame == (int)MAV_FRAME_GLOBAL) { /* global, absolute waypoint */ @@ -145,8 +200,9 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.altitude = param7_alt_z; sp.altitude_is_relative = false; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(param1, param2, param3, param4, command, &sp); - /* Initialize publication if necessary */ + /* Initialize setpoint publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); @@ -154,6 +210,113 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } + + /* fill triplet: previous, current, next waypoint */ + struct vehicle_global_position_set_triplet_s triplet; + + /* current waypoint is same as sp */ + memcpy(&(triplet.current), &sp, sizeof(sp)); + + /* + * Check if previous WP (in mission, not in execution order) + * is available and identify correct index + */ + int last_setpoint_index = -1; + bool last_setpoint_valid = false; + + /* at first waypoint, but cycled once through mission */ + if (index == 0 && last_waypoint_index > 0) { + last_setpoint_index = last_waypoint_index; + } else { + last_setpoint_index = index - 1; + } + + while (last_setpoint_index >= 0) { + + if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && + (wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || + wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || + wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || + wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + last_setpoint_valid = true; + break; + } + + last_setpoint_index--; + } + + /* + * Check if next WP (in mission, not in execution order) + * is available and identify correct index + */ + int next_setpoint_index = -1; + bool next_setpoint_valid = false; + + /* at last waypoint, try to re-loop through mission as default */ + if (index == (wpm->size - 1) && wpm->size > 1) { + next_setpoint_index = 0; + } else if (wpm->size > 1) { + next_setpoint_index = index + 1; + } + + while (next_setpoint_index < wpm->size - 1) { + + if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || + wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || + wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || + wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + next_setpoint_valid = true; + break; + } + + next_setpoint_index++; + } + + /* populate last and next */ + + triplet.previous_valid = false; + triplet.next_valid = false; + + if (last_setpoint_valid) { + triplet.previous_valid = true; + struct vehicle_global_position_setpoint_s sp; + sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f; + sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; + sp.altitude = wpm->waypoints[last_setpoint_index].z; + sp.altitude_is_relative = false; + sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(wpm->waypoints[last_setpoint_index].param1, + wpm->waypoints[last_setpoint_index].param2, + wpm->waypoints[last_setpoint_index].param3, + wpm->waypoints[last_setpoint_index].param4, + wpm->waypoints[last_setpoint_index].command, &sp); + memcpy(&(triplet.previous), &sp, sizeof(sp)); + } + + if (next_setpoint_valid) { + triplet.next_valid = true; + struct vehicle_global_position_setpoint_s sp; + sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f; + sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; + sp.altitude = wpm->waypoints[next_setpoint_index].z; + sp.altitude_is_relative = false; + sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(wpm->waypoints[next_setpoint_index].param1, + wpm->waypoints[next_setpoint_index].param2, + wpm->waypoints[next_setpoint_index].param3, + wpm->waypoints[next_setpoint_index].param4, + wpm->waypoints[next_setpoint_index].command, &sp); + memcpy(&(triplet.next), &sp, sizeof(sp)); + } + + /* Initialize triplet publication if necessary */ + if (global_position_set_triplet_pub < 0) { + global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet); + + } else { + orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); + } + sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { @@ -164,6 +327,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.altitude = param7_alt_z; sp.altitude_is_relative = true; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize publication if necessary */ if (global_position_setpoint_pub < 0) { @@ -173,6 +337,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } + + sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { @@ -192,8 +358,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, } sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); + } else { + warnx("non-navigation WP, ignoring"); + mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring."); + return; } + /* only set this for known waypoint types (non-navigation types would have returned earlier) */ + last_waypoint_index = index; + mavlink_missionlib_send_gcs_string(buf); printf("%s\n", buf); //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index cbf08aeb2..bfccb2d38 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -40,7 +40,7 @@ SRCS += mavlink.c \ missionlib.c \ mavlink_parameters.c \ mavlink_log.c \ - mavlink_receiver.c \ + mavlink_receiver.cpp \ orb_listener.c \ waypoints.c diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 56cbdb26b..a9a611998 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -72,6 +72,8 @@ struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_safety_s safety; +struct actuator_controls_effective_s actuators_0; +struct vehicle_attitude_s att; struct mavlink_subscriptions mavlink_subs; @@ -116,6 +118,7 @@ static void l_debug_key_value(const struct listener *l); static void l_optical_flow(const struct listener *l); static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); +static void l_airspeed(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -140,6 +143,7 @@ static const struct listener listeners[] = { {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, {l_home, &mavlink_subs.home_sub, 0}, + {l_airspeed, &mavlink_subs.airspeed_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -192,7 +196,7 @@ l_sensor_combined(const struct listener *l) raw.gyro_rad_s[1], raw.gyro_rad_s[2], raw.magnetometer_ga[0], raw.magnetometer_ga[1], raw.magnetometer_ga[2], - raw.baro_pres_mbar, 0 /* no diff pressure yet */, + raw.baro_pres_mbar, raw.differential_pressure_pa, raw.baro_alt_meter, raw.baro_temp_celcius, fields_updated); @@ -202,9 +206,6 @@ l_sensor_combined(const struct listener *l) void l_vehicle_attitude(const struct listener *l) { - struct vehicle_attitude_s att; - - /* copy attitude data into local buffer */ orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); @@ -564,28 +565,26 @@ l_manual_control_setpoint(const struct listener *l) void l_vehicle_attitude_controls(const struct listener *l) { - struct actuator_controls_effective_s actuators; - - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl0 ", - actuators.control_effective[0]); + actuators_0.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl1 ", - actuators.control_effective[1]); + actuators_0.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl2 ", - actuators.control_effective[2]); + actuators_0.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl3 ", - actuators.control_effective[3]); + actuators_0.control_effective[3]); } } @@ -626,6 +625,22 @@ l_home(const struct listener *l) mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); } +void +l_airspeed(const struct listener *l) +{ + struct airspeed_s airspeed; + + orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); + + float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); + float alt = global_pos.alt; + float climb = global_pos.vz; + + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, + ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb); +} + static void * uorb_receive_thread(void *arg) { @@ -765,6 +780,10 @@ uorb_receive_start(void) mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ + /* --- AIRSPEED / VFR / HUD --- */ + mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ + /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 221957d46..3b968b911 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -52,6 +52,7 @@ #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/vehicle_global_position_set_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_control_mode.h> @@ -61,7 +62,9 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_safety.h> #include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/telemetry_status.h> #include <uORB/topics/debug_key_value.h> +#include <uORB/topics/airspeed.h> #include <drivers/drv_rc_input.h> struct mavlink_subscriptions { @@ -85,6 +88,7 @@ struct mavlink_subscriptions { int optical_flow; int rates_setpoint_sub; int home_sub; + int airspeed_sub; }; extern struct mavlink_subscriptions mavlink_subs; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index a131b143b..cefcca468 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -347,15 +347,24 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ { static uint16_t counter; - // Do not flood the precious wireless link with debug data - // if (wpm->size > 0 && counter % 10 == 0) { - // printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id); - // } + if (wpm->current_active_wp_id < wpm->size) { + float orbit; + if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) { - if (wpm->current_active_wp_id < wpm->size) { + orbit = wpm->waypoints[wpm->current_active_wp_id].param2; + + } else if (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) { + + orbit = wpm->waypoints[wpm->current_active_wp_id].param3; + } else { + + // XXX set default orbit via param + orbit = 15.0f; + } - float orbit = wpm->waypoints[wpm->current_active_wp_id].param2; int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; float dist = -1.0f; @@ -374,10 +383,9 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ } if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw + wpm->pos_reached = true; - if (counter % 100 == 0) - printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit); } // else @@ -394,29 +402,47 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (wpm->timestamp_firstinside_orbit == 0) { // Announce that last waypoint was reached - printf("Reached waypoint %u for the first time \n", cur_wp->seq); mavlink_wpm_send_waypoint_reached(cur_wp->seq); wpm->timestamp_firstinside_orbit = now; } // check if the MAV was long enough inside the waypoint orbit //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param2 * 1000) { - printf("Reached waypoint %u long enough \n", cur_wp->seq); + bool time_elapsed = false; + + if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) { + if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + time_elapsed = true; + } + } else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + time_elapsed = true; + } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { + time_elapsed = true; + } + + if (time_elapsed) { if (cur_wp->autocontinue) { cur_wp->current = 0; - if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { - /* the last waypoint was reached, if auto continue is - * activated restart the waypoint list from the beginning - */ - wpm->current_active_wp_id = 0; + /* only accept supported navigation waypoints, skip unknown ones */ + do { - } else { - if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) - wpm->current_active_wp_id++; - } + if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { + /* the last waypoint was reached, if auto continue is + * activated restart the waypoint list from the beginning + */ + wpm->current_active_wp_id = 0; + + } else { + if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) + wpm->current_active_wp_id++; + } + + } while (!(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)); // Fly to next waypoint wpm->timestamp_firstinside_orbit = 0; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index dbea2be08..35d53b880 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -503,7 +503,7 @@ int mavlink_onboard_main(int argc, char *argv[]) errx(0, "mavlink already running\n"); thread_should_exit = false; - mavlink_task = task_spawn("mavlink_onboard", + mavlink_task = task_spawn_cmd("mavlink_onboard", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, 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 25447aaba..70a61fc4e 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -439,7 +439,7 @@ int multirotor_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1 + optioncount], "start")) { thread_should_exit = false; - mc_task = task_spawn("multirotor_att_control", + mc_task = task_spawn_cmd("multirotor_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 15, 2048, diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 4a1129e1f..083311674 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -1,12 +1,12 @@ /**************************************************************************** * * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,7 +39,15 @@ /* * @file multirotor_attitude_control.c - * Implementation of attitude controller + * + * Implementation of attitude controller for multirotors. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #include "multirotor_attitude_control.h" @@ -129,16 +137,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s static uint64_t last_run = 0; static uint64_t last_input = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; last_run = hrt_absolute_time(); if (last_input != att_sp->timestamp) { last_input = att_sp->timestamp; } -// static int sensor_delay; -// sensor_delay = hrt_absolute_time() - att->timestamp; - static int motor_skip_counter = 0; static PID_t pitch_controller; @@ -156,8 +160,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); initialized = true; } @@ -168,8 +172,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } /* reset integral if on ground */ diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h index 4e70a5103..6dd5b39fd 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -1,12 +1,12 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,7 +39,15 @@ /* * @file multirotor_attitude_control.h - * Attitude control for multi rotors. + * + * Definition of attitude controller for multirotors. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #ifndef MULTIROTOR_ATTITUDE_CONTROL_H_ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 641d833f0..ee8c37580 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -1,8 +1,10 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. * Author: Tobias Naegeli <naegelit@student.ethz.ch> * Lorenz Meier <lm@inf.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,10 +38,12 @@ /** * @file multirotor_rate_control.c * - * Implementation of rate controller + * Implementation of rate controller for multirotors. * * @author Tobias Naegeli <naegelit@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ #include "multirotor_rate_control.h" @@ -158,8 +162,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; static uint64_t last_input = 0; -// float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; - if (last_input != rate_sp->timestamp) { last_input = rate_sp->timestamp; } @@ -187,8 +189,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); } @@ -196,8 +198,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); } /* reset integral if on ground */ @@ -208,31 +210,15 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + rates[1], 0.0f, deltaT, + &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); - - /* increase resilience to faulty control inputs */ - if (isfinite(pitch_control)) { - pitch_control_last = pitch_control; - - } else { - pitch_control = pitch_control_last; - warnx("rej. NaN ctrl pitch"); - } - - /* increase resilience to faulty control inputs */ - if (isfinite(roll_control)) { - roll_control_last = roll_control; - - } else { - roll_control = roll_control_last; - warnx("rej. NaN ctrl roll"); - } + rates[0], 0.0f, deltaT, + &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); - /* control yaw rate */ //XXX use library here and use rates_acc[2] + /* control yaw rate */ //XXX use library here float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); /* increase resilience to faulty control inputs */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 465549540..695ff3e16 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -1,12 +1,12 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,7 +39,15 @@ /* * @file multirotor_attitude_control.h - * Attitude control for multi rotors. + * + * Definition of rate controller for multirotors. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #ifndef MULTIROTOR_RATE_CONTROL_H_ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 7b8d83aa8..f39d11438 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -94,7 +94,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn(). + * to task_spawn_cmd(). */ int multirotor_pos_control_main(int argc, char *argv[]) { @@ -110,7 +110,7 @@ int multirotor_pos_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("multirotor pos control", + deamon_task = task_spawn_cmd("multirotor pos control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 60, 4096, diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index 10dee3f22..984bd1329 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -123,7 +123,7 @@ int position_estimator_mc_main(int argc, char *argv[]) } thread_should_exit = false; - position_estimator_mc_task = task_spawn("position_estimator_mc", + position_estimator_mc_task = task_spawn_cmd("position_estimator_mc", SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index f744698be..81566eb2a 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -41,7 +41,7 @@ #include <nuttx/arch.h> #include <arch/stm32/chip.h> -#include <stm32_internal.h> +#include <stm32.h> #include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> diff --git a/src/modules/sdlog/sdlog.c b/src/modules/sdlog/sdlog.c index 84a9eb6ac..c22523bf2 100644 --- a/src/modules/sdlog/sdlog.c +++ b/src/modules/sdlog/sdlog.c @@ -161,7 +161,7 @@ bool logging_enabled = true; * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn(). + * to task_spawn_cmd(). */ int sdlog_main(int argc, char *argv[]) { @@ -177,7 +177,7 @@ int sdlog_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn("sdlog", + deamon_task = task_spawn_cmd("sdlog", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, 4096, diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 2e1e4fd4d..b3243f7b5 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * 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 @@ -37,7 +37,7 @@ * * Ring FIFO buffer for binary log data. * - * @author Anton Babushkin <rk3dov@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <string.h> diff --git a/src/modules/sdlog2/logbuffer.h b/src/modules/sdlog2/logbuffer.h index 31521f722..3a5e3a29f 100644 --- a/src/modules/sdlog2/logbuffer.h +++ b/src/modules/sdlog2/logbuffer.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * 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 @@ -37,7 +37,7 @@ * * Ring FIFO buffer for binary log data. * - * @author Anton Babushkin <rk3dov@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> */ #ifndef SDLOG2_RINGBUFFER_H_ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8a07855c0..31da81f5c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -2,7 +2,7 @@ * * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier <lm@inf.ethz.ch> - * Anton Babushkin <rk3dov@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 @@ -40,7 +40,7 @@ * does the heavy SD I/O in a low-priority worker thread. * * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Anton Babushkin <rk3dov@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <nuttx/config.h> @@ -239,7 +239,7 @@ int sdlog2_main(int argc, char *argv[]) } main_thread_should_exit = false; - deamon_task = task_spawn("sdlog2", + deamon_task = task_spawn_cmd("sdlog2", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, 3000, @@ -695,6 +695,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_AIRS_s log_AIRS; struct log_ARSP_s log_ARSP; struct log_FLOW_s log_FLOW; + struct log_GPOS_s log_GPOS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1097,7 +1098,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - // TODO not implemented yet + log_msg.msg_type = LOG_GPOS_MSG; + log_msg.body.log_GPOS.lat = buf.global_pos.lat; + log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.alt = buf.global_pos.alt; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + LOGBUFFER_WRITE_AND_COUNT(GPOS); } /* --- VICON POSITION --- */ diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index 59b91d90d..5c175ef7e 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * 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 @@ -37,7 +37,7 @@ * * General log format structures and macro. * - * @author Anton Babushkin <rk3dov@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> */ /* diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 96c44e7c2..358637f93 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * 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 @@ -37,7 +37,7 @@ * * Log messages and structures definition. * - * @author Anton Babushkin <rk3dov@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> */ #ifndef SDLOG2_MESSAGES_H_ @@ -218,6 +218,17 @@ struct log_FLOW_s { uint8_t quality; uint8_t sensor_id; }; + +/* --- GPOS - GLOBAL POSITION ESTIMATE --- */ +#define LOG_GPOS_MSG 16 +struct log_GPOS_s { + int32_t lat; + int32_t lon; + float alt; + float vel_n; + float vel_e; + float vel_d; +}; #pragma pack(pop) /* construct list of all message formats */ @@ -239,6 +250,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), + LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 04b2cde8b..7077972aa 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -48,6 +48,10 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); + PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 560ef6406..49a1da83b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -194,6 +194,7 @@ private: float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; + float gyro_scale[3]; float mag_offset[3]; float mag_scale[3]; float accel_offset[3]; @@ -241,6 +242,7 @@ private: param_t rc_demix; param_t gyro_offset[3]; + param_t gyro_scale[3]; param_t accel_offset[3]; param_t accel_scale[3]; param_t mag_offset[3]; @@ -481,6 +483,9 @@ Sensors::Sensors() : _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); _parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF"); + _parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE"); + _parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE"); + _parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE"); /* accel offsets */ _parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF"); @@ -681,6 +686,9 @@ Sensors::parameters_update() param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1])); param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2])); + param_get(_parameter_handles.gyro_scale[0], &(_parameters.gyro_scale[0])); + param_get(_parameter_handles.gyro_scale[1], &(_parameters.gyro_scale[1])); + param_get(_parameter_handles.gyro_scale[2], &(_parameters.gyro_scale[2])); /* accel offsets */ param_get(_parameter_handles.accel_offset[0], &(_parameters.accel_offset[0])); @@ -968,11 +976,11 @@ Sensors::parameter_update_poll(bool forced) int fd = open(GYRO_DEVICE_PATH, 0); struct gyro_scale gscale = { _parameters.gyro_offset[0], - 1.0f, + _parameters.gyro_scale[0], _parameters.gyro_offset[1], - 1.0f, + _parameters.gyro_scale[1], _parameters.gyro_offset[2], - 1.0f, + _parameters.gyro_scale[2], }; if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) @@ -1429,7 +1437,7 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = task_spawn("sensors_task", + _sensors_task = task_spawn_cmd("sensors_task", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, @@ -1486,6 +1494,7 @@ int sensors_main(int argc, char *argv[]) } } - errx(1, "unrecognized command"); + warnx("unrecognized command"); + return 1; } diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h index 5d485b01f..064426f21 100644 --- a/src/modules/systemlib/conversions.h +++ b/src/modules/systemlib/conversions.h @@ -43,11 +43,7 @@ #define CONVERSIONS_H_ #include <float.h> #include <stdint.h> - -#define CONSTANTS_ONE_G 9.80665f // m/s^2 -#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f // kg/m^3 -#define CONSTANTS_AIR_GAS_CONST 287.1f // J/(kg * K) -#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f // °C +#include <systemlib/geo/geo.h> __BEGIN_DECLS diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index 20b711fa6..8fdff8ac0 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -1,9 +1,8 @@ /**************************************************************************** - * configs/px4fmu/src/up_leds.c - * arch/arm/src/board/up_leds.c * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt <gnutt@nuttx.org> + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * Petri Tanskanen <petri.tanskanen@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -15,7 +14,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 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. * @@ -34,18 +33,26 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ - +/** + * @file cpuload.c + * + * Measurement of CPU load of each individual task. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + */ #include <nuttx/config.h> +#include <nuttx/sched.h> +#include <sys/types.h> #include <stdint.h> #include <stdbool.h> + +#include <arch/arch.h> + #include <debug.h> #include <sys/time.h> -#include <sched.h> #include <arch/board/board.h> #include <drivers/drv_hrt.h> @@ -54,26 +61,13 @@ #ifdef CONFIG_SCHED_INSTRUMENTATION -/**************************************************************************** - * Definitions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -__EXPORT void sched_note_start(FAR _TCB *tcb); -__EXPORT void sched_note_stop(FAR _TCB *tcb); -__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb); - -/**************************************************************************** - * Name: - ****************************************************************************/ +__EXPORT void sched_note_start(FAR struct tcb_s *tcb); +__EXPORT void sched_note_stop(FAR struct tcb_s *tcb); +__EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb); __EXPORT struct system_load_s system_load; -extern FAR _TCB *sched_gettcb(pid_t pid); +extern FAR struct _TCB *sched_gettcb(pid_t pid); void cpuload_initialize_once() { @@ -109,7 +103,7 @@ void cpuload_initialize_once() // } } -void sched_note_start(FAR _TCB *tcb) +void sched_note_start(FAR struct tcb_s *tcb) { /* search first free slot */ int i; @@ -128,7 +122,7 @@ void sched_note_start(FAR _TCB *tcb) } } -void sched_note_stop(FAR _TCB *tcb) +void sched_note_stop(FAR struct tcb_s *tcb) { int i; @@ -145,7 +139,7 @@ void sched_note_stop(FAR _TCB *tcb) } } -void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb) +void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb) { uint64_t new_time = hrt_absolute_time(); diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h index a97047ea8..c7aa18d3c 100644 --- a/src/modules/systemlib/cpuload.h +++ b/src/modules/systemlib/cpuload.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 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 @@ -43,7 +43,7 @@ struct system_load_taskinfo_s { uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load uint64_t curr_start_time; ///< Start time of the current scheduling slot uint64_t start_time; ///< FIRST start time of task - FAR struct _TCB *tcb; ///< + FAR struct tcb_s *tcb; ///< bool valid; ///< Task is currently active / valid }; @@ -60,4 +60,6 @@ __EXPORT extern struct system_load_s system_load; __EXPORT void cpuload_initialize_once(void); +__END_DECLS + #endif diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index daf17ef8b..6c0e876d1 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 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 @@ -61,7 +61,7 @@ const char * getprogname(void) { #if CONFIG_TASK_NAME_SIZE > 0 - _TCB *thisproc = sched_self(); + FAR struct tcb_s *thisproc = sched_self(); return thisproc->name; #else diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h index 84097b49f..dadec51ec 100644 --- a/src/modules/systemlib/geo/geo.h +++ b/src/modules/systemlib/geo/geo.h @@ -45,9 +45,23 @@ * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> */ +#pragma once + +__BEGIN_DECLS #include <stdbool.h> +#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ +#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */ +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ +#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */ + +/* compatibility aliases */ +#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH +#define GRAVITY_MSS CONSTANTS_ONE_G + +// XXX remove struct crosstrack_error_s { bool past_end; // Flag indicating we are past the end of the line/arc segment float distance; // Distance in meters to closest point on line/arc @@ -111,3 +125,5 @@ __EXPORT float _wrap_180(float bearing); __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); + +__END_DECLS diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 2aafe0636..9308100b0 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -1,9 +1,11 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,14 +38,21 @@ /** * @file pid.c - * Implementation of generic PID control interface + * + * Implementation of generic PID control interface. + * + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ #include "pid.h" #include <math.h> __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, float diff_filter_factor, uint8_t mode) + float limit, uint8_t mode, float dt_min) { pid->kp = kp; pid->ki = ki; @@ -51,17 +60,15 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; - pid->diff_filter_factor = diff_filter_factor; + pid->dt_min = dt_min; pid->count = 0.0f; pid->saturated = 0.0f; pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous_filtered = 0.0f; - pid->control_previous = 0.0f; + pid->error_previous = 0.0f; pid->integral = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) { int ret = 0; @@ -100,13 +107,6 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } - if (isfinite(diff_filter_factor)) { - pid->diff_filter_factor = diff_filter_factor; - - } else { - ret = 1; - } - return ret; } @@ -144,20 +144,16 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - float error_filtered; // Calculate or measured current error derivative - if (pid->mode == PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; - error_filtered = pid->error_previous_filtered + (error - pid->error_previous_filtered) * pid->diff_filter_factor; - d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt - pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; - error_filtered = pid->error_previous_filtered + (- val - pid->error_previous_filtered) * pid->diff_filter_factor; - d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt - pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -165,6 +161,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } + if (!isfinite(d)) { + d = 0.0f; + } + // Calculate the error integral and check for saturation i = pid->integral + (error * dt); @@ -175,7 +175,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } else { if (!isfinite(i)) { - i = 0; + i = 0.0f; } pid->integral = i; @@ -184,19 +184,17 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the output. Limit output magnitude to pid->limit float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - if (output > pid->limit) output = pid->limit; - - if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { - pid->last_output = output; - } + if (output > pid->limit) { + output = pid->limit; - pid->control_previous = pid->last_output; + } else if (output < -pid->limit) { + output = -pid->limit; + } - // if (isfinite(error)) { // Why is this necessary? DEW - // pid->error_previous = error; - // } + pid->last_output = output; + } *ctrl_p = (error * pid->kp); *ctrl_i = (i * pid->ki); diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index eb9df96cc..5f2650f69 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -1,9 +1,11 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +38,14 @@ /** * @file pid.h - * Definition of generic PID control interface + * + * Definition of generic PID control interface. + * + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ #ifndef PID_H_ @@ -44,6 +53,8 @@ #include <stdint.h> +__BEGIN_DECLS + /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error * val_dot in pid_calculate() will be ignored */ #define PID_MODE_DERIVATIV_CALC 0 @@ -62,22 +73,22 @@ typedef struct { float intmax; float sp; float integral; - float error_previous_filtered; - float control_previous; + float error_previous; float last_output; float limit; + float dt_min; uint8_t mode; - float diff_filter_factor; uint8_t count; uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d); __EXPORT void pid_reset_integral(PID_t *pid); +__END_DECLS #endif /* PID_H_ */ diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index afb7eca29..3283aad8a 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -50,7 +50,7 @@ #include "systemlib.h" -static void kill_task(FAR _TCB *tcb, FAR void *arg); +static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); void killall() { @@ -60,12 +60,12 @@ void killall() sched_foreach(kill_task, NULL); } -static void kill_task(FAR _TCB *tcb, FAR void *arg) +static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) { kill(tcb->pid, SIGUSR1); } -int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) +int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) { int pid; diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 2c53c648b..0194b5e52 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,11 @@ #include <float.h> #include <stdint.h> -__BEGIN_DECLS - /** Reboots the board */ extern void up_systemreset(void) noreturn_function; +__BEGIN_DECLS + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); @@ -58,7 +58,7 @@ __EXPORT void killall(void); #endif /** Starts a task and performs any specific accounting, scheduler setup, etc. */ -__EXPORT int task_spawn(const char *name, +__EXPORT int task_spawn_cmd(const char *name, int priority, int scheduler, int stack_size, diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b602b1714..8ae47ba8b 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -113,6 +113,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp #include "topics/vehicle_global_position_set_triplet.h" ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s); +#include "topics/mission.h" +ORB_DEFINE(mission, struct mission_s); + #include "topics/vehicle_attitude_setpoint.h" ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); @@ -166,6 +169,11 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +#include "topics/telemetry_status.h" +ORB_DEFINE(telemetry_status, struct telemetry_status_s); + #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); +#include "topics/navigation_capabilities.h" +ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 8ce85213b..1ffeda764 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -52,10 +52,11 @@ * Differential pressure. */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - uint16_t differential_pressure_pa; /**< Differential pressure reading */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint16_t differential_pressure_pa; /**< Differential pressure reading */ uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ + float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ + float temperature; /**< Temperature provided by sensor */ }; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h new file mode 100644 index 000000000..253f444b3 --- /dev/null +++ b/src/modules/uORB/topics/mission.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * 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 mission_item.h + * Definition of one mission item. + */ + +#ifndef TOPIC_MISSION_H_ +#define TOPIC_MISSION_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +enum NAV_CMD { + NAV_CMD_WAYPOINT = 0, + NAV_CMD_LOITER_TURN_COUNT, + NAV_CMD_LOITER_TIME_LIMIT, + NAV_CMD_LOITER_UNLIMITED, + NAV_CMD_RETURN_TO_LAUNCH, + NAV_CMD_LAND, + NAV_CMD_TAKEOFF +}; + +/** + * Global position setpoint in WGS84 coordinates. + * + * This is the position the MAV is heading towards. If it of type loiter, + * the MAV is circling around it with the given loiter radius in meters. + */ +struct mission_item_s +{ + bool altitude_is_relative; /**< true if altitude is relative from start point */ + double lat; /**< latitude in degrees * 1E7 */ + double lon; /**< longitude in degrees * 1E7 */ + float altitude; /**< altitude in meters */ + float yaw; /**< in radians NED -PI..+PI */ + float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ + uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ + enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ + float param1; + float param2; + float param3; + float param4; +}; + +struct mission_s +{ + struct mission_item_s *items; + unsigned count; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(mission); + +#endif diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h new file mode 100644 index 000000000..6a3e811e3 --- /dev/null +++ b/src/modules/uORB/topics/navigation_capabilities.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2012-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 navigation_capabilities.h + * + * Definition of navigation capabilities uORB topic. + */ + +#ifndef TOPIC_NAVIGATION_CAPABILITIES_H_ +#define TOPIC_NAVIGATION_CAPABILITIES_H_ + +#include "../uORB.h" +#include <stdint.h> + +/** + * @addtogroup topics + * @{ + */ + +/** + * Airspeed + */ +struct navigation_capabilities_s { + float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(navigation_capabilities); + +#endif diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h new file mode 100644 index 000000000..f30852de5 --- /dev/null +++ b/src/modules/uORB/topics/telemetry_status.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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 telemetry_status.h + * + * Telemetry status topics - radio status outputs + */ + +#ifndef TOPIC_TELEMETRY_STATUS_H +#define TOPIC_TELEMETRY_STATUS_H + +#include <stdint.h> +#include "../uORB.h" + +enum TELEMETRY_STATUS_RADIO_TYPE { + TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0, + TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO, + TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET, + TELEMETRY_STATUS_RADIO_TYPE_WIRE +}; + +struct telemetry_status_s { + uint64_t timestamp; + enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ + unsigned rssi; /**< local signal strength */ + unsigned remote_rssi; /**< remote signal strength */ + unsigned rxerrors; /**< receive errors */ + unsigned fixed; /**< count of error corrected packets */ + uint8_t noise; /**< background noise level */ + uint8_t remote_noise; /**< remote background noise level */ + uint8_t txbuf; /**< how full the tx buffer is as a percentage */ +}; + +ORB_DECLARE(telemetry_status); + +#endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index eec6a8229..3ae3ff28c 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -45,6 +45,7 @@ #include <stdint.h> #include <stdbool.h> #include "../uORB.h" +#include "mission.h" /** * @addtogroup topics @@ -65,7 +66,12 @@ struct vehicle_global_position_setpoint_s float altitude; /**< altitude in meters */ float yaw; /**< in radians NED -PI..+PI */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ - bool is_loiter; /**< true if loitering is enabled */ + uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ + enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ + float param1; + float param2; + float param3; + float param4; }; /** diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 4bcce18fb..7752ffe67 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -146,6 +146,15 @@ int preflight_check_main(int argc, char *argv[]) bool rc_ok = true; char nbuf[20]; + /* first check channel mappings */ + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + for (int i = 0; i < 12; i++) { /* should the channel be enabled? */ uint8_t count = 0; @@ -209,8 +218,8 @@ int preflight_check_main(int argc, char *argv[]) count++; } - /* XXX needs inspection of all the _MAP params */ - // if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); // /* give system time to flush error message in case there are more */ // usleep(100000); diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 59d2bc8f1..efe62685c 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +36,8 @@ * @file top.c * Tool similar to UNIX top command * @see http://en.wikipedia.org/wiki/Top_unix + * + * @author Lorenz Meier <lm@inf.ethz.ch> */ #include <nuttx/config.h> |