fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 01:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

SO(3) estimator and quaternion receive by mavlink implemented

Browse Source
This commit is contained in:
Hyon Lim
2013-11-29 02:05:15 +09:00
parent f1fece2bb6
commit bcd745fb0d
4 changed files with 136 additions and 314 deletions
Download Patch File Download Diff File Expand all files Collapse all files
makefiles/config_px4fmu-v1_default.mk
+1
View File
@@ -72,6 +72,7 @@ MODULES += modules/gpio_led
# Estimation modules (EKF / other filters) # Estimation modules (EKF / other filters)
# #
MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3_comp
MODULES += modules/att_pos_estimator_ekf MODULES += modules/att_pos_estimator_ekf
MODULES += modules/position_estimator_inav MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator MODULES += examples/flow_position_estimator
src/modules/attitude_estimator_so3_comp/README
+1 -3
View File
@@ -1,5 +1,3 @@
Synopsis Synopsis
nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200 nsh> attitude_estimator_so3_comp start
Option -d is for debugging packet. See code for detailed packet structure.
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+120 -310
View File
@@ -56,20 +56,16 @@ extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[])
static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */ static bool thread_running = false; /**< Deamon status flag */
static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */ static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */
//! Auxiliary variables to reduce number of repeated operations
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */
static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */ static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
static bool bFilterInit = false;
//! Auxiliary variables to reduce number of repeated operations
static float q0q0, q0q1, q0q2, q0q3; static float q0q0, q0q1, q0q2, q0q3;
static float q1q1, q1q2, q1q3; static float q1q1, q1q2, q1q3;
static float q2q2, q2q3; static float q2q2, q2q3;
static float q3q3; static float q3q3;
static bool bFilterInit = false;
//! Serial packet related
static int uart;
static int baudrate;
/** /**
* Mainloop of attitude_estimator_so3_comp. * Mainloop of attitude_estimator_so3_comp.
@@ -81,15 +77,18 @@ int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]);
*/ */
static void usage(const char *reason); static void usage(const char *reason);
/* Function prototypes */
float invSqrt(float number);
void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz);
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt);
static void static void
usage(const char *reason) usage(const char *reason)
{ {
if (reason) if (reason)
fprintf(stderr, "%s\n", reason); fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d <devicename>] [-b <baud rate>]\n" fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status}\n");
"-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n"
"ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n");
exit(1); exit(1);
} }
@@ -106,12 +105,10 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
if (argc < 1) if (argc < 1)
usage("missing command"); usage("missing command");
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (thread_running) { if (thread_running) {
printf("attitude_estimator_so3_comp already running\n"); warnx("already running\n");
/* this is not an error */ /* this is not an error */
exit(0); exit(0);
} }
@@ -120,20 +117,20 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp", attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
12400, 14000,
attitude_estimator_so3_comp_thread_main, attitude_estimator_so3_comp_thread_main,
(const char **)argv); (argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0); exit(0);
} }
if (!strcmp(argv[1], "stop")) { if (!strcmp(argv[1], "stop")) {
thread_should_exit = true; thread_should_exit = true;
while(thread_running){ while (thread_running){
usleep(200000); usleep(200000);
printf(".");
} }
printf("terminated.");
warnx("stopped");
exit(0); exit(0);
} }
@@ -157,7 +154,8 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
//--------------------------------------------------------------------------------------------------- //---------------------------------------------------------------------------------------------------
// Fast inverse square-root // Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float number) { float invSqrt(float number)
{
volatile long i; volatile long i;
volatile float x, y; volatile float x, y;
volatile const float f = 1.5F; volatile const float f = 1.5F;
@@ -221,48 +219,47 @@ void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, floa
q3q3 = q3 * q3; q3q3 = q3 * q3;
} }
void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) { void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt)
{
float recipNorm; float recipNorm;
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
//! Make filter converge to initial solution faster // Make filter converge to initial solution faster
//! This function assumes you are in static position. // This function assumes you are in static position.
//! WARNING : in case air reboot, this can cause problem. But this is very // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.
//! unlikely happen. if(bFilterInit == false) {
if(bFilterInit == false)
{
NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz); NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
bFilterInit = true; bFilterInit = true;
} }
//! If magnetometer measurement is available, use it. //! If magnetometer measurement is available, use it.
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
float hx, hy, hz, bx, bz; float hx, hy, hz, bx, bz;
float halfwx, halfwy, halfwz; float halfwx, halfwy, halfwz;
// Normalise magnetometer measurement // Normalise magnetometer measurement
// Will sqrt work better? PX4 system is powerful enough? // Will sqrt work better? PX4 system is powerful enough?
recipNorm = invSqrt(mx * mx + my * my + mz * mz); recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm; mx *= recipNorm;
my *= recipNorm; my *= recipNorm;
mz *= recipNorm; mz *= recipNorm;
// Reference direction of Earth's magnetic field // Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2); hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
bx = sqrt(hx * hx + hy * hy); bx = sqrt(hx * hx + hy * hy);
bz = hz; bz = hz;
// Estimated direction of magnetic field // Estimated direction of magnetic field
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction and measured direction of field vectors // Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += (my * halfwz - mz * halfwy); halfex += (my * halfwz - mz * halfwy);
halfey += (mz * halfwx - mx * halfwz); halfey += (mz * halfwx - mx * halfwz);
halfez += (mx * halfwy - my * halfwx); halfez += (mx * halfwy - my * halfwx);
} }
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
@@ -293,7 +290,9 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki
gyro_bias[1] += twoKi * halfey * dt; gyro_bias[1] += twoKi * halfey * dt;
gyro_bias[2] += twoKi * halfez * dt; gyro_bias[2] += twoKi * halfez * dt;
gx += gyro_bias[0]; // apply integral feedback
// apply integral feedback
gx += gyro_bias[0];
gy += gyro_bias[1]; gy += gyro_bias[1];
gz += gyro_bias[2]; gz += gyro_bias[2];
} }
@@ -337,208 +336,43 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
q3 *= recipNorm; q3 *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic // Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0; q0q0 = q0 * q0;
q0q1 = q0 * q1; q0q1 = q0 * q1;
q0q2 = q0 * q2; q0q2 = q0 * q2;
q0q3 = q0 * q3; q0q3 = q0 * q3;
q1q1 = q1 * q1; q1q1 = q1 * q1;
q1q2 = q1 * q2; q1q2 = q1 * q2;
q1q3 = q1 * q3; q1q3 = q1 * q3;
q2q2 = q2 * q2; q2q2 = q2 * q2;
q2q3 = q2 * q3; q2q3 = q2 * q3;
q3q3 = q3 * q3; q3q3 = q3 * q3;
}
void send_uart_byte(char c)
{
write(uart,&c,1);
}
void send_uart_bytes(uint8_t *data, int length)
{
write(uart,data,(size_t)(sizeof(uint8_t)*length));
}
void send_uart_float(float f) {
uint8_t * b = (uint8_t *) &f;
//! Assume float is 4-bytes
for(int i=0; i<4; i++) {
uint8_t b1 = (b[i] >> 4) & 0x0f;
uint8_t b2 = (b[i] & 0x0f);
uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
send_uart_bytes(&c1,1);
send_uart_bytes(&c2,1);
}
}
void send_uart_float_arr(float *arr, int length)
{
for(int i=0;i<length;++i)
{
send_uart_float(arr[i]);
send_uart_byte(',');
}
}
int open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
{
int speed;
switch (baud) {
case 0: speed = B0; break;
case 50: speed = B50; break;
case 75: speed = B75; break;
case 110: speed = B110; break;
case 134: speed = B134; break;
case 150: speed = B150; break;
case 200: speed = B200; break;
case 300: speed = B300; break;
case 600: speed = B600; break;
case 1200: speed = B1200; break;
case 1800: speed = B1800; break;
case 2400: speed = B2400; break;
case 4800: speed = B4800; break;
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
case 460800: speed = B460800; break;
case 921600: speed = B921600; break;
default:
printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
return -EINVAL;
}
printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud);
uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
*is_usb = false;
/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
} else {
*is_usb = true;
}
return uart;
} }
/* /*
* [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) * Nonliner complementary filter on SO(3), attitude estimator main function.
*/
/*
* EKF Attitude Estimator main function.
* *
* Estimates the attitude recursively once started. * Estimates the attitude once started.
* *
* @param argc number of commandline arguments (plus command name) * @param argc number of commandline arguments (plus command name)
* @param argv strings containing the arguments * @param argv strings containing the arguments
*/ */
int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]) int attitude_estimator_so3_comp_thread_main(int argc, char *argv[])
{ {
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
//! Serial debug related
int ch;
struct termios uart_config_original;
bool usb_uart;
bool debug_mode = false;
char *device_name = "/dev/ttyS2";
baudrate = 115200;
//! Time constant //! Time constant
float dt = 0.005f; float dt = 0.005f;
/* output euler angles */ /* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f}; float euler[3] = {0.0f, 0.0f, 0.0f};
float Rot_matrix[9] = {1.f, 0, 0, /* Initialization */
0, 1.f, 0, float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */
0, 0, 1.f
}; /**< init: identity matrix */
float acc[3] = {0.0f, 0.0f, 0.0f}; float acc[3] = {0.0f, 0.0f, 0.0f};
float gyro[3] = {0.0f, 0.0f, 0.0f}; float gyro[3] = {0.0f, 0.0f, 0.0f};
float mag[3] = {0.0f, 0.0f, 0.0f}; float mag[3] = {0.0f, 0.0f, 0.0f};
/* work around some stupidity in task_create's argv handling */ warnx("main thread started");
argc -= 2;
argv += 2;
//! -d <device_name>, default : /dev/ttyS2
//! -b <baud_rate>, default : 115200
while ((ch = getopt(argc,argv,"d:b:")) != EOF){
switch(ch){
case 'b':
baudrate = strtoul(optarg, NULL, 10);
if(baudrate == 0)
printf("invalid baud rate '%s'",optarg);
break;
case 'd':
device_name = optarg;
debug_mode = true;
break;
default:
usage("invalid argument");
}
}
if(debug_mode){
printf("Opening debugging port for 3D visualization\n");
uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0)
printf("could not open %s", device_name);
else
printf("Open port success\n");
}
// print text
printf("Nonlinear SO3 Attitude Estimator initialized..\n\n");
fflush(stdout);
int overloadcounter = 19;
/* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time();
struct sensor_combined_s raw; struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw)); memset(&raw, 0, sizeof(raw));
@@ -555,8 +389,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to raw data */ /* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* rate-limit raw data updates to 200Hz */ /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
orb_set_interval(sub_raw, 4); orb_set_interval(sub_raw, 3);
/* subscribe to param changes */ /* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update)); int sub_params = orb_subscribe(ORB_ID(parameter_update));
@@ -565,17 +399,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */ /* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); //orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
//orb_advert_t att_pub = -1;
orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0; int loopcounter = 0;
int printcounter = 0; int printcounter = 0;
thread_running = true; thread_running = true;
/* advertise debug value */
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
// orb_advert_t pub_dbg = -1;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs // XXX write this out to perf regs
@@ -588,9 +420,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* initialize parameter handles */ /* initialize parameter handles */
parameters_init(&so3_comp_param_handles); parameters_init(&so3_comp_param_handles);
parameters_update(&so3_comp_param_handles, &so3_comp_params);
uint64_t start_time = hrt_absolute_time(); uint64_t start_time = hrt_absolute_time();
bool initialized = false; bool initialized = false;
bool state_initialized = false;
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
unsigned offset_count = 0; unsigned offset_count = 0;
@@ -615,12 +449,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
if (!control_mode.flag_system_hil_enabled) { if (!control_mode.flag_system_hil_enabled) {
fprintf(stderr, warnx("WARNING: Not getting sensors - sensor app running?");
"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
} }
} else { } else {
/* only update parameters if they changed */ /* only update parameters if they changed */
if (fds[1].revents & POLLIN) { if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */ /* read from param to clear updated flag */
@@ -644,11 +475,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
gyro_offsets[2] += raw.gyro_rad_s[2]; gyro_offsets[2] += raw.gyro_rad_s[2];
offset_count++; offset_count++;
if (hrt_absolute_time() - start_time > 3000000LL) { if (hrt_absolute_time() > start_time + 3000000l) {
initialized = true; initialized = true;
gyro_offsets[0] /= offset_count; gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count; gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count; gyro_offsets[2] /= offset_count;
warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]);
} }
} else { } else {
@@ -668,9 +500,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
sensor_last_timestamp[0] = raw.timestamp; sensor_last_timestamp[0] = raw.timestamp;
} }
gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */ /* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) { if (sensor_last_count[1] != raw.accelerometer_counter) {
@@ -696,31 +528,14 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
mag[1] = raw.magnetometer_ga[1]; mag[1] = raw.magnetometer_ga[1];
mag[2] = raw.magnetometer_ga[2]; mag[2] = raw.magnetometer_ga[2];
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
// if (overloadcounter == 20) {
// printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
// overloadcounter = 0;
// }
overloadcounter++;
}
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */ /* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05f && dt > 0.005f) { if (!state_initialized && dt < 0.05f && dt > 0.001f) {
dt = 0.005f; state_initialized = true;
parameters_update(&so3_comp_param_handles, &so3_comp_params); warnx("state initialized");
const_initialized = true;
} }
/* do not execute the filter if not initialized */ /* do not execute the filter if not initialized */
if (!const_initialized) { if (!state_initialized) {
continue; continue;
} }
@@ -728,18 +543,23 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// NOTE : Accelerometer is reversed. // NOTE : Accelerometer is reversed.
// Because proper mount of PX4 will give you a reversed accelerometer readings. // Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],
-acc[0], -acc[1], -acc[2],
mag[0], mag[1], mag[2],
so3_comp_params.Kp,
so3_comp_params.Ki,
dt);
// Convert q->R, This R converts inertial frame to body frame. // Convert q->R, This R converts inertial frame to body frame.
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13
Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3); // 21 Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1); // 23 Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23
Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2); // 31 Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31
Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1); // 32 Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
//1-2-3 Representation. //1-2-3 Representation.
//Equation (290) //Equation (290)
@@ -747,29 +567,42 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix. // Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll
euler[1] = -asinf(Rot_matrix[2]); //! Pitch euler[1] = -asinf(Rot_matrix[2]); //! Pitch
euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]); //! Yaw euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]); //! Yaw
/* swap values for next iteration, check for fatal inputs */ /* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
/* Do something */ // Publish only finite euler angles
att.roll = euler[0] - so3_comp_params.roll_off;
att.pitch = euler[1] - so3_comp_params.pitch_off;
att.yaw = euler[2] - so3_comp_params.yaw_off;
} else { } else {
/* due to inputs or numerical failure the output is invalid, skip it */ /* due to inputs or numerical failure the output is invalid, skip it */
// Due to inputs or numerical failure the output is invalid
warnx("infinite euler angles, rotation matrix:");
warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]);
warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]);
warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]);
// Don't publish anything
continue; continue;
} }
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data); if (last_data > 0 && raw.timestamp > last_data + 12000) {
warnx("sensor data missed");
}
last_data = raw.timestamp; last_data = raw.timestamp;
/* send out */ /* send out */
att.timestamp = raw.timestamp; att.timestamp = raw.timestamp;
// Quaternion
att.q[0] = q0;
att.q[1] = q1;
att.q[2] = q2;
att.q[3] = q3;
att.q_valid = true;
// XXX Apply the same transformation to the rotation matrix // Euler angle rate. But it needs to be investigated again.
att.roll = euler[0] - so3_comp_params.roll_off;
att.pitch = euler[1] - so3_comp_params.pitch_off;
att.yaw = euler[2] - so3_comp_params.yaw_off;
//! Euler angle rate. But it needs to be investigated again.
/* /*
att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3);
att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3);
@@ -783,53 +616,30 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
att.pitchacc = 0; att.pitchacc = 0;
att.yawacc = 0; att.yawacc = 0;
//! Quaternion
att.q[0] = q0;
att.q[1] = q1;
att.q[2] = q2;
att.q[3] = q3;
att.q_valid = true;
/* TODO: Bias estimation required */ /* TODO: Bias estimation required */
memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
/* copy rotation matrix */ /* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(float)*9); memcpy(&att.R, Rot_matrix, sizeof(float)*9);
att.R_valid = true; att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Publish
// Broadcast if (att_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
} else { } else {
warnx("NaN in roll/pitch/yaw estimate!"); warnx("NaN in roll/pitch/yaw estimate!");
orb_advertise(ORB_ID(vehicle_attitude), &att);
} }
perf_end(so3_comp_loop_perf); perf_end(so3_comp_loop_perf);
//! This will print out debug packet to visualization software
if(debug_mode)
{
float quat[4];
quat[0] = q0;
quat[1] = q1;
quat[2] = q2;
quat[3] = q3;
send_uart_float_arr(quat,4);
send_uart_byte('\n');
}
} }
} }
} }
loopcounter++; loopcounter++;
}// while }
thread_running = false; thread_running = false;
/* Reset the UART flags to original state */
if (!usb_uart)
tcsetattr(uart, TCSANOW, &uart_config_original);
return 0; return 0;
} }
src/modules/mavlink/orb_listener.c
+14 -1
View File
@@ -242,7 +242,7 @@ l_vehicle_attitude(const struct listener *l)
att.rollspeed, att.rollspeed,
att.pitchspeed, att.pitchspeed,
att.yawspeed); att.yawspeed);
/* limit VFR message rate to 10Hz */ /* limit VFR message rate to 10Hz */
hrt_abstime t = hrt_absolute_time(); hrt_abstime t = hrt_absolute_time();
if (t >= last_sent_vfr + 100000) { if (t >= last_sent_vfr + 100000) {
@@ -252,6 +252,19 @@ l_vehicle_attitude(const struct listener *l)
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
} }
/* send quaternion values if it exists */
if(att.q_valid) {
mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
att.q[0],
att.q[1],
att.q[2],
att.q[3],
att.rollspeed,
att.pitchspeed,
att.yawspeed);
}
} }
attitude_counter++; attitude_counter++;