dekf.h
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 #ifndef __DEKF_H__
00033 #define __DEKF_H__
00034 
00035 #include <math.h>
00036 #include <HL_interface.h>
00037 #include <ekf/rtwtypes.h>
00038 #include "uart.h"
00039 
00040 #define DEKF_CORRECTION_SMOOTING_LENGTH 100
00041 #define DEKF_WATCHDOG_TIMEOUT 10  // timeout in [s]
00042 
00043 typedef struct
00044 {
00045   real32_T current_state[HLI_EKF_STATE_SIZE];
00046   real32_T last_state[HLI_EKF_STATE_SIZE];
00047   real32_T acc[3];
00048   real32_T ang_vel[3];
00049   real32_T dt;
00050   real32_T q_tmp[4];
00051   HLI_EKF_STATE state_out;
00052   HLI_EKF_STATE state_in;
00053   PacketInfo * packet_info;
00054   uint64_t last_time;
00055   HLI_EXT_POSITION * pos_ctrl_input;
00056   real32_T ctrl_correction[6];
00057   real32_T ctrl_correction_step[6];
00058   int ctrl_correction_count;
00059   int propagate_state;
00060   unsigned int watchdog;
00061   char initialize_event;
00062 } DekfContext;
00063 
00064 void DEKF_init(DekfContext * self, HLI_EXT_POSITION * pos_ctrl_input);
00065 
00066 void DEKF_sendState(DekfContext * self, int64_t  timestamp);
00067 
00068 void DEKF_step(DekfContext * self, int64_t timestamp);
00069 
00070 char DEKF_getInitializeEvent(DekfContext * self);
00071 
00072 inline void initState(DekfContext * self);
00073 
00074 inline void correctState(DekfContext * self);
00075 
00076 inline void writeControllerOutput(DekfContext * self);
00077 
00078 inline real32_T yawFromQuaternion(const real32_T q[4]);
00079 inline void quaternionMultiplication(const real32_T q1[4], const real32_T q2[4], real32_T q[4]);
00080 
00082 inline int float2Int(float x);
00083 
00085 inline short float2Short(float x);
00086 
00087 
00088 
00089 #endif


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