dekf.c
Go to the documentation of this file.
00001 /*
00002 
00003  Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004  You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006  All rights reserved.
00007 
00008  Redistribution and use in source and binary forms, with or without
00009  modification, are permitted provided that the following conditions are met:
00010  * Redistributions of source code must retain the above copyright
00011  notice, this list of conditions and the following disclaimer.
00012  * Redistributions in binary form must reproduce the above copyright
00013  notice, this list of conditions and the following disclaimer in the
00014  documentation and/or other materials provided with the distribution.
00015  * Neither the name of ETHZ-ASL nor the
00016  names of its contributors may be used to endorse or promote products
00017  derived from this software without specific prior written permission.
00018 
00019  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022  DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030  */
00031 
00032 #include "dekf.h"
00033 #include "LL_HL_comm.h"
00034 
00035 #include <ekf/autogen_ekf_propagation.h>
00036 #include <ekf/autogen_ekf_propagation_initialize.h>
00037 
00038 // conversion from AscTec acceleration values to m/s^2
00039 const real32_T DEKF_ASCTEC_ACC_TO_SI = 9.81e-3;
00040 
00041 // conversion from AscTec turn rates to rad/s
00042 const real32_T DEKF_ASCTEC_OMEGA_TO_SI = 0.015 * M_PI / 180.0;
00043 
00044 void DEKF_init(DekfContext * self, HLI_EXT_POSITION * pos_ctrl_input){
00045   int i;
00046 
00047   // init state vector
00048   for(i=0; i<HLI_EKF_STATE_SIZE; i++){
00049     self->last_state[i] = 0;
00050     self->current_state[i] = 0;
00051   }
00052   self->last_state[6] = 1;     // unit quaternion
00053   self->current_state[6] = 1;  // unit quaternion
00054 
00055   self->last_time = 0;
00056   self->ctrl_correction_count = 0;
00057   self->propagate_state = FALSE;
00058   self->watchdog = 0;
00059 
00060   autogen_ekf_propagation_initialize();
00061 
00062   self->pos_ctrl_input = pos_ctrl_input;
00063 
00064   self->packet_info = registerPacket(HLI_PACKET_ID_EKF_STATE, &self->state_in);
00065 }
00066 
00067 void DEKF_sendState(DekfContext * self, int64_t timestamp)
00068 {
00069   int i = 0;
00070   self->state_out.timestamp = timestamp;
00071 
00072   // TODO: smoothing of data to make Nyquist happy ;-)
00073   // acceleration, angular velocities following the ENU convention (x front, y left, z up)
00074   self->state_out.acc_x = LL_1khz_attitude_data.acc_x;
00075   self->state_out.acc_y = -LL_1khz_attitude_data.acc_y;
00076   self->state_out.acc_z = -LL_1khz_attitude_data.acc_z;
00077   self->state_out.ang_vel_roll = LL_1khz_attitude_data.angvel_roll;
00078   self->state_out.ang_vel_pitch = -LL_1khz_attitude_data.angvel_pitch;
00079   self->state_out.ang_vel_yaw = -LL_1khz_attitude_data.angvel_yaw;
00080 
00081   for (i = 0; i < HLI_EKF_STATE_SIZE; i++)
00082   {
00083     self->state_out.state[i] = self->current_state[i];
00084   }
00085 
00086   writePacket2Ringbuffer(HLI_PACKET_ID_EKF_STATE, (unsigned char*)&self->state_out, sizeof(self->state_out));
00087 }
00088 
00089 void DEKF_step(DekfContext * self, int64_t timestamp)
00090 {
00091   self->watchdog ++;
00092 
00093   int i = 0;
00094 
00095   int64_t idt = timestamp - self->last_time;
00096   self->dt = (real32_T)idt * 1.0e-6;
00097   self->last_time = timestamp;
00098 
00099   // if idt > 100ms or negative, something went really wrong
00100   if(idt > 100000 || idt < 0)
00101     return;
00102 
00103   if(self->watchdog > 1000 * DEKF_WATCHDOG_TIMEOUT){
00104     self->watchdog = 1000 * DEKF_WATCHDOG_TIMEOUT;
00105     self->propagate_state = FALSE;
00106   }
00107 
00108   // bring data to SI units and ENU coordinates
00109   self->acc[0] = ((real32_T)LL_1khz_attitude_data.acc_x) * DEKF_ASCTEC_ACC_TO_SI;
00110   self->acc[1] = -((real32_T)LL_1khz_attitude_data.acc_y) * DEKF_ASCTEC_ACC_TO_SI;
00111   self->acc[2] = -((real32_T)LL_1khz_attitude_data.acc_z) * DEKF_ASCTEC_ACC_TO_SI;
00112 
00113   self->ang_vel[0] = ((real32_T)LL_1khz_attitude_data.angvel_roll) * DEKF_ASCTEC_OMEGA_TO_SI;
00114   self->ang_vel[1] = -((real32_T)LL_1khz_attitude_data.angvel_pitch) * DEKF_ASCTEC_OMEGA_TO_SI;
00115   self->ang_vel[2] = -((real32_T)LL_1khz_attitude_data.angvel_yaw) * DEKF_ASCTEC_OMEGA_TO_SI;
00116 
00117 //  // disable system inputs for testing
00118 //  self->acc[0] = 0;
00119 //  self->acc[1] = 0;
00120 //  self->acc[2] = 9.81;
00121 //  self->ang_vel[0] = 0;
00122 //  self->ang_vel[1] = 0;
00123 //  self->ang_vel[2] = 0;
00124 
00125   if (self->propagate_state == TRUE)
00126   {
00127     autogen_ekf_propagation(self->last_state, self->acc, self->ang_vel, self->dt, self->current_state);
00128   }
00129   else
00130   {
00131     // hold state, set velocity to zero
00132     self->current_state[3] = 0;
00133     self->current_state[4] = 0;
00134     self->current_state[5] = 0;
00135 
00136     //TODO: yaw ?!?
00137   }
00138 
00139 
00140   self->initialize_event = 0;
00141 
00142   if (self->packet_info->updated)
00143   {
00144     self->packet_info->updated = 0;
00145     self->propagate_state = TRUE;
00146     self->watchdog = 0;
00147 
00148     if (self->state_in.flag == HLI_EKF_STATE_STATE_CORRECTION)
00149     {
00150       // correct states
00151       correctState(self);
00152     }
00153     else if (self->state_in.flag == HLI_EKF_STATE_INITIALIZATION)
00154     {
00155       initState(self);
00156       self->ctrl_correction_count = 0;
00157       self->initialize_event = 1;
00158       writeControllerOutput(self);
00159     }
00160   }
00161 
00162   writeControllerOutput(self);
00163 
00164   for (i = 0; i < HLI_EKF_STATE_SIZE; i++)
00165   {
00166     self->last_state[i] = self->current_state[i];
00167   }
00168 
00169 }
00170 
00171 void initState(DekfContext * self)
00172 {
00173   for (int i = 0; i < HLI_EKF_STATE_SIZE; i++)
00174   {
00175     self->current_state[i] = self->state_in.state[i];
00176   }
00177 }
00178 
00179 void correctState(DekfContext * self)
00180 {
00181   // position
00182   self->current_state[0] += self->state_in.state[0];
00183   self->current_state[1] += self->state_in.state[1];
00184   self->current_state[2] += self->state_in.state[2];
00185 
00186   // velocity
00187   self->current_state[3] += self->state_in.state[3];
00188   self->current_state[4] += self->state_in.state[4];
00189   self->current_state[5] += self->state_in.state[5];
00190 
00191   // orientation
00192   self->q_tmp[0] = self->current_state[6];
00193   self->q_tmp[1] = self->current_state[7];
00194   self->q_tmp[2] = self->current_state[8];
00195   self->q_tmp[3] = self->current_state[9];
00196 
00197   quaternionMultiplication(self->q_tmp, &self->state_in.state[6], &self->current_state[6]);
00198 
00199   // gyro bias
00200   self->current_state[10] += self->state_in.state[10];
00201   self->current_state[11] += self->state_in.state[11];
00202   self->current_state[12] += self->state_in.state[12];
00203 
00204   // acceleration bias
00205   self->current_state[13] += self->state_in.state[13];
00206   self->current_state[14] += self->state_in.state[14];
00207   self->current_state[15] += self->state_in.state[15];
00208 
00209   self->ctrl_correction_count = DEKF_CORRECTION_SMOOTING_LENGTH;
00210 
00211   // pos_ctrl is NED !!!
00212   self->ctrl_correction[0] = self->current_state[0] - ((real32_T) self->pos_ctrl_input->x) * 1e-3;
00213   self->ctrl_correction[1] = self->current_state[1] - (-(real32_T) self->pos_ctrl_input->y) * 1e-3;
00214   self->ctrl_correction[2] = self->current_state[2] - (-(real32_T) self->pos_ctrl_input->z) * 1e-3;
00215   self->ctrl_correction[3] = self->current_state[3] - ((real32_T) self->pos_ctrl_input->vX) * 1e-3;
00216   self->ctrl_correction[4] = self->current_state[4] - (-(real32_T) self->pos_ctrl_input->vY) * 1e-3;
00217   self->ctrl_correction[5] = self->current_state[5] - (-(real32_T) self->pos_ctrl_input->vZ) * 1e-3;
00218 
00219   for(int i=0; i<6; i++){
00220     self->ctrl_correction_step[i] = self->ctrl_correction[i] / (real32_T)DEKF_CORRECTION_SMOOTING_LENGTH;
00221   }
00222 //  self->last_correction[i] = self->current_state[i] -
00223 
00224 }
00225 
00226 void writeControllerOutput(DekfContext * self)
00227 {
00228   if (self->ctrl_correction_count > 0)
00229   {
00230     for (int i = 0; i < 6; i++)
00231     {
00232       self->ctrl_correction[i] -= self->ctrl_correction_step[i];
00233     }
00234     self->ctrl_correction_count--;
00235   }
00236   else if (self->ctrl_correction_count == 0)
00237 //  else
00238   {
00239     for (int i = 0; i < 6; i++)
00240     {
00241       self->ctrl_correction[i] = 0;
00242     }
00243     self->ctrl_correction_count--;
00244   }
00245 
00246   self->pos_ctrl_input->bitfield = EXT_POSITION_BYPASS_FILTER;
00247   self->pos_ctrl_input->x = float2Int((self->current_state[0]-self->ctrl_correction[0]) * 1000.0);
00248   self->pos_ctrl_input->y = -float2Int((self->current_state[1]-self->ctrl_correction[1]) * 1000.0);
00249   self->pos_ctrl_input->z = -float2Int((self->current_state[2]-self->ctrl_correction[2]) * 1000.0);
00250   self->pos_ctrl_input->vX = float2Short((self->current_state[3]-self->ctrl_correction[3]) * 1000.0);
00251   self->pos_ctrl_input->vY = -float2Short((self->current_state[4]-self->ctrl_correction[4]) * 1000.0);
00252   self->pos_ctrl_input->vZ = -float2Short((self->current_state[5]-self->ctrl_correction[5]) * 1000.0);
00253 
00254   real32_T * const q = &self->current_state[6];
00255 
00256   real32_T yaw = yawFromQuaternion(q);
00257   self->pos_ctrl_input->heading = 360000 - (int)(((yaw < 0 ? yaw + 2 * M_PI : yaw) * 180.0 / M_PI) * 1000.0);
00258 
00259   //    statusData.debug1 = pos_ctrl_input->heading;
00260   //    statusData.debug2 = yaw*180.0/M_PI*1000;
00261 
00262 //      pos_ctrl_input->count = sdkLoops;
00263   self->pos_ctrl_input->qualX = 100;
00264   self->pos_ctrl_input->qualY = 100;
00265   self->pos_ctrl_input->qualZ = 100;
00266   self->pos_ctrl_input->qualVx = 100;
00267   self->pos_ctrl_input->qualVy = 100;
00268   self->pos_ctrl_input->qualVz = 100;
00269 }
00270 
00271 char DEKF_getInitializeEvent(DekfContext * self)
00272 {
00273   return self->initialize_event;
00274 }
00275 
00276 real32_T yawFromQuaternion(const real32_T q[4])
00277 {
00278   real32_T x = 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]);
00279   real32_T y = 2.0 * (q[3] * q[0] + q[1] * q[2]);
00280   real32_T yaw = 0;
00281   real32_T r = 0;
00282 
00283   //  yaw = atan2(y, x);
00284 
00285   //alternate atan2: http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
00286   real32_T abs_y = fabs(y) + 1e-10; // kludge to prevent 0/0 condition
00287   if (x >= 0)
00288   {
00289     r = (x - abs_y) / (x + abs_y);
00290     yaw = r * (0.1963 * r * r - 0.9817) + M_PI / 4.0;
00291   }
00292   else
00293   {
00294     r = (x + abs_y) / (abs_y - x);
00295     yaw = r * (0.1963 * r * r - 0.9817) + 3.0 * M_PI / 4.0;
00296   }
00297   if (y < 0)
00298     yaw = -yaw; // negate if in quad III or IV;
00299 
00300   return yaw;
00301 }
00302 
00303 void quaternionMultiplication(const real32_T q1[4], const real32_T q2[4], real32_T q[4])
00304 {
00305   q[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
00306   q[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
00307   q[2] = q1[0] * q2[2] + q1[2] * q2[0] - q1[1] * q2[3] + q1[3] * q2[1];
00308   q[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0];
00309 }
00310 
00311 int float2Int(float x)
00312 {
00313   // TODO: range checking?
00314   return x >= 0 ? (int)(x + 0.5) : (int)(x - 0.5);
00315 }
00316 
00317 short float2Short(float x)
00318 {
00319   // TODO: range checking?
00320   return x >= 0 ? (short)(x + 0.5) : (short)(x - 0.5);
00321 }


asctec_hl_firmware
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Tue Jan 7 2014 11:05:19