mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 04:33:10 +08:00
Merge pull request #156 from jgoppert/sensor_hil_rebase
Rebase of changes to sensor_hil_fixedwing branch.
This commit is contained in:
px4dev
@@ -8,6 +8,7 @@
|
||||
apps/namedapp/namedapp_list.h
|
||||
apps/namedapp/namedapp_proto.h
|
||||
Make.dep
|
||||
*.pyc
|
||||
*.o
|
||||
*.a
|
||||
*.d
|
||||
@@ -43,3 +44,7 @@ cscope.out
|
||||
.configX-e
|
||||
nuttx-export.zip
|
||||
dot.gdbinit
|
||||
mavlink/include/mavlink/v0.9/
|
||||
.*.swp
|
||||
.swp
|
||||
core
|
||||
|
||||
@@ -134,7 +134,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|
||||
|
||||
case SYSTEM_STATE_REBOOT:
|
||||
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|
||||
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
|
||||
|| current_status->flag_hil_enabled) {
|
||||
invalid_state = false;
|
||||
/* set system flags according to state */
|
||||
current_status->flag_system_armed = false;
|
||||
@@ -708,7 +709,9 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
|
||||
case SYSTEM_STATE_REBOOT:
|
||||
printf("try to reboot\n");
|
||||
|
||||
if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
|
||||
if (current_system_state == SYSTEM_STATE_STANDBY
|
||||
|| current_system_state == SYSTEM_STATE_PREFLIGHT
|
||||
|| current_status->flag_hil_enabled) {
|
||||
printf("system will reboot\n");
|
||||
mavlink_log_critical(mavlink_fd, "Rebooting..");
|
||||
usleep(200000);
|
||||
|
||||
@@ -108,7 +108,7 @@ int control_demo_main(int argc, char *argv[])
|
||||
deamon_task = task_spawn("control_demo",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 10,
|
||||
4096,
|
||||
5120,
|
||||
control_demo_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
|
||||
@@ -6,31 +6,31 @@
|
||||
// 16 is max name length
|
||||
|
||||
// gyro low pass filter
|
||||
PARAM_DEFINE_FLOAT(FWB_P_LP, 10.0f); // roll rate low pass cut freq
|
||||
PARAM_DEFINE_FLOAT(FWB_Q_LP, 10.0f); // pitch rate low pass cut freq
|
||||
PARAM_DEFINE_FLOAT(FWB_R_LP, 10.0f); // yaw rate low pass cut freq
|
||||
PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq
|
||||
PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq
|
||||
PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq
|
||||
|
||||
// yaw washout
|
||||
PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
|
||||
|
||||
// stabilization mode
|
||||
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.1f); // roll rate 2 aileron
|
||||
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
|
||||
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
|
||||
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron
|
||||
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.5f); // pitch rate 2 elevator
|
||||
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder
|
||||
|
||||
// psi -> phi -> p
|
||||
PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 2.0f); // heading 2 roll
|
||||
PARAM_DEFINE_FLOAT(FWB_PHI2P, 2.0f); // roll to roll rate
|
||||
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 1.0f); // roll limit
|
||||
PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll
|
||||
PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate
|
||||
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f); // roll limit, 28 deg
|
||||
|
||||
// velocity -> theta
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.2f);
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -1.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f);
|
||||
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);
|
||||
|
||||
|
||||
// theta -> q
|
||||
@@ -41,15 +41,15 @@ PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
|
||||
|
||||
// h -> thr
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.005f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.001f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 250.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
|
||||
|
||||
// crosstrack
|
||||
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // 90 deg
|
||||
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);
|
||||
|
||||
// speed command
|
||||
PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
|
||||
@@ -58,6 +58,6 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
|
||||
|
||||
// trim
|
||||
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);
|
||||
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.7f);
|
||||
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f);
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -60,6 +60,11 @@
|
||||
#include <poll.h>
|
||||
#include <unistd.h>
|
||||
|
||||
/**
|
||||
* Kalman filter navigation class
|
||||
* http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
* Discrete-time extended Kalman filter
|
||||
*/
|
||||
class KalmanNav : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
@@ -70,7 +75,7 @@ public:
|
||||
void predictFast(float dt);
|
||||
void predictSlow(float dt);
|
||||
void correctAtt();
|
||||
void correctGps();
|
||||
void correctPos();
|
||||
virtual void updateParams();
|
||||
protected:
|
||||
math::Matrix F;
|
||||
@@ -79,8 +84,8 @@ protected:
|
||||
math::Matrix V;
|
||||
math::Matrix HAtt;
|
||||
math::Matrix RAtt;
|
||||
math::Matrix HGps;
|
||||
math::Matrix RGps;
|
||||
math::Matrix HPos;
|
||||
math::Matrix RPos;
|
||||
math::Dcm C_nb;
|
||||
math::Quaternion q;
|
||||
control::UOrbSubscription<sensor_combined_s> _sensors;
|
||||
@@ -94,6 +99,8 @@ protected:
|
||||
uint64_t _attTimeStamp;
|
||||
uint64_t _outTimeStamp;
|
||||
uint16_t _navFrames;
|
||||
uint16_t _missFast;
|
||||
uint16_t _missSlow;
|
||||
float fN, fE, fD;
|
||||
// states
|
||||
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
|
||||
@@ -103,8 +110,8 @@ protected:
|
||||
control::BlockParam<float> _vGyro;
|
||||
control::BlockParam<float> _vAccel;
|
||||
control::BlockParam<float> _rMag;
|
||||
control::BlockParam<float> _rGpsV;
|
||||
control::BlockParam<float> _rGpsGeo;
|
||||
control::BlockParam<float> _rGpsVel;
|
||||
control::BlockParam<float> _rGpsPos;
|
||||
control::BlockParam<float> _rGpsAlt;
|
||||
control::BlockParam<float> _rAccel;
|
||||
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
|
||||
|
||||
@@ -3,8 +3,8 @@
|
||||
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
|
||||
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_MAG, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_V, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_GEO, 1.0e-7f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 10.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
|
||||
|
||||
+60
-31
@@ -52,6 +52,23 @@ Dcm::Dcm() :
|
||||
{
|
||||
}
|
||||
|
||||
Dcm::Dcm(float c00, float c01, float c02,
|
||||
float c10, float c11, float c12,
|
||||
float c20, float c21, float c22) :
|
||||
Matrix(3, 3)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
dcm(0, 0) = c00;
|
||||
dcm(0, 1) = c01;
|
||||
dcm(0, 2) = c02;
|
||||
dcm(1, 0) = c10;
|
||||
dcm(1, 1) = c11;
|
||||
dcm(1, 2) = c12;
|
||||
dcm(2, 0) = c20;
|
||||
dcm(2, 1) = c21;
|
||||
dcm(2, 2) = c22;
|
||||
}
|
||||
|
||||
Dcm::Dcm(const float *data) :
|
||||
Matrix(3, 3, data)
|
||||
{
|
||||
@@ -61,22 +78,22 @@ Dcm::Dcm(const Quaternion &q) :
|
||||
Matrix(3, 3)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
float a = q.getA();
|
||||
float b = q.getB();
|
||||
float c = q.getC();
|
||||
float d = q.getD();
|
||||
float aSq = a * a;
|
||||
float bSq = b * b;
|
||||
float cSq = c * c;
|
||||
float dSq = d * d;
|
||||
double a = q.getA();
|
||||
double b = q.getB();
|
||||
double c = q.getC();
|
||||
double d = q.getD();
|
||||
double aSq = a * a;
|
||||
double bSq = b * b;
|
||||
double cSq = c * c;
|
||||
double dSq = d * d;
|
||||
dcm(0, 0) = aSq + bSq - cSq - dSq;
|
||||
dcm(0, 1) = 2 * (b * c - a * d);
|
||||
dcm(0, 2) = 2 * (a * c + b * d);
|
||||
dcm(1, 0) = 2 * (b * c + a * d);
|
||||
dcm(0, 1) = 2.0 * (b * c - a * d);
|
||||
dcm(0, 2) = 2.0 * (a * c + b * d);
|
||||
dcm(1, 0) = 2.0 * (b * c + a * d);
|
||||
dcm(1, 1) = aSq - bSq + cSq - dSq;
|
||||
dcm(1, 2) = 2 * (c * d - a * b);
|
||||
dcm(2, 0) = 2 * (b * d - a * c);
|
||||
dcm(2, 1) = 2 * (a * b + c * d);
|
||||
dcm(1, 2) = 2.0 * (c * d - a * b);
|
||||
dcm(2, 0) = 2.0 * (b * d - a * c);
|
||||
dcm(2, 1) = 2.0 * (a * b + c * d);
|
||||
dcm(2, 2) = aSq - bSq - cSq + dSq;
|
||||
}
|
||||
|
||||
@@ -84,12 +101,12 @@ Dcm::Dcm(const EulerAngles &euler) :
|
||||
Matrix(3, 3)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
float cosPhi = cosf(euler.getPhi());
|
||||
float sinPhi = sinf(euler.getPhi());
|
||||
float cosThe = cosf(euler.getTheta());
|
||||
float sinThe = sinf(euler.getTheta());
|
||||
float cosPsi = cosf(euler.getPsi());
|
||||
float sinPsi = sinf(euler.getPsi());
|
||||
double cosPhi = cos(euler.getPhi());
|
||||
double sinPhi = sin(euler.getPhi());
|
||||
double cosThe = cos(euler.getTheta());
|
||||
double sinThe = sin(euler.getTheta());
|
||||
double cosPsi = cos(euler.getPsi());
|
||||
double sinPsi = sin(euler.getPsi());
|
||||
|
||||
dcm(0, 0) = cosThe * cosPsi;
|
||||
dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
|
||||
@@ -116,18 +133,30 @@ Dcm::~Dcm()
|
||||
int __EXPORT dcmTest()
|
||||
{
|
||||
printf("Test DCM\t\t: ");
|
||||
// default ctor
|
||||
ASSERT(matrixEqual(Dcm(),
|
||||
Matrix::identity(3)));
|
||||
// quaternion ctor
|
||||
ASSERT(matrixEqual(
|
||||
Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
|
||||
Dcm(0.9362934f, -0.2750958f, 0.2183507f,
|
||||
0.2896295f, 0.9564251f, -0.0369570f,
|
||||
-0.1986693f, 0.0978434f, 0.9751703f)));
|
||||
// euler angle ctor
|
||||
ASSERT(matrixEqual(
|
||||
Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
|
||||
Dcm(0.9362934f, -0.2750958f, 0.2183507f,
|
||||
0.2896295f, 0.9564251f, -0.0369570f,
|
||||
-0.1986693f, 0.0978434f, 0.9751703f)));
|
||||
// rotations
|
||||
Vector3 vB(1, 2, 3);
|
||||
ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)),
|
||||
Matrix::identity(3)));
|
||||
ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)),
|
||||
Matrix::identity(3)));
|
||||
ASSERT(vectorEqual(Vector3(-2, 1, 3),
|
||||
Dcm(EulerAngles(0, 0, M_PI_2_F))*vB));
|
||||
ASSERT(vectorEqual(Vector3(3, 2, -1),
|
||||
Dcm(EulerAngles(0, M_PI_2_F, 0))*vB));
|
||||
ASSERT(vectorEqual(Vector3(1, -3, 2),
|
||||
Dcm(EulerAngles(M_PI_2_F, 0, 0))*vB));
|
||||
ASSERT(vectorEqual(Vector3(3, 2, -1),
|
||||
ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
|
||||
Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
|
||||
ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
|
||||
Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
|
||||
ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
|
||||
Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
|
||||
ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
|
||||
Dcm(EulerAngles(
|
||||
M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
|
||||
printf("PASS\n");
|
||||
|
||||
@@ -64,6 +64,13 @@ public:
|
||||
*/
|
||||
Dcm();
|
||||
|
||||
/**
|
||||
* scalar ctor
|
||||
*/
|
||||
Dcm(float c00, float c01, float c02,
|
||||
float c10, float c11, float c12,
|
||||
float c20, float c21, float c22);
|
||||
|
||||
/**
|
||||
* data ctor
|
||||
*/
|
||||
|
||||
@@ -97,23 +97,27 @@ EulerAngles::~EulerAngles()
|
||||
int __EXPORT eulerAnglesTest()
|
||||
{
|
||||
printf("Test EulerAngles\t: ");
|
||||
EulerAngles euler(1, 2, 3);
|
||||
EulerAngles euler(0.1f, 0.2f, 0.3f);
|
||||
|
||||
// test ctor
|
||||
ASSERT(vectorEqual(Vector3(1, 2, 3), euler));
|
||||
ASSERT(equal(euler.getPhi(), 1));
|
||||
ASSERT(equal(euler.getTheta(), 2));
|
||||
ASSERT(equal(euler.getPsi(), 3));
|
||||
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
|
||||
ASSERT(equal(euler.getPhi(), 0.1f));
|
||||
ASSERT(equal(euler.getTheta(), 0.2f));
|
||||
ASSERT(equal(euler.getPsi(), 0.3f));
|
||||
|
||||
// test dcm ctor
|
||||
euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
|
||||
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
|
||||
|
||||
// test quat ctor
|
||||
euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
|
||||
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
|
||||
|
||||
// test assignment
|
||||
euler.setPhi(4);
|
||||
ASSERT(equal(euler.getPhi(), 4));
|
||||
euler.setTheta(5);
|
||||
ASSERT(equal(euler.getTheta(), 5));
|
||||
euler.setPsi(6);
|
||||
ASSERT(equal(euler.getPsi(), 6));
|
||||
euler.setPhi(0.4f);
|
||||
euler.setTheta(0.5f);
|
||||
euler.setPsi(0.6f);
|
||||
ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
|
||||
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
|
||||
@@ -79,32 +79,34 @@ Quaternion::Quaternion(const Vector &v) :
|
||||
Quaternion::Quaternion(const Dcm &dcm) :
|
||||
Vector(4)
|
||||
{
|
||||
setA(0.5f * sqrtf(1 + dcm(0, 0) +
|
||||
dcm(1, 1) + dcm(2, 2)));
|
||||
setB((dcm(2, 1) - dcm(1, 2)) /
|
||||
(4 * getA()));
|
||||
setC((dcm(0, 2) - dcm(2, 0)) /
|
||||
(4 * getA()));
|
||||
setD((dcm(1, 0) - dcm(0, 1)) /
|
||||
(4 * getA()));
|
||||
// avoiding singularities by not using
|
||||
// division equations
|
||||
setA(0.5 * sqrt(1.0 +
|
||||
double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
|
||||
setB(0.5 * sqrt(1.0 +
|
||||
double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
|
||||
setC(0.5 * sqrt(1.0 +
|
||||
double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
|
||||
setD(0.5 * sqrt(1.0 +
|
||||
double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(const EulerAngles &euler) :
|
||||
Vector(4)
|
||||
{
|
||||
float cosPhi_2 = cosf(euler.getPhi() / 2.0f);
|
||||
float cosTheta_2 = cosf(euler.getTheta() / 2.0f);
|
||||
float cosPsi_2 = cosf(euler.getPsi() / 2.0f);
|
||||
float sinPhi_2 = sinf(euler.getPhi() / 2.0f);
|
||||
float sinTheta_2 = sinf(euler.getTheta() / 2.0f);
|
||||
float sinPsi_2 = sinf(euler.getPsi() / 2.0f);
|
||||
double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
|
||||
double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
|
||||
double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
|
||||
double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
|
||||
double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
|
||||
double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
|
||||
setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
|
||||
sinPhi_2 * sinTheta_2 * sinPsi_2);
|
||||
setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
|
||||
cosPhi_2 * sinTheta_2 * sinPsi_2);
|
||||
setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
|
||||
sinPhi_2 * cosTheta_2 * sinPsi_2);
|
||||
setD(cosPhi_2 * cosTheta_2 * sinPsi_2 +
|
||||
setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
|
||||
sinPhi_2 * sinTheta_2 * cosPsi_2);
|
||||
}
|
||||
|
||||
@@ -142,38 +144,29 @@ int __EXPORT quaternionTest()
|
||||
printf("Test Quaternion\t\t: ");
|
||||
// test default ctor
|
||||
Quaternion q;
|
||||
ASSERT(equal(q.getA(), 1));
|
||||
ASSERT(equal(q.getB(), 0));
|
||||
ASSERT(equal(q.getC(), 0));
|
||||
ASSERT(equal(q.getD(), 0));
|
||||
ASSERT(equal(q.getA(), 1.0f));
|
||||
ASSERT(equal(q.getB(), 0.0f));
|
||||
ASSERT(equal(q.getC(), 0.0f));
|
||||
ASSERT(equal(q.getD(), 0.0f));
|
||||
// test float ctor
|
||||
q = Quaternion(0, 1, 0, 0);
|
||||
ASSERT(equal(q.getA(), 0));
|
||||
ASSERT(equal(q.getB(), 1));
|
||||
ASSERT(equal(q.getC(), 0));
|
||||
ASSERT(equal(q.getD(), 0));
|
||||
q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
|
||||
ASSERT(equal(q.getA(), 0.1825742f));
|
||||
ASSERT(equal(q.getB(), 0.3651484f));
|
||||
ASSERT(equal(q.getC(), 0.5477226f));
|
||||
ASSERT(equal(q.getD(), 0.7302967f));
|
||||
// test euler ctor
|
||||
q = Quaternion(EulerAngles(0, 0, 0));
|
||||
ASSERT(equal(q.getA(), 1));
|
||||
ASSERT(equal(q.getB(), 0));
|
||||
ASSERT(equal(q.getC(), 0));
|
||||
ASSERT(equal(q.getD(), 0));
|
||||
q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
|
||||
ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
|
||||
// test dcm ctor
|
||||
q = Quaternion(Dcm());
|
||||
ASSERT(equal(q.getA(), 1));
|
||||
ASSERT(equal(q.getB(), 0));
|
||||
ASSERT(equal(q.getC(), 0));
|
||||
ASSERT(equal(q.getD(), 0));
|
||||
ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f)));
|
||||
// TODO test derivative
|
||||
// test accessors
|
||||
q.setA(0.1);
|
||||
q.setB(0.2);
|
||||
q.setC(0.3);
|
||||
q.setD(0.4);
|
||||
ASSERT(equal(q.getA(), 0.1));
|
||||
ASSERT(equal(q.getB(), 0.2));
|
||||
ASSERT(equal(q.getC(), 0.3));
|
||||
ASSERT(equal(q.getD(), 0.4));
|
||||
q.setA(0.1f);
|
||||
q.setB(0.2f);
|
||||
q.setC(0.3f);
|
||||
q.setD(0.4f);
|
||||
ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
Binary file not shown.
@@ -0,0 +1,63 @@
|
||||
clc
|
||||
clear
|
||||
function out = float_truncate(in, digits)
|
||||
out = round(in*10^digits)
|
||||
out = out/10^digits
|
||||
endfunction
|
||||
|
||||
phi = 0.1
|
||||
theta = 0.2
|
||||
psi = 0.3
|
||||
|
||||
cosPhi = cos(phi)
|
||||
cosPhi_2 = cos(phi/2)
|
||||
sinPhi = sin(phi)
|
||||
sinPhi_2 = sin(phi/2)
|
||||
|
||||
cosTheta = cos(theta)
|
||||
cosTheta_2 = cos(theta/2)
|
||||
sinTheta = sin(theta)
|
||||
sinTheta_2 = sin(theta/2)
|
||||
|
||||
cosPsi = cos(psi)
|
||||
cosPsi_2 = cos(psi/2)
|
||||
sinPsi = sin(psi)
|
||||
sinPsi_2 = sin(psi/2)
|
||||
|
||||
C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi;
|
||||
cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi;
|
||||
-sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]
|
||||
|
||||
disp(C_nb)
|
||||
//C_nb = float_truncate(C_nb,3)
|
||||
//disp(C_nb)
|
||||
|
||||
theta = asin(-C_nb(3,1))
|
||||
phi = atan(C_nb(3,2), C_nb(3,3))
|
||||
psi = atan(C_nb(2,1), C_nb(1,1))
|
||||
printf('phi %f\n', phi)
|
||||
printf('theta %f\n', theta)
|
||||
printf('psi %f\n', psi)
|
||||
|
||||
q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2;
|
||||
sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2;
|
||||
cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2;
|
||||
cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]
|
||||
|
||||
//q = float_truncate(q,3)
|
||||
|
||||
a = q(1)
|
||||
b = q(2)
|
||||
c = q(3)
|
||||
d = q(4)
|
||||
printf('q: %f %f %f %f\n', a, b, c, d)
|
||||
a2 = a*a
|
||||
b2 = b*b
|
||||
c2 = c*c
|
||||
d2 = d*d
|
||||
|
||||
C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c);
|
||||
2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b);
|
||||
2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]
|
||||
|
||||
disp(C2_nb)
|
||||
@@ -77,7 +77,7 @@
|
||||
/* define MAVLink specific parameters */
|
||||
PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
|
||||
PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
|
||||
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_QUADROTOR);
|
||||
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
|
||||
|
||||
__EXPORT int mavlink_main(int argc, char *argv[]);
|
||||
|
||||
@@ -98,7 +98,7 @@ static bool mavlink_link_termination_allowed = false;
|
||||
mavlink_system_t mavlink_system = {
|
||||
100,
|
||||
50,
|
||||
MAV_TYPE_QUADROTOR,
|
||||
MAV_TYPE_FIXED_WING,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
@@ -148,6 +148,10 @@ set_hil_on_off(bool hil_enabled)
|
||||
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 */
|
||||
|
||||
@@ -43,8 +43,12 @@ 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.
|
||||
@@ -54,4 +58,4 @@ extern orb_advert_t pub_hil_attitude;
|
||||
* requested change could not be made or was
|
||||
* redundant.
|
||||
*/
|
||||
extern int set_hil_on_off(bool hil_enabled);
|
||||
extern int set_hil_on_off(bool hil_enabled);
|
||||
|
||||
@@ -86,8 +86,12 @@ static struct offboard_control_setpoint_s offboard_control_sp;
|
||||
|
||||
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 cmd_pub = -1;
|
||||
static orb_advert_t flow_pub = -1;
|
||||
@@ -302,6 +306,161 @@ handle_message(mavlink_message_t *msg)
|
||||
|
||||
if (mavlink_hil_enabled) {
|
||||
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) {
|
||||
|
||||
mavlink_raw_imu_t imu;
|
||||
mavlink_msg_raw_imu_decode(msg, &imu);
|
||||
|
||||
/* packet counter */
|
||||
static uint16_t hil_counter = 0;
|
||||
static uint16_t hil_frames = 0;
|
||||
static uint64_t old_timestamp = 0;
|
||||
|
||||
/* hil gyro */
|
||||
static const float mrad2rad = 1.0e-3f;
|
||||
hil_sensors.timestamp = timestamp;
|
||||
hil_sensors.gyro_counter = hil_counter;
|
||||
hil_sensors.gyro_raw[0] = imu.xgyro;
|
||||
hil_sensors.gyro_raw[1] = imu.ygyro;
|
||||
hil_sensors.gyro_raw[2] = imu.zgyro;
|
||||
hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad;
|
||||
hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad;
|
||||
hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad;
|
||||
|
||||
/* accelerometer */
|
||||
hil_sensors.accelerometer_counter = hil_counter;
|
||||
static const float mg2ms2 = 9.8f / 1000.0f;
|
||||
hil_sensors.accelerometer_raw[0] = imu.xacc;
|
||||
hil_sensors.accelerometer_raw[1] = imu.yacc;
|
||||
hil_sensors.accelerometer_raw[2] = imu.zacc;
|
||||
hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc;
|
||||
hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc;
|
||||
hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc;
|
||||
hil_sensors.accelerometer_mode = 0; // TODO what is this?
|
||||
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
|
||||
|
||||
/* adc */
|
||||
hil_sensors.adc_voltage_v[0] = 0;
|
||||
hil_sensors.adc_voltage_v[1] = 0;
|
||||
hil_sensors.adc_voltage_v[2] = 0;
|
||||
|
||||
/* magnetometer */
|
||||
float mga2ga = 1.0e-3f;
|
||||
hil_sensors.magnetometer_counter = hil_counter;
|
||||
hil_sensors.magnetometer_raw[0] = imu.xmag;
|
||||
hil_sensors.magnetometer_raw[1] = imu.ymag;
|
||||
hil_sensors.magnetometer_raw[2] = imu.zmag;
|
||||
hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga;
|
||||
hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga;
|
||||
hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga;
|
||||
hil_sensors.magnetometer_range_ga = 32.7f; // int16
|
||||
hil_sensors.magnetometer_mode = 0; // TODO what is this
|
||||
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
|
||||
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
|
||||
|
||||
// increment counters
|
||||
hil_counter += 1 ;
|
||||
hil_frames += 1 ;
|
||||
|
||||
// output
|
||||
if ((timestamp - old_timestamp) > 1000000) {
|
||||
printf("receiving hil imu at %d hz\n", hil_frames);
|
||||
old_timestamp = timestamp;
|
||||
hil_frames = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) {
|
||||
|
||||
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;
|
||||
|
||||
/* gps */
|
||||
hil_gps.timestamp = gps.time_usec;
|
||||
hil_gps.counter = hil_counter++;
|
||||
hil_gps.time_gps_usec = gps.time_usec;
|
||||
hil_gps.lat = gps.lat;
|
||||
hil_gps.lon = gps.lon;
|
||||
hil_gps.alt = gps.alt;
|
||||
hil_gps.counter_pos_valid = hil_counter++;
|
||||
hil_gps.eph = gps.eph;
|
||||
hil_gps.epv = gps.epv;
|
||||
hil_gps.s_variance = 100;
|
||||
hil_gps.p_variance = 100;
|
||||
hil_gps.vel = gps.vel;
|
||||
hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
|
||||
hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f);
|
||||
hil_gps.vel_d = 0.0f;
|
||||
hil_gps.cog = gps.cog;
|
||||
hil_gps.fix_type = gps.fix_type;
|
||||
hil_gps.satellites_visible = gps.satellites_visible;
|
||||
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
|
||||
|
||||
// increment counters
|
||||
hil_counter += 1 ;
|
||||
hil_frames += 1 ;
|
||||
|
||||
// output
|
||||
if ((timestamp - old_timestamp) > 1000000) {
|
||||
printf("receiving hil gps at %d hz\n", hil_frames);
|
||||
old_timestamp = timestamp;
|
||||
hil_frames = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) {
|
||||
|
||||
mavlink_raw_pressure_t press;
|
||||
mavlink_msg_raw_pressure_decode(msg, &press);
|
||||
|
||||
/* packet counter */
|
||||
static uint16_t hil_counter = 0;
|
||||
static uint16_t hil_frames = 0;
|
||||
static uint64_t old_timestamp = 0;
|
||||
|
||||
/* baro */
|
||||
/* TODO, set ground_press/ temp during calib */
|
||||
static const float ground_press = 1013.25f; // mbar
|
||||
static const float ground_tempC = 21.0f;
|
||||
static const float ground_alt = 0.0f;
|
||||
static const float T0 = 273.15;
|
||||
static const float R = 287.05f;
|
||||
static const float g = 9.806f;
|
||||
|
||||
float tempC = press.temperature / 100.0f;
|
||||
float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
|
||||
float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs);
|
||||
hil_sensors.baro_counter = hil_counter;
|
||||
hil_sensors.baro_pres_mbar = press.press_abs;
|
||||
hil_sensors.baro_alt_meter = h;
|
||||
hil_sensors.baro_temp_celcius = tempC;
|
||||
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
|
||||
|
||||
// increment counters
|
||||
hil_counter += 1 ;
|
||||
hil_frames += 1 ;
|
||||
|
||||
// output
|
||||
if ((timestamp - old_timestamp) > 1000000) {
|
||||
printf("receiving hil pressure at %d hz\n", hil_frames);
|
||||
old_timestamp = timestamp;
|
||||
hil_frames = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
|
||||
|
||||
mavlink_hil_state_t hil_state;
|
||||
@@ -412,7 +571,7 @@ receive_thread(void *arg)
|
||||
int uart_fd = *((int *)arg);
|
||||
|
||||
const int timeout = 1000;
|
||||
uint8_t ch;
|
||||
uint8_t buf[32];
|
||||
|
||||
mavlink_message_t msg;
|
||||
|
||||
@@ -423,13 +582,12 @@ receive_thread(void *arg)
|
||||
struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
|
||||
|
||||
if (poll(fds, 1, timeout) > 0) {
|
||||
/* non-blocking read until buffer is empty */
|
||||
int nread = 0;
|
||||
/* non-blocking read */
|
||||
size_t nread = read(uart_fd, buf, sizeof(buf));
|
||||
ASSERT(nread > 0)
|
||||
|
||||
do {
|
||||
nread = read(uart_fd, &ch, 1);
|
||||
|
||||
if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
|
||||
for (size_t i = 0; i < nread; i++) {
|
||||
if (mavlink_parse_char(chan, buf[i], &msg, &status)) { //parse the char
|
||||
/* handle generic messages and commands */
|
||||
handle_message(&msg);
|
||||
|
||||
@@ -439,7 +597,7 @@ receive_thread(void *arg)
|
||||
/* Handle packet with parameter component */
|
||||
mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
|
||||
}
|
||||
} while (nread > 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -452,6 +610,10 @@ receive_start(int uart)
|
||||
pthread_attr_t receiveloop_attr;
|
||||
pthread_attr_init(&receiveloop_attr);
|
||||
|
||||
// set to non-blocking read
|
||||
int flags = fcntl(uart, F_GETFL, 0);
|
||||
fcntl(uart, F_SETFL, flags | O_NONBLOCK);
|
||||
|
||||
struct sched_param param;
|
||||
param.sched_priority = SCHED_PRIORITY_MAX - 40;
|
||||
(void)pthread_attr_setschedparam(&receiveloop_attr, ¶m);
|
||||
|
||||
Reference in New Issue
Block a user