fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 15:17:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into new_state_machine

Browse Source 
compiling again
Conflicts:
	src/modules/fixedwing_att_control/fixedwing_att_control_att.c
	src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
	src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
	src/modules/mavlink/orb_listener.c
	src/modules/multirotor_att_control/multirotor_attitude_control.c
	src/modules/multirotor_att_control/multirotor_rate_control.c
	src/modules/systemlib/pid/pid.c
	src/modules/systemlib/pid/pid.h
	src/modules/uORB/objects_common.cpp
This commit is contained in:
Julian Oes
2013-07-08 10:31:32 +02:00
parent 76346bfe19 cf2dbdf9a1
commit 88389ea255
2165 changed files with 10906 additions and 485356 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ardrone_interface/ardrone_interface.c
+1 -1
View File
@@ -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,
src/drivers/boards/px4fmu/px4fmu_init.c
+1 -1
View File
@@ -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"
src/drivers/boards/px4fmu/px4fmu_internal.h
+1 -1
View File
@@ -50,7 +50,7 @@
__BEGIN_DECLS
/* these headers are not C++ safe */
#include <stm32_internal.h>
#include <stm32.h>
/****************************************************************************************************
src/drivers/boards/px4fmu/px4fmu_led.c
+1 -1
View File
@@ -41,7 +41,7 @@
#include <stdbool.h>
#include "stm32_internal.h"
#include "stm32.h"
#include "px4fmu_internal.h"
#include <arch/board/board.h>
src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
+1 -1
View File
@@ -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>
src/drivers/boards/px4fmu/px4fmu_spi.c
+1 -1
View File
@@ -52,7 +52,7 @@
#include "up_arch.h"
#include "chip.h"
#include "stm32_internal.h"
#include "stm32.h"
#include "px4fmu_internal.h"
/************************************************************************************
src/drivers/boards/px4fmu/px4fmu_usb.c
+1 -1
View File
@@ -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"
/************************************************************************************
src/drivers/boards/px4io/px4io_init.c
+1 -1
View File
@@ -54,7 +54,7 @@
#include <nuttx/arch.h>
#include "stm32_internal.h"
#include "stm32.h"
#include "px4io_internal.h"
#include "stm32_uart.h"
src/drivers/boards/px4io/px4io_internal.h
+1 -1
View File
@@ -47,7 +47,7 @@
#include <nuttx/compiler.h>
#include <stdint.h>
#include <stm32_internal.h>
#include <stm32.h>
/************************************************************************************
* Definitions
src/drivers/boards/px4io/px4io_pwm_servo.c
+1 -1
View File
@@ -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>
src/drivers/drv_gpio.h
+3 -3
View File
@@ -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.
*
src/drivers/ets_airspeed/ets_airspeed.cpp
+3 -2
View File
@@ -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;
src/drivers/hil/hil.cpp
+1 -1
View File
@@ -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,
src/drivers/hott_telemetry/hott_telemetry_main.c
+1 -1
View File
@@ -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,
src/drivers/hott_telemetry/messages.c
+1 -1
View File
@@ -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;
src/drivers/md25/md25_main.cpp
+1 -1
View File
@@ -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,
src/drivers/mkblctrl/mkblctrl.cpp
+1 -1
View File
@@ -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,
src/drivers/px4fmu/fmu.cpp
+1 -1
View File
@@ -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,
src/drivers/px4io/uploader.cpp
+8 -1
View File
@@ -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;
}
src/drivers/stm32/adc/adc.cpp
+1 -1
View File
@@ -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>
src/drivers/stm32/drv_hrt.c
+4 -2
View File
@@ -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
src/drivers/stm32/drv_pwm_servo.c
+1 -1
View File
@@ -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>
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+1 -1
View File
@@ -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>
src/examples/fixedwing_control/main.c
+1 -1
View File
@@ -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,
src/examples/flow_position_control/flow_position_control_params.c
+11
View File
@@ -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);
src/examples/px4_daemon_app/px4_daemon_app.c
+1 -1
View File
@@ -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,
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+110 -88
View File
@@ -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);
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+15 -2
View File
@@ -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 */
src/modules/att_pos_estimator_ekf/kalman_main.cpp
+12 -9
View File
@@ -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;
src/modules/att_pos_estimator_ekf/module.mk
-3
View File
@@ -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
src/modules/att_pos_estimator_ekf/params.c
+37 -4
View File
@@ -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);
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+1 -1
View File
@@ -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,
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+1 -1
View File
@@ -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,
src/modules/commander/accelerometer_calibration.c
+42 -8
View File
@@ -1,12 +1,45 @@
/*
* accelerometer_calibration.c
/****************************************************************************
*
* 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>
*
* Transform acceleration vector to true orientation and scale
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * * * Model * * *
* 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.c
*
* 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"
src/modules/commander/accelerometer_calibration.h
+39 -4
View File
@@ -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_
src/modules/commander/commander.c
+1 -1
View File
@@ -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,
src/modules/controllib/block/UOrbPublication.hpp
+6 -6
View File
@@ -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() {}
/*
src/modules/controllib/fixedwing.cpp
+5 -5
View File
@@ -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;
src/modules/controllib/fixedwing.hpp
+3 -3
View File
@@ -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:
src/modules/fixedwing_att_control/fixedwing_att_control_att.c
+4 -4
View File
@@ -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) */
src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+1 -1
View File
@@ -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,
src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
+6 -6
View File
@@ -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);
}
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+2 -1
View File
@@ -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,
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+9 -9
View File
@@ -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,
src/modules/mathlib/math/Dcm.cpp
+9
View File
@@ -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)
{
src/modules/mathlib/math/Dcm.hpp
+5
View File
@@ -76,6 +76,11 @@ public:
*/
Dcm(const float *data);
/**
* array ctor
*/
Dcm(const float data[3][3]);
/**
* quaternion ctor
*/
src/modules/mathlib/math/Limits.cpp
+146
View File
@@ -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;
}
}
src/modules/mathlib/math/Limits.hpp
+87
View File
@@ -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);
}
src/modules/mathlib/math/Quaternion.hpp
+1 -1
View File
@@ -37,7 +37,7 @@
* math quaternion lib
*/
//#pragma once
#pragma once
#include "Vector.hpp"
#include "Matrix.hpp"
src/modules/mathlib/math/Vector2f.cpp
+103
View File
@@ -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
src/modules/mathlib/math/Vector2f.hpp
+79
View File
@@ -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
src/modules/mathlib/math/Vector3.cpp
+2 -2
View File
@@ -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);
src/modules/mathlib/math/Vector3.hpp
+6 -1
View File
@@ -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
src/modules/mathlib/math/arm/Vector.hpp
+19 -3
View File
@@ -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
src/modules/mathlib/math/generic/Vector.hpp
+21 -3
View File
@@ -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
src/modules/mathlib/mathlib.h
+4
View File
@@ -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
src/modules/mathlib/module.mk
+3 -1
View File
@@ -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
src/modules/mavlink/mavlink.c
+6 -9
View File
@@ -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,
src/modules/mavlink/mavlink_hil.h
-9
View File
@@ -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.
*
src/modules/mavlink/mavlink_receiver.c → src/modules/mavlink/mavlink_receiver.cpp
+142 -69
View File
@@ -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);
/* 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);
}
// increment counters
hil_counter += 1 ;
hil_frames += 1 ;
// 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
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;
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;
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, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2048);
pthread_attr_setstacksize(&receiveloop_attr, 3000);
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
src/modules/mavlink/missionlib.c
+174 -1
View File
@@ -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)
@@ -122,6 +127,52 @@ uint64_t mavlink_missionlib_get_system_timestamp()
return hrt_absolute_time();
}
/**
* 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.
*
@@ -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);
src/modules/mavlink/module.mk
+1 -1
View File
@@ -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
src/modules/mavlink/orb_listener.c
+30 -11
View File
@@ -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);
src/modules/mavlink/orb_topics.h
+4
View File
@@ -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;
src/modules/mavlink/waypoints.c
+47 -21
View File
@@ -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 = wpm->waypoints[wpm->current_active_wp_id].param2;
float orbit;
if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
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;
}
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;
src/modules/mavlink_onboard/mavlink.c
+1 -1
View File
@@ -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,
src/modules/multirotor_att_control/multirotor_att_control_main.c
+1 -1
View File
@@ -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,
src/modules/multirotor_att_control/multirotor_attitude_control.c
+19 -15
View File
@@ -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 */
src/modules/multirotor_att_control/multirotor_attitude_control.h
+15 -7
View File
@@ -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_
src/modules/multirotor_att_control/multirotor_rate_control.c
+15 -29
View File
@@ -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);
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");
}
/* 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 */
src/modules/multirotor_att_control/multirotor_rate_control.h
+16 -8
View File
@@ -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_
src/modules/multirotor_pos_control/multirotor_pos_control.c
+2 -2
View File
@@ -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,
src/modules/position_estimator_mc/position_estimator_mc_main.c
+1 -1
View File
@@ -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,
src/modules/px4iofirmware/adc.c
+1 -1
View File
@@ -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>
src/modules/sdlog/sdlog.c
+2 -2
View File
@@ -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,
src/modules/sdlog2/logbuffer.c
+2 -2
View File
@@ -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>
src/modules/sdlog2/logbuffer.h
+2 -2
View File
@@ -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_
src/modules/sdlog2/sdlog2.c
+12 -4
View File
@@ -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 --- */
src/modules/sdlog2/sdlog2_format.h
+2 -2
View File
@@ -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>
*/
/*
src/modules/sdlog2/sdlog2_messages.h
+14 -2
View File
@@ -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);
src/modules/sensors/sensor_params.c
+4
View File
@@ -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);
src/modules/sensors/sensors.cpp
+14 -5
View File
@@ -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;
}
src/modules/systemlib/conversions.h
+1 -5
View File
@@ -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
src/modules/systemlib/cpuload.c
+24 -30
View File
@@ -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();
src/modules/systemlib/cpuload.h
+4 -2
View File
@@ -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
src/modules/systemlib/err.c
+2 -2
View File
@@ -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
src/modules/systemlib/geo/geo.h
+16
View File
@@ -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
src/modules/systemlib/pid/pid.c
+34 -36
View File
@@ -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,20 +184,18 @@ __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)) {
if (output > pid->limit) {
output = pid->limit;
} else if (output < -pid->limit) {
output = -pid->limit;
}
pid->last_output = output;
}
pid->control_previous = pid->last_output;
// if (isfinite(error)) { // Why is this necessary? DEW
// pid->error_previous = error;
// }
*ctrl_p = (error * pid->kp);
*ctrl_i = (i * pid->ki);
*ctrl_d = (d * pid->kd);
src/modules/systemlib/pid/pid.h
+21 -10
View File
@@ -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_ */
src/modules/systemlib/systemlib.c
+3 -3
View File
@@ -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;
src/modules/systemlib/systemlib.h
+3 -3
View File
@@ -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,
src/modules/uORB/objects_common.cpp
+8
View File
@@ -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);
src/modules/uORB/topics/differential_pressure.h
+4 -3
View File
@@ -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 */
};
src/modules/uORB/topics/mission.h
+99
View File
@@ -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
src/modules/uORB/topics/navigation_capabilities.h
+65
View File
@@ -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
src/modules/uORB/topics/telemetry_status.h
+67
View File
@@ -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 */
src/modules/uORB/topics/vehicle_global_position_setpoint.h
+7 -1
View File
@@ -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;
};
/**
src/systemcmds/preflight_check/preflight_check.c
+11 -2
View File
@@ -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);
src/systemcmds/top/top.c
+4 -2
View File
@@ -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>