Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

Test flight has been performed with nonlinear SO(3) attitude estimator.

Browse Source 
Here are few observations:
 - When the system initialized, roll angle is initially reversed.
   As filter converged, it becomes normal.
 - I put a negative sign on roll, yaw. It should naturally has right
   sign, but I do not know why for now. Let me investigate again.
 - Gain : I do not know what gain is good for quadrotor flight.
   Let me take a look Ardupilot gain in the later.

Anyway, you can fly with this attitude estimator.
This commit is contained in:
Hyon Lim (Retina)
2013-05-23 16:12:29 +10:00
parent 364d1a06e3
commit 4bf0505421
2 changed files with 114 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files
nuttx/configs/px4fmu/nsh/defconfig
+3 -3
View File
@@ -248,7 +248,7 @@ CONFIG_SERIAL_TERMIOS=y
CONFIG_SERIAL_CONSOLE_REINIT=y CONFIG_SERIAL_CONSOLE_REINIT=y
CONFIG_STANDARD_SERIAL=y CONFIG_STANDARD_SERIAL=y
CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART1_SERIAL_CONSOLE=n
CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART2_SERIAL_CONSOLE=n
CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n
CONFIG_UART4_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n
@@ -561,7 +561,7 @@ CONFIG_START_MONTH=1
CONFIG_START_DAY=1 CONFIG_START_DAY=1
CONFIG_GREGORIAN_TIME=n CONFIG_GREGORIAN_TIME=n
CONFIG_JULIAN_TIME=n CONFIG_JULIAN_TIME=n
CONFIG_DEV_CONSOLE=y CONFIG_DEV_CONSOLE=n
CONFIG_DEV_LOWCONSOLE=n CONFIG_DEV_LOWCONSOLE=n
CONFIG_MUTEX_TYPES=n CONFIG_MUTEX_TYPES=n
CONFIG_PRIORITY_INHERITANCE=y CONFIG_PRIORITY_INHERITANCE=y
@@ -925,7 +925,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512
# Size of the serial receive/transmit buffers. Default 256. # Size of the serial receive/transmit buffers. Default 256.
# #
CONFIG_CDCACM=y CONFIG_CDCACM=y
CONFIG_CDCACM_CONSOLE=n CONFIG_CDCACM_CONSOLE=y
#CONFIG_CDCACM_EP0MAXPACKET #CONFIG_CDCACM_EP0MAXPACKET
CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN=1
#CONFIG_CDCACM_EPINTIN_FSSIZE #CONFIG_CDCACM_EPINTIN_FSSIZE
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+83 -81
View File
@@ -44,8 +44,9 @@ 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 */
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 integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
static float gravity_vector[3] = {0.0f,0.0f,0.0f}; /** estimated gravity vector */
//! Serial packet related //! Serial packet related
static int uart; static int uart;
@@ -152,30 +153,7 @@ float invSqrt(float number) {
void MahonyAHRSupdate(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 MahonyAHRSupdate(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 q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float hx, hy, bx, bz; float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
float halfex, halfey, halfez;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
//MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az, twoKp, twoKi, dt);
return;
}
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic // Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0; q0q0 = q0 * q0;
@@ -189,38 +167,70 @@ void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az
q2q3 = q2 * q3; q2q3 = q2 * q3;
q3q3 = q3 * q3; q3q3 = q3 * q3;
//! If magnetometer measurement is available, use it.
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
float hx, hy, bx, bz;
float halfwx, halfwy, halfwz;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= 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));
bx = sqrt(hx * hx + hy * hy); bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
// Estimated direction of gravity and magnetic field // Estimated direction of magnetic field
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
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 = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy); halfex += (my * halfwz - mz * halfwy);
halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz); halfey += (mz * halfwx - mx * halfwz);
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx); halfez += (mx * halfwy - my * halfwx);
}
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
float halfvx, halfvy, halfvz;
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity and magnetic field
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += ay * halfvz - az * halfvy;
halfey += az * halfvx - ax * halfvz;
halfez += ax * halfvy - ay * halfvx;
}
// Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
// Compute and apply integral feedback if enabled // Compute and apply integral feedback if enabled
if(twoKi > 0.0f) { if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * dt; // integral error scaled by Ki gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki
integralFBy += twoKi * halfey * dt; gyro_bias[1] += twoKi * halfey * dt;
integralFBz += twoKi * halfez * dt; gyro_bias[2] += twoKi * halfez * dt;
gx += integralFBx; // apply integral feedback gx += gyro_bias[0]; // apply integral feedback
gy += integralFBy; gy += gyro_bias[1];
gz += integralFBz; gz += gyro_bias[2];
} }
else { else {
integralFBx = 0.0f; // prevent integral windup gyro_bias[0] = 0.0f; // prevent integral windup
integralFBy = 0.0f; gyro_bias[1] = 0.0f;
integralFBz = 0.0f; gyro_bias[2] = 0.0f;
} }
// Apply proportional feedback // Apply proportional feedback
@@ -309,11 +319,11 @@ int open_uart(int baud, const char *uart_name, struct termios *uart_config_origi
case 460800: speed = B460800; break; case 460800: speed = B460800; break;
case 921600: speed = B921600; break; case 921600: speed = B921600; break;
default: default:
fprintf(stderr, "ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
return -EINVAL; return -EINVAL;
} }
printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud);
uart = open(uart_name, O_RDWR | O_NOCTTY); uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */ /* Try to set baud rate */
@@ -325,7 +335,7 @@ int open_uart(int baud, const char *uart_name, struct termios *uart_config_origi
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) { if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
/* Back up the original uart configuration to restore it after exit */ /* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
fprintf(stderr, "ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
close(uart); close(uart);
return -1; return -1;
} }
@@ -338,14 +348,14 @@ int open_uart(int baud, const char *uart_name, struct termios *uart_config_origi
/* Set baud rate */ /* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
fprintf(stderr, "ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart); close(uart);
return -1; return -1;
} }
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
fprintf(stderr, "ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
close(uart); close(uart);
return -1; return -1;
} }
@@ -420,9 +430,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
} }
if(debug_mode){ if(debug_mode){
printf("Opening debugging port for 3D visualization\n");
uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart); uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0) if (uart < 0)
printf("could not open %s", device_name); printf("could not open %s", device_name);
else
printf("Open port success\n");
} }
// print text // print text
@@ -638,30 +651,22 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
Rot_matrix[7] = 2.0 * (c * d - a * b); // 32 Rot_matrix[7] = 2.0 * (c * d - a * b); // 32
Rot_matrix[8] = 2*aSq - 1 + 2*dSq; // 33 Rot_matrix[8] = 2*aSq - 1 + 2*dSq; // 33
gravity_vector[0] = 2*(q1*q3-q0*q2);
gravity_vector[1] = 2*(q0*q1+q2*q3);
gravity_vector[2] = aSq - bSq - cSq + dSq;
//euler[0] = atan2f(Rot_matrix[7], Rot_matrix[8]); //euler[0] = atan2f(Rot_matrix[7], Rot_matrix[8]);
//euler[1] = asinf(-Rot_matrix[6]); //euler[1] = -asinf(Rot_matrix[6]);
//euler[2] = atan2f(Rot_matrix[3],Rot_matrix[0]); //euler[2] = atan2f(Rot_matrix[3],Rot_matrix[0]);
/* FIXME : Work around this later... // Euler angle directly from quaternion.
float theta = asinf(-Rot_matrix[6]); // -r_{31} euler[0] = -atan2f(gravity_vector[1], sqrtf(gravity_vector[0]*gravity_vector[0] + gravity_vector[2]*gravity_vector[2])); // roll
euler[1] = theta; // pitch angle euler[1] = atan2f(gravity_vector[0], sqrtf(gravity_vector[1]*gravity_vector[1] + gravity_vector[2]*gravity_vector[2])); // pitch
euler[2] = -atan2f(2*(q1*q2-q0*q3),2*(q0*q0+q1*q1)-1); // yaw
if(fabsf(theta - M_PI_2_F) < 1.0e-3f){
euler[0] = 0.0f;
euler[2] = atan2f(Rot_matrix[5] - Rot_matrix[1], Rot_matrix[2] + Rot_matrix[4] - euler[0]);
} else if (fabsf(theta + M_PI_2_F) < 1.0e-3f) {
euler[0] = 0.0f;
euler[2] = atan2f(Rot_matrix[5] - Rot_matrix[1], Rot_matrix[2] + Rot_matrix[4] - euler[0]);
} else {
euler[0] = atan2f(Rot_matrix[7], Rot_matrix[8]);
euler[2] = atan2f(Rot_matrix[3], Rot_matrix[0]);
}
*/
euler[2] = atan2f(2*(q1*q2-q0*q3),2*(q0*q0+q1*q1)-1);
euler[1]= -asinf(2*(q1*q3+q0*q2));
euler[0] = atan2f(2*(q2*q3-q0*q1),2*(q0*q0+q3*q3)-1);
//euler[2] = atan2(2 * q1*q2 - 2 * q0*q3, 2 * q0*q0 + 2 * q1*q1 - 1); // psi
//euler[1] = -asin(2 * q1*q3 + 2 * q0*q2); // theta
//euler[0] = atan2(2 * q2*q3 - 2 * q0*q1, 2 * q0*q0 + 2 * q3*q3 - 1); // phi
/* 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])) {
@@ -684,19 +689,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
att.yaw = euler[2] - so3_comp_params.yaw_off; att.yaw = euler[2] - so3_comp_params.yaw_off;
/* FIXME : This can be a problem for rate controller. Rate in body or inertial? */ /* FIXME : This can be a problem for rate controller. Rate in body or inertial? */
att.rollspeed = q1; att.rollspeed = gyro[0];
att.pitchspeed = q2; att.pitchspeed = gyro[1];
att.yawspeed = q3; att.yawspeed = gyro[2];
att.rollacc = 0;
att.pitchacc = 0;
att.yawacc = 0;
/* /* TODO: Bias estimation required */
att.rollacc = x_aposteriori[3]; memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
att.pitchacc = x_aposteriori[4];
att.yawacc = x_aposteriori[5];
*/
//att.yawspeed =z_k[2] ;
/* copy offsets */
//memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* copy rotation matrix */ /* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
@@ -712,6 +713,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
perf_end(so3_comp_loop_perf); perf_end(so3_comp_loop_perf);
//! This will print out debug packet to visualization software
if(debug_mode) if(debug_mode)
{ {
float quat[4]; float quat[4];
@@ -722,12 +724,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
send_uart_float_arr(quat,4); send_uart_float_arr(quat,4);
send_uart_byte('\n'); send_uart_byte('\n');
} }
}// else }
} }
} }
loopcounter++; loopcounter++;
} }// while
thread_running = false; thread_running = false;