diff options
3 files changed, 50 insertions, 15 deletions
diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 94d99112e..02e224302 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -248,7 +248,7 @@ CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y CONFIG_STANDARD_SERIAL=y -CONFIG_USART1_SERIAL_CONSOLE=n +CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n @@ -561,7 +561,7 @@ CONFIG_START_MONTH=1 CONFIG_START_DAY=1 CONFIG_GREGORIAN_TIME=n CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=n +CONFIG_DEV_CONSOLE=y CONFIG_DEV_LOWCONSOLE=n CONFIG_MUTEX_TYPES=n CONFIG_PRIORITY_INHERITANCE=y @@ -925,7 +925,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512 # Size of the serial receive/transmit buffers. Default 256. # CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_CONSOLE=n #CONFIG_CDCACM_EP0MAXPACKET CONFIG_CDCACM_EPINTIN=1 #CONFIG_CDCACM_EPINTIN_FSSIZE diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 9bb749caf..3cbc62ea1 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -57,6 +57,7 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ 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 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 bool bFilterInit = false; @@ -170,7 +171,7 @@ float invSqrt(float number) { //! Using accelerometer, sense the gravity vector. //! Using magnetometer, sense yaw. -void MahonyAHRSinit(float ax, float ay, float az, float mx, float my, float mz) +void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz) { float initialRoll, initialPitch; float cosRoll, sinRoll, cosPitch, sinPitch; @@ -218,7 +219,7 @@ void MahonyAHRSinit(float ax, float ay, float az, float mx, float my, float mz) q3q3 = q3 * q3; } -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 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 halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; @@ -228,7 +229,7 @@ void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az //! unlikely happen. if(bFilterInit == false) { - MahonyAHRSinit(ax,ay,az,mx,my,mz); + NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz); bFilterInit = true; } @@ -306,14 +307,25 @@ void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az gz += twoKp * halfez; } - // Integrate rate of change of quaternion + //! Integrate rate of change of quaternion +#if 0 gx *= (0.5f * dt); // pre-multiply common factors gy *= (0.5f * dt); gz *= (0.5f * dt); - q0 +=(-q1 * gx - q2 * gy - q3 * gz); - q1 += (q0 * gx + q2 * gz - q3 * gy); - q2 += (q0 * gy - q1 * gz + q3 * gx); - q3 += (q0 * gz + q1 * gy - q2 * gx); +#endif + + // Time derivative of quaternion. q_dot = 0.5*q\otimes omega. + //! q_k = q_{k-1} + dt*\dot{q} + //! \dot{q} = 0.5*q \otimes P(\omega) + dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz); + dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy); + dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx); + dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx); + + q0 += dt*dq0; + q1 += dt*dq1; + q2 += dt*dq2; + q3 += dt*dq3; // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); @@ -528,8 +540,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); + + //! Initialize attitude vehicle uORB message. struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); + struct vehicle_status_s state; memset(&state, 0, sizeof(state)); @@ -711,7 +726,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // NOTE : Accelerometer is reversed. // Because proper mount of PX4 will give you a reversed accelerometer readings. - MahonyAHRSupdate(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. Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 @@ -752,14 +767,27 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitch = euler[1] - so3_comp_params.pitch_off; att.yaw = euler[2] - so3_comp_params.yaw_off; - /* FIXME : This can be a problem for rate controller. Rate in body or inertial? */ + //! Euler angle rate. But it needs to be investigated again. + /* + 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.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3); + */ att.rollspeed = gyro[0]; att.pitchspeed = gyro[1]; att.yawspeed = gyro[2]; + att.rollacc = 0; att.pitchacc = 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 */ memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c index 1d5e0670a..f962515df 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c @@ -19,8 +19,15 @@ #include "attitude_estimator_so3_comp_params.h" /* This is filter gain for nonlinear SO3 complementary filter */ -PARAM_DEFINE_FLOAT(SO3_COMP_KP, 0.5f); -PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.0f); +/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place. + Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP. + If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which + will compensate gyro bias which depends on temperature and vibration of your vehicle */ +PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time. + //! You can set this gain higher if you want more fast response. + //! But note that higher gain will give you also higher overshoot. +PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change) + //! This gain is depend on your vehicle status. /* offsets in roll, pitch and yaw of sensor plane and body */ PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); |