Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 05:45:59 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

CHIMU

Browse Source
This commit is contained in:
Christophe De Wagter
2011-04-01 20:09:12 +02:00
parent 4115c24c0d
commit 15761b8537
8 changed files with 1033 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/AirborneCodeReorg/ChimuLisaFw.xml
+184
View File
@@ -0,0 +1,184 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- TwinStart Multiplex
Lisa/L board (http://paparazzi.enac.fr/wiki/Lisa)
XBee modem
LEA 5H GPS
robbe R617
-->
<airframe name="TwinStar CHIMU Lisa/L">
<firmware name="fixedwing">
<target name="sim" board="pc"/>
<target name="ap" board="lisa_l_1.0"/>
<subsystem name="radio_control" type="ppm"/>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="control"/>
<subsystem name="attitude" type="chimu">
<configure name="CHIMU_PORT" value="UART1"/>
</subsystem>
<subsystem name="gps" type="ublox_lea5h">
<configure name="GPS_PORT" value="UART3"/>
</subsystem>
<subsystem name="navigation"/>
</firmware>
<firmware name="lisa_l_test_progs">
<target name="tunnel" board="lisa_l_1.0"/>
<target name="test_led" board="lisa_l_1.0"/>
<target name="test_uart" board="lisa_l_1.0"/>
<target name="test_servos" board="lisa_l_1.0"/>
<target name="test_telemetry" board="lisa_l_1.0"/>
<target name="test_baro" board="lisa_l_1.0"/>
<target name="test_imu_b2" board="lisa_l_1.0"/>
<target name="test_imu_b2_2" board="lisa_l_1.0"/>
<target name="test_imu_aspirin" board="lisa_l_1.0"/>
<target name="test_rc_spektrum" board="lisa_l_1.0"/>
<target name="test_rc_ppm" board="lisa_l_1.0"/>
<target name="test_adc" board="lisa_l_1.0"/>
<target name="test_hmc5843" board="lisa_l_1.0"/>
<target name="test_itg3200" board="lisa_l_1.0"/>
<target name="test_adxl345" board="lisa_l_1.0"/>
<target name="test_esc_mkk_simple" board="lisa_l_1.0"/>
<target name="test_esc_asctecv1_simple" board="lisa_l_1.0"/>
<target name="test_actuators_mkk" board="lisa_l_1.0"/>
<target name="test_actuators_asctecv1" board="lisa_l_1.0"/>
</firmware>
<modules>
<load name="light.xml">
<define name="LIGHT_LED_STROBE" value="3"/>
<define name="LIGHT_LED_NAV" value="4"/>
<define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
<define name="NAV_LIGHT_MODE_DEFAULT" value="0"/>
</load>
</modules>
<servos>
<servo name="MOTOR" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="AILEVON_LEFT" no="1" min="1900" neutral="1534" max="1100"/>
<servo name="AILEVON_RIGHT" no="2" min="1100" neutral="1468" max="1900"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
</rc_commands>
<section name="MIXER">
<define name="AILEVON_AILERON_RATE" value="0.45"/>
<define name="AILEVON_ELEVATOR_RATE" value="0.8"/>
</section>
<command_laws>
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
<set servo="MOTOR" value="@THROTTLE"/>
<set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
<set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
</command_laws>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="RadOfDeg(50)"/>
<define name="MAX_PITCH" value="RadOfDeg(35)"/>
</section>
<section name="INFRARED" prefix="IR_">
<define name="ADC_IR1_NEUTRAL" value="2048"/>
<define name="ADC_IR2_NEUTRAL" value="2048"/>
<define name="ADC_TOP_NEUTRAL" value="2048"/>
<define name="LATERAL_CORRECTION" value="-1"/>
<define name="LONGITUDINAL_CORRECTION" value="1"/>
<define name="VERTICAL_CORRECTION" value="1.5"/>
<define name="HORIZ_SENSOR_TILTED" value="1"/>
<define name="IR1_SIGN" value="1"/>
<define name="IR2_SIGN" value="-1"/>
<define name="TOP_SIGN" value="1"/>
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
</section>
<section name="MISC">
<define name="NOMINAL_AIRSPEED" value="15." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/>
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="CONTROL_RATE" value="60" unit="Hz"/>
<define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
</section>
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
<!-- outer loop proportional gain -->
<define name="ALTITUDE_PGAIN" value="-0.06"/> <!-- -0.024 -->
<!-- outer loop saturation -->
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.45"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.35"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.85"/>
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.2" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PGAIN" value="-0.023"/> <!-- -0.012 -->
<define name="AUTO_THROTTLE_IGAIN" value="0.01"/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
<!-- auto pitch inner loop -->
<define name="AUTO_PITCH_PGAIN" value="-0.06"/> <!-- -0.03 -->
<define name="AUTO_PITCH_IGAIN" value="0.0"/>
<define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
<define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
<define name="THROTTLE_SLEW" value="0.1"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="-0.9"/>
<define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="ROLL_PGAIN" value="6600."/>
<define name="AILERON_OF_THROTTLE" value="0.0"/>
<define name="PITCH_PGAIN" value="-5500."/>
<define name="PITCH_DGAIN" value="0.4"/>
<define name="ELEVATOR_OF_ROLL" value="2400"/>
</section>
<section name="NAV">
<define name="NAV_PITCH" value="0."/>
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
</section>
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
<define name="HOME_RADIUS" value="100" unit="m"/>
</section>
<section name="SIMU">
<define name="YAW_RESPONSE_FACTOR" value="0.5"/>
</section>
</airframe>
conf/airframes/TU_Delft/yapa_xsens.xml
+76 -72
View File
@@ -7,85 +7,85 @@
<airframe name="Yapa v1">
<servos>
<servo name="MOTOR" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="AILEVON_LEFT" no="2" min="1130" neutral="1575" max="1880"/>
<servo name="AILEVON_RIGHT" no="6" min="1980" neutral="1465" max="1170"/>
<servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="AILERON_LEFT" no="2" min="1000" neutral="1500" max="2000"/>
<servo name="AILERON_RIGHT" no="6" min="2000" neutral="1500" max="1000"/>
<servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
<servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="HATCH" failsafe_value="0"/>
<axis name="CAM_TILT" failsafe_value="0"/>
<axis name="CAM_PAN" failsafe_value="0"/>
<axis name="BRAKE" failsafe_value="0"/> <!-- both elerons up as butterfly brake ? -->
</commands>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="BRAKE" value="@YAW"/>
</rc_commands>
<section name="MIXER">
<define name="AILEVON_AILERON_RATE" value="0.75"/>
<define name="AILEVON_ELEVATOR_RATE" value="0.75"/>
<section name="SERVO_MIXER_GAINS">
<define name="AILERON_RATE_UP" value="0.50f"/>
<define name="AILERON_RATE_DOWN" value="0.25f"/>
<define name="AILERON_RATE_UP_BRAKE" value="0.5f"/>
<define name="AILERON_RATE_DOWN_BRAKE" value="0.9f"/>
<define name="PITCH_GAIN" value="0.9f"/>
<define name="YAW_THRUST" value="0.0f"/>
<define name="BRAKE_AILEVON" value="-0.68f"/>
<define name="BRAKE_PITCH" value="0.0f"/>
<define name="MAX_BRAKE_RATE" value="150"/>
<define name="LIMIT(X,XL,XH)" value="( ((X)>(XH)) ? (XH) : ( ((X)>(XL)) ? (X) : (XL) ) )"/>
</section>
<command_laws>
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
<set servo="MOTOR" value="@THROTTLE"/>
<set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
<set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
<!-- Brake Rate Limiter -->
<let var="brake_value_nofilt" value="LIMIT(-@BRAKE, 0, MAX_PPRZ)"/>
<let var="test; \
static int16_t _var_brake_value = 0; \
_var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
int verwaarloos_deze_warning_CDW" value="0"/>
<!-- Differential Aileron Depending on Brake Value -->
<let var="aileron_up_rate" value="(AILERON_RATE_UP * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_UP_BRAKE * $brake_value)"/>
<let var="aileron_down_rate" value="(AILERON_RATE_DOWN * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_DOWN_BRAKE * $brake_value)"/>
<let var="aileron_up" value="(@ROLL * (((float)$aileron_up_rate) / ((float)MAX_PPRZ)))"/>
<let var="aileron_down" value="(@ROLL * (((float)$aileron_down_rate) / ((float)MAX_PPRZ)))"/>
<let var="leftturn" value="(@ROLL >= 0? 1 : 0)"/>
<let var="rightturn" value="(1 - $leftturn)"/>
<set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON)"/>
<set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON)"/>
<!-- Differential Thurst -->
<set servo="THROTTLE" value="@THROTTLE"/>
<!-- Pitch with Brake-Trim Function -->
<set servo="ELEVATOR" value="-@PITCH * PITCH_GAIN + BRAKE_PITCH * $brake_value"/>
</command_laws>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.7"/>
<define name="MAX_PITCH" value="0.6"/>
</section>
<section name="INFRARED" prefix="IR_">
<define name="ADC_IR1_NEUTRAL" value="512"/>
<define name="ADC_IR2_NEUTRAL" value="512"/>
<define name="ADC_TOP_NEUTRAL" value="512"/>
<define name="LATERAL_CORRECTION" value="1."/>
<define name="LONGITUDINAL_CORRECTION" value="1."/>
<define name="VERTICAL_CORRECTION" value="1.5"/>
<define name="HORIZ_SENSOR_TILTED" value="1"/>
<define name="IR2_SIGN" value="-1"/>
<define name="TOP_SIGN" value="-1"/>
<define name="ROLL_NEUTRAL_DEFAULT" value="0." unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="deg"/>
<define name="CORRECTION_UP" value="1."/>
<define name="CORRECTION_DOWN" value="1."/>
<define name="CORRECTION_LEFT" value="1."/>
<define name="CORRECTION_RIGHT" value="1."/>
</section>
<section name="GYRO" prefix="GYRO_">
<define name="ADC_ROLL_NEUTRAL" value="500"/>
<define name="DYNAMIC_RANGE" value="300" unit="deg/s"/>
<define name="ADXRS300_RESISTOR_BRIDGE" value="(3.3/(3.3+1.8))"/>
<define name="ADXRS300_SENSITIVITY" value="5" unit="mV/(deg/s)"/>
<define name="ROLL_SCALE" value="3.3*1000./1024./(GYRO_ADXRS300_SENSITIVITY*GYRO_ADXRS300_RESISTOR_BRIDGE)" unit="deg/s/adc_unit"/>
<define name="ROLL_DIRECTION" value="1."/>
<define name="MAX_PITCH" value="0.7"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="2000"/>
<define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
</section>
<section name="MISC">
<define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/>
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="CONTROL_RATE" value="60" unit="Hz"/>
<define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
<!-- <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
@@ -148,18 +148,16 @@
<define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
<define name="DESCENT_NAV_RATIO" value="1.0"/>
</section>
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
</section>
<section name="DATALINK" prefix="DATALINK_">
<define name="DEVICE_TYPE" value="XBEE"/>
<define name="DEVICE_ADDRESS" value="...."/>
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
<define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
<define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
<define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
<define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
</section>
<section name="DIGITAL_CAMERA" prefix="DC_">
@@ -169,22 +167,29 @@
<modules>
<!--
<load name="ins_xsens_MTiG_fixedwing.xml">
<configure name="XSENS_UART_NR" value="0"/>
</load>
<load name="light.xml">
<define name="LIGHT_LED_STROBE" value="3"/>
<define name="LIGHT_LED_NAV" value="4"/>
<define name="LIGHT_LED_NAV" value="2"/>
<define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
<define name="NAV_LIGHT_MODE_DEFAULT" value="0"/>
</load>-->
<load name="digital_cam_i2c.xml"/>
<!-- <load name="digital_cam.xml" >
<define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
</load>
<!-- <load name="digital_cam_i2c.xml"/> -->
<load name="digital_cam.xml" >
<define name="DC_SHUTTER_LED" value="3"/>
</load>
--> </modules>
</modules>
<firmware name="fixedwing">
<target name="ap" board="tiny_2.11">
<define name="AGR_CLIMB"/>
<define name="STRONG_WIND"/>
<define name="WIND_INFO"/>
<define name="WIND_INFO_RET"/>
<define name="LOITER_TRIM"/>
<define name="ALT_KALMAN"/>
</target>
<target name="sim" board="pc"/>
@@ -193,17 +198,16 @@
<!-- Communication -->
<subsystem name="telemetry" type="xbee_api">
<configure name="MODEM_BAUD" value="B9600"/>
<configure name="MODEM_BAUD" value="B57600"/>
</subsystem>
<!-- Actuators -->
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="attitude" type="infrared"/>
<subsystem name="gps" type="ublox_lea4p"/>
<subsystem name="navigation"/>
<subsystem name="navigation" type="extra"/>
<subsystem name="gps" type="xsens"/>
<subsystem name="i2c"/>
<!--<subsystem name="i2c"/>-->
</firmware>
conf/modules/ins_chimu.xml
+48
View File
@@ -0,0 +1,48 @@
# attitude via IR sensors
#
# CHIMU using UART
#
ap.CFLAGS += -DUSE_$(CHIMU_PORT)
ap.CFLAGS += -D$(CHIMU_PORT)_BAUD=B115200
#
# CHIMU Status LED
#
ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
ap.srcs += $(SRC_SUBSYSTEMS)/imu/imu_chimu.c
#
# LPC only has one ADC
#
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DADC_CHANNEL_IR1=$(ADC_IR1) -DUSE_$(ADC_IR1)
ap.CFLAGS += -DADC_CHANNEL_IR2=$(ADC_IR2) -DUSE_$(ADC_IR2)
ap.CFLAGS += -DADC_CHANNEL_IR_TOP=$(ADC_IR_TOP) -DUSE_$(ADC_IR_TOP)
endif
#
# On STM32 let's hardwire infrared sensors to AD1 for now
#
ifeq ($(ARCH), stm32)
ap.CFLAGS += -DUSE_AD1
ap.CFLAGS += -DADC_CHANNEL_IR1=$(ADC_IR1_CHAN) -DUSE_AD1_$(ADC_IR1)
ap.CFLAGS += -DADC_CHANNEL_IR2=$(ADC_IR2_CHAN) -DUSE_AD1_$(ADC_IR2)
ap.CFLAGS += -DADC_CHANNEL_IR_TOP=$(ADC_IR_TOP_CHAN) -DUSE_AD1_$(ADC_IR_TOP)
endif
ap.CFLAGS += -DADC_CHANNEL_IR_NB_SAMPLES=$(ADC_IR_NB_SAMPLES)
$(TARGET).CFLAGS += -DUSE_INFRARED
$(TARGET).srcs += subsystems/sensors/infrared.c
$(TARGET).srcs += subsystems/sensors/infrared_adc.c
sim.srcs += $(SRC_ARCH)/sim_ir.c
jsbsim.srcs += $(SRC_ARCH)/jsbsim_ir.c
conf/radios/R617FS_5ch_neg.xml
+52
View File
@@ -0,0 +1,52 @@
<?xml version="1.0"?>
<!-- $Id$
--
-- (c) 2003 Pascal Brisset, Antoine Drouin
--
-- This file is part of paparazzi.
--
-- paparazzi is free software; you can redistribute it and/or modify
-- it under the terms of the GNU General Public License as published by
-- the Free Software Foundation; either version 2, or (at your option)
-- any later version.
--
-- paparazzi is distributed in the hope that it will be useful,
-- but WITHOUT ANY WARRANTY; without even the implied warranty of
-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-- GNU General Public License for more details.
--
-- You should have received a copy of the GNU General Public License
-- along with paparazzi; see the file COPYING. If not, write to
-- the Free Software Foundation, 59 Temple Place - Suite 330,
-- Boston, MA 02111-1307, USA.
-->
<!--
-- Attributes of root (Radio) tag :
-- name: name of RC
-- min: min width of a pulse to be considered as a data pulse
-- max: max width of a pulse to be considered as a data pulse
-- sync: min width of a pulse to be considered as a synchro pulse
-- min, max and sync are expressed in micro-seconds
-->
<!--
-- Attributes of channel tag :
-- ctl: name of the command on the transmitter - only for displaying
-- no: order in the PPM frame
-- function: logical command
-- averaged: channel filtered through several frames (for discrete commands)
-- min: minimum pulse length (micro-seconds)
-- max: maximum pulse length (micro-seconds)
-- neutral: neutral pulse length (micro-seconds)
Note: a command may be reversed by exchanging min and max values
-->
<!DOCTYPE radio SYSTEM "radio.dtd">
<radio name="FASST TX Module" data_min="700" data_max="2300" sync_min="4000" sync_max="15000" pulse_type="NEGATIVE">
<channel ctl="A" function="ROLL" min="1100" neutral="1500" max="1900" average="0"/> <!-- right stick left/right -->
<channel ctl="B" function="PITCH" min="1100" neutral="1500" max="1900" average="0"/> <!-- left stick up/down -->
<channel ctl="C" function="THROTTLE" min="1100" neutral="1100" max="1900" average="0"/> <!-- right stick up/down -->
<channel ctl="D" function="YAW" min="1900" neutral="1500" max="1100" average="0"/> <!-- left stick left/right-->
<channel ctl="E" function="MODE" min="1100" neutral="1500" max="1900" average="0"/> <!-- left switch -->
</radio>
sw/airborne/modules/ins/endian_functions.c
+74
View File
@@ -0,0 +1,74 @@
/**********************************************************************************************
* Ryan Mechatronics firmware (C) 2007 - All Rights Reserved
* CONFIDENTIAL: NO PART OF THIS CODE MAY BE RELEASED WITHOUT WRITTEN PERMISSION
* ---------------------------------------------------------------------------------------------
*
* Module:
* Endian Functions - Handles various low level endian swap functions
*
***********************************************************************************************/
//-----------------------------------------------------------------------------------
// Includes
//-----------------------------------------------------------------------------------
#include "endian_functions.h"
#include <globals.h>
short int ShortSwap( short int s )
{
unsigned char b1, b2;
b1 = s & 255;
b2 = (s >> 8) & 255;
return (b1 << 8) + b2;
}
short int ShortNoSwap( short int s )
{
return s;
}
int LongSwap (int i)
{
unsigned char b1, b2, b3, b4;
b1 = i & 255;
b2 = ( i >> 8 ) & 255;
b3 = ( i>>16 ) & 255;
b4 = ( i>>24 ) & 255;
return ((int)b1 << 24) + ((int)b2 << 16) + ((int)b3 << 8) + b4;
}
int LongNoSwap( int i )
{
return i;
}
float FloatSwap( float f )
{
union
{
float f;
unsigned char b[4];
} dat1, dat2;
dat1.f = f;
dat2.b[0] = dat1.b[3];
dat2.b[1] = dat1.b[2];
dat2.b[2] = dat1.b[1];
dat2.b[3] = dat1.b[0];
return dat2.f;
}
float FloatNoSwap( float f )
{
return f;
}
sw/airborne/modules/ins/endian_functions.h
+26
View File
@@ -0,0 +1,26 @@
/**********************************************************************************************
* Ryan Mechatronics firmware (C) 2007 - All Rights Reserved
* CONFIDENTIAL: NO PART OF THIS CODE MAY BE RELEASED WITHOUT WRITTEN PERMISSION
* ---------------------------------------------------------------------------------------------
*
* Module:
* Endian Functions - Handles various low level endian swap functions
*
***********************************************************************************************/
#ifndef ENDIAN_H
#define ENDIAN_H
short int ShortSwap( short int s );
short int ShortNoSwap( short int s );
int LongSwap (int i);
int LongNoSwap( int i );
float FloatSwap( float f );
float FloatNoSwap( float f );
#endif
sw/airborne/modules/ins/imu_chimu.c
+450
View File
@@ -0,0 +1,450 @@
/*
* Copyright (C) 2011 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/*---------------------------------------------------------------------------
Copyright (c) Ryan Mechatronics 2008. All Rights Reserved.
File: *.c
Description: CHIMU Protocol Parser
Public Functions:
CHIMU_Init Create component instance
CHIMU_Done Free component instance
CHIMU_Parse Parse the RX byte stream message
Applicable Documents:
CHIMU User Manual
Adapted to paparazzi by C. De Wagter
---------------------------------------------------------------------------*/
#include "imu_chimu.h"
#include "string.h"
//#include "crc.h"
#include "endian_functions.h"
//#include "util.h"
#include "math.h"
//---[Defines]------------------------------------------------------
#define CHIMU_STATE_MACHINE_START 0
#define CHIMU_STATE_MACHINE_HEADER2 1
#define CHIMU_STATE_MACHINE_LEN 2
#define CHIMU_STATE_MACHINE_DEVICE 3
#define CHIMU_STATE_MACHINE_ID 4
#define CHIMU_STATE_MACHINE_PAYLOAD 5
#define CHIMU_STATE_MACHINE_XSUM 6
//---[DEFINES for Message List]---------------------------------------
//Message ID's that go TO the CHIMU
#define MSG00_PING 0x00
#define MSG01_BIAS 0x01
#define MSG02_DACMODE 0x02
#define MSG03_CALACC 0x03
#define MSG04_CALMAG 0x04
#define MSG05_CALRATE 0x05
#define MSG06_CONFIGCLR 0x06
#define MSG07_CONFIGSET 0x07
#define MSG08_SAVEGYROBIAS 0x08
#define MSG09_ESTIMATOR 0x09
#define MSG0A_SFCHECK 0x0A
#define MSG0B_CENTRIP 0x0B
#define MSG0C_INITGYROS 0x0C
#define MSG0D_DEVICEID 0x0D
#define MSG0E_REFVECTOR 0x0E
#define MSG0F_RESET 0x0F
#define MSG10_UARTSETTINGS 0x10
#define MSG11_SERIALNUMBER 0x11
//Output message identifiers from the CHIMU unit
//---[Defines]------------------------------------------------------
#define CHIMU_Msg_0_Ping 0
#define CHIMU_Msg_1_IMU_Raw 1
#define CHIMU_Msg_2_IMU_FP 2
#define CHIMU_Msg_3_IMU_Attitude 3
#define CHIMU_Msg_4_BiasSF 4
#define CHIMU_Msg_5_BIT 5
#define CHIMU_Msg_6_MagCal 6
#define CHIMU_Msg_7_GyroBias 7
#define CHIMU_Msg_8_TempCal 8
#define CHIMU_Msg_9_DAC_Offsets 9
#define CHIMU_Msg_10_Res 10
#define CHIMU_Msg_11_Res 11
#define CHIMU_Msg_12_Res 12
#define CHIMU_Msg_13_Res 13
#define CHIMU_Msg_14_RefVector 14
#define CHIMU_Msg_15_SFCheck 15
//---[COM] defines
#define CHIMU_COM_ID_LOW 0x00
#define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above
#define NP_MAX_CMD_LEN 8 // maximum command length (CHIMU address)
#define NP_MAX_DATA_LEN 256 // maximum data length
#define NP_MAX_CHAN 36 // maximum number of channels
#define NP_WAYPOINT_ID_LEN 32 // waypoint max string len
#define NP_XSUM_LEN 3 // chars in checksum string
#define CHIMU_STANDARD 0x00
/*---------------------------------------------------------------------------
Name: CHIMU_Init
---------------------------------------------------------------------------*/
void CHIMU_Init(CHIMU_PARSER_DATA *pstData)
{
unsigned char i;
pstData->m_State = CHIMU_STATE_MACHINE_START;
pstData->m_Checksum = 0x00;
pstData->m_ReceivedChecksum = 0x00;
pstData->m_Index = 0;
pstData->m_PayloadIndex = 0;
//Sensor data holder
pstData->m_sensor.cputemp=0.0;
for (i=0;i<3;i++)
{
pstData->m_sensor.acc[i]=0.0;
pstData->m_sensor.rate[i]=0.0;
pstData->m_sensor.mag[i]=0.0;
}
pstData->m_sensor.spare1=0.0;
//Attitude data
pstData->m_attitude.euler.phi = 0.0;
pstData->m_attitude.euler.theta = 0.0;
pstData->m_attitude.euler.psi = 0.0;
//Attitude rate data
pstData->m_attrates.euler.phi = 0.0;
pstData->m_attrates.euler.theta = 0.0;
pstData->m_attrates.euler.psi = 0.0;
for (i=0; i<CHIMU_RX_BUFFERSIZE; i++)
{
pstData->m_Payload[i]= 0x00;
pstData->m_FullMessage[i]= 0x00;
}
pstData->m_MsgLen = 0;
pstData->m_MsgID = 0;
pstData->m_TempDeviceID =0;
pstData->m_DeviceID = 0x01; //look at this later
}
/*---------------------------------------------------------------------------
Name: CHIMU_Parse
Abstract: Parse message, returns TRUE if new data.
Note: A typical sentence is constructed as:
---------------------------------------------------------------------------*/
unsigned char CHIMU_Parse(
unsigned char btData, /* input byte stream buffer */
unsigned char bInputType, /* for future use if special builds of CHIMU data are performed */
CHIMU_PARSER_DATA *pstData) /* resulting data */
{
//long int i;
char bUpdate = FALSE;
switch (pstData->m_State) {
case CHIMU_STATE_MACHINE_START: // Waiting for start character 0xAE
if(btData == 0xAE)
{
pstData->m_State = CHIMU_STATE_MACHINE_HEADER2;
pstData->m_Index = 0;
pstData->m_FullMessage[pstData->m_Index++] = btData;
} else {
;;
}
bUpdate = FALSE;
break;
case CHIMU_STATE_MACHINE_HEADER2: // Waiting for second header character 0xAE
if(btData == 0xAE)
{
pstData->m_State = CHIMU_STATE_MACHINE_LEN;
pstData->m_FullMessage[pstData->m_Index++]=btData;
} else {
pstData->m_State = CHIMU_STATE_MACHINE_START;
} //Fail to see header. Restart.
break;
case CHIMU_STATE_MACHINE_LEN: // Get chars to read
if( btData <= CHIMU_RX_BUFFERSIZE)
{
pstData->m_MsgLen = btData ; // It might be invalid, but we do a check on buffer size
pstData->m_FullMessage[pstData->m_Index++]=btData;
pstData->m_State = CHIMU_STATE_MACHINE_DEVICE;
} else {
pstData->m_State = CHIMU_STATE_MACHINE_START; //Length byte exceeds buffer. Signal a fail and restart
//BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL);
}
break;
case CHIMU_STATE_MACHINE_DEVICE: // Get device. If not us, ignore and move on. Allows common com with Monkey / Chipmunk
if( (btData == pstData->m_DeviceID) || (btData == 0xAA))
{ //0xAA is global message
pstData->m_TempDeviceID = btData;
pstData->m_FullMessage[pstData->m_Index++]=btData;
pstData->m_State = CHIMU_STATE_MACHINE_ID;
} else {
pstData->m_State = CHIMU_STATE_MACHINE_START;
} //Fail to see correct device ID. Restart.
break;
case CHIMU_STATE_MACHINE_ID: // Get ID
pstData->m_MsgID = btData; // might be invalid, chgeck it out here:
if ( (pstData->m_MsgID<CHIMU_COM_ID_LOW) || (pstData->m_MsgID>CHIMU_COM_ID_HIGH))
{
pstData->m_State = CHIMU_STATE_MACHINE_START;
//BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL);
} else {
pstData->m_FullMessage[pstData->m_Index++]=btData;
pstData->m_PayloadIndex = 0;
pstData->m_State = CHIMU_STATE_MACHINE_PAYLOAD; //Finally....Good to go...
}
break;
case CHIMU_STATE_MACHINE_PAYLOAD: // Waiting for number of bytes in payload
pstData->m_Payload[pstData->m_PayloadIndex++]= btData;
pstData->m_FullMessage[pstData->m_Index++]=btData;
if ((pstData->m_Index) >= (pstData->m_MsgLen + 5)) //Now we have the payload. Verify XSUM and then parse it next
{
// TODO Redo Checksum
// pstData->m_Checksum = (unsigned char) ((UpdateCRC(0xFFFFFFFF , pstData->m_FullMessage , (unsigned long) (pstData->m_MsgLen)+5)) & 0xFF);
pstData->m_State = CHIMU_STATE_MACHINE_XSUM;
} else {
return FALSE;
}
break;
case CHIMU_STATE_MACHINE_XSUM: // Verify
pstData->m_ReceivedChecksum = btData;
pstData->m_FullMessage[pstData->m_Index++]=btData;
if (pstData->m_Checksum!=pstData->m_ReceivedChecksum)
{
bUpdate = FALSE;
//BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL);
} else {
//Xsum passed, go parse it.
// We have pstData->m_MsgID to parse off of, pstData->m_pstData->m_Payload as the data.
bUpdate = CHIMU_ProcessMessage(&pstData->m_MsgID, pstData->m_Payload, pstData);
}
pstData->m_State = CHIMU_STATE_MACHINE_START;
break;
default:
pstData->m_State = CHIMU_STATE_MACHINE_START;
} /* End of SWITCH */
return (bUpdate);
}
///////////////////////////////////////////////////////////////////////////////
// Process CHIMU sentence - Use the CHIMU address (*pCommand) and call the
// appropriate sentence data processor.
///////////////////////////////////////////////////////////////////////////////
unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData)
{
//Msgs from CHIMU are off limits (i.e.any CHIMU messages sent up the uplink should go to
//CHIMU).
//Any CHIMU messages coming from the ground should be ignored, as that byte stream goes up to CHIMU
// by itself. However, here we should decode CHIMU messages being received and
// a) pass them down to ground
// b) grab the data from the CHIMU for our own needs / purposes
int CHIMU_index =0;
float sanity_check=0.0;
switch (pstData->m_MsgID){
case CHIMU_Msg_0_Ping:
CHIMU_index = 0;
gCHIMU_SW_Exclaim = pPayloadData[CHIMU_index]; CHIMU_index++;
gCHIMU_SW_Major = pPayloadData[CHIMU_index]; CHIMU_index++;
gCHIMU_SW_Minor = pPayloadData[CHIMU_index]; CHIMU_index++;
gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index]<<8) & (0x0000FF00); CHIMU_index++;
gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++;
return TRUE;
break;
case CHIMU_Msg_1_IMU_Raw:
break;
case CHIMU_Msg_2_IMU_FP:
CHIMU_index = 0;
memmove (&pstData->m_sensor.cputemp, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.cputemp));CHIMU_index += (sizeof(pstData->m_sensor.cputemp));
pstData->m_sensor.cputemp = FloatSwap(pstData->m_sensor.cputemp);
memmove (&pstData->m_sensor.acc[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.acc));CHIMU_index += (sizeof(pstData->m_sensor.acc));
pstData->m_sensor.acc[0] = FloatSwap(pstData->m_sensor.acc[0]);
pstData->m_sensor.acc[1] = FloatSwap(pstData->m_sensor.acc[1]);
pstData->m_sensor.acc[2] = FloatSwap(pstData->m_sensor.acc[2]);
memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate));CHIMU_index += (sizeof(pstData->m_sensor.rate));
pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]);
pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]);
pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]);
memmove (&pstData->m_sensor.mag[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.mag));CHIMU_index += (sizeof(pstData->m_sensor.mag));
pstData->m_sensor.mag[0] = FloatSwap(pstData->m_sensor.mag[0]);
pstData->m_sensor.mag[1] = FloatSwap(pstData->m_sensor.mag[1]);
pstData->m_sensor.mag[2] = FloatSwap(pstData->m_sensor.mag[2]);
memmove (&pstData->m_sensor.spare1, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.spare1));CHIMU_index += (sizeof(pstData->m_sensor.spare1));
pstData->m_sensor.spare1 = FloatSwap(pstData->m_sensor.spare1);
return TRUE;
break;
case CHIMU_Msg_3_IMU_Attitude:
//Attitude message data from CHIMU
// Includes attitude and rates only, along with configuration bits
// All you need for control
//Led_On(LED_RED);
CHIMU_index = 0;
memmove (&pstData->m_attitude.euler, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.euler));CHIMU_index += sizeof(pstData->m_attitude.euler);
pstData->m_attitude.euler.phi = FloatSwap(pstData->m_attitude.euler.phi);
pstData->m_attitude.euler.theta = FloatSwap(pstData->m_attitude.euler.theta);
pstData->m_attitude.euler.psi = FloatSwap(pstData->m_attitude.euler.psi);
memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate));CHIMU_index += (sizeof(pstData->m_sensor.rate));
pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]);
pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]);
pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]);
memmove (&pstData->m_attitude.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.q));CHIMU_index += sizeof(pstData->m_attitude.q);
pstData->m_attitude.q.s = FloatSwap(pstData->m_attitude.q.s);
pstData->m_attitude.q.v.x = FloatSwap(pstData->m_attitude.q.v.x);
pstData->m_attitude.q.v.y = FloatSwap(pstData->m_attitude.q.v.y);
pstData->m_attitude.q.v.z = FloatSwap(pstData->m_attitude.q.v.z);
memmove (&pstData->m_attrates.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attrates.q));CHIMU_index += sizeof(pstData->m_attitude.q);
pstData->m_attrates.q.s = FloatSwap(pstData->m_attrates.q.s);
pstData->m_attrates.q.v.x = FloatSwap(pstData->m_attrates.q.v.x);
pstData->m_attrates.q.v.y = FloatSwap(pstData->m_attrates.q.v.y);
pstData->m_attrates.q.v.z = FloatSwap(pstData->m_attrates.q.v.z);
//Now put the rates into the Euler section as well. User can use pstData->m_attitude and pstData->m_attrates structures for control
pstData->m_attrates.euler.phi = pstData->m_sensor.rate[0];
pstData->m_attrates.euler.theta = pstData->m_sensor.rate[1];
pstData->m_attrates.euler.psi = pstData->m_sensor.rate[2];
/*
// TODO: Read configuration bits
gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++;
gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++;
gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++;
bC0_SPI_En = BitTest (gConfigInfo, 0);
bC1_HWCentrip_En = BitTest (gConfigInfo, 1);
bC2_TempCal_En = BitTest (gConfigInfo, 2);
bC3_RateOut_En = BitTest (gConfigInfo, 3);
bC4_TBD = BitTest (gConfigInfo, 4);
bC5_Quat_Est = BitTest (gConfigInfo, 5);
bC6_SWCentrip_En = BitTest (gConfigInfo, 6);
bC7_AllowHW_Override = BitTest (gConfigInfo, 7);
//CHIMU currently (v 1.3) does not compute Eulers if quaternion estimator is selected
if(bC5_Quat_Est == TRUE)
{
pstData->m_attitude = GetEulersFromQuat((pstData->m_attitude));
}
*/
//NEW: Checks for bad attitude data (bad SPI maybe?)
// Only allow globals to contain updated data if it makes sense
sanity_check = (pstData->m_attitude.q.s * pstData->m_attitude.q.s);
sanity_check += (pstData->m_attitude.q.v.x * pstData->m_attitude.q.v.x);
sanity_check += (pstData->m_attitude.q.v.y * pstData->m_attitude.q.v.y);
sanity_check += (pstData->m_attitude.q.v.z * pstData->m_attitude.q.v.z);
if ((sanity_check > 0.8) && (sanity_check < 1.2)) //Should be 1.0 (normalized quaternion)
{
// gAttitude = pstData->m_attitude;
// gAttRates = pstData->m_attrates;
// gSensor = pstData->m_sensor;
} else
{
//TODO: Log BIT that indicates IMU message incoming failed (maybe SPI error?)
}
//Led_Off(LED_RED);
return TRUE;
break;
case CHIMU_Msg_4_BiasSF:
case CHIMU_Msg_5_BIT:
case CHIMU_Msg_6_MagCal:
case CHIMU_Msg_7_GyroBias:
case CHIMU_Msg_8_TempCal:
case CHIMU_Msg_9_DAC_Offsets:
case CHIMU_Msg_10_Res:
case CHIMU_Msg_11_Res:
case CHIMU_Msg_12_Res:
case CHIMU_Msg_13_Res:
case CHIMU_Msg_14_RefVector:
case CHIMU_Msg_15_SFCheck:
break;
default:
return FALSE;
break;
}
}
CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude)
{
CHIMU_attitude_data ps;
ps = attitude;
float x, sqw,sqx,sqy,sqz,norm;
sqw = ps.q.s * ps.q.s;
sqx = ps.q.v.x * ps.q.v.x;
sqy = ps.q.v.y * ps.q.v.y;
sqz = ps.q.v.z * ps.q.v.z;
norm = sqrt(sqw + sqx + sqy + sqz);
//Normalize the quat
ps.q.s = ps.q.s / norm;
ps.q.v.x = ps.q.v.x / norm;
ps.q.v.y = ps.q.v.y / norm;
ps.q.v.z = ps.q.v.z / norm;
ps.euler.phi =atan2(2.0 * (ps.q.s * ps.q.v.x + ps.q.v.y * ps.q.v.z), (1 - 2 * (sqx + sqy)));
if (ps.euler.phi < 0) ps.euler.phi = ps.euler.phi + 2 *PI;
x = ((2.0 * (ps.q.s * ps.q.v.y - ps.q.v.z * ps.q.v.x)));
//Below needed in event normalization not done
if (x > 1.0) x = 1.0;
if (x < -1.0) x = -1.0;
//
if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == 0.5)
{
ps.euler.theta = 2 *atan2(ps.q.v.x, ps.q.s);
}
else
if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == -0.5)
{
ps.euler.theta = -2 *atan2(ps.q.v.x, ps.q.s);
}
else{
ps.euler.theta = asin(x);
}
ps.euler.psi = atan2(2.0 * (ps.q.s * ps.q.v.z + ps.q.v.x * ps.q.v.y), (1 - 2 * (sqy + sqz)));
if (ps.euler.psi < 0)
{
ps.euler.psi = ps.euler.psi + (2 * PI);
}
return(ps);
}
sw/airborne/modules/ins/imu_chimu.h
+123
View File
@@ -0,0 +1,123 @@
/*
* Copyright (C) 2011 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/*---------------------------------------------------------------------------
Copyright (c) Ryan Mechatronics 2008. All Rights Reserved.
File: *.c
Description: CHIMU Protocol Parser
Public Functions:
CHIMU_Init Create component instance
CHIMU_Done Free component instance
CHIMU_Parse Parse the RX byte stream message
Applicable Documents:
CHIMU parsing documentation
---------------------------------------------------------------------------*/
#include "paparazzi.h"
//---[Defines]------------------------------------------------------
#ifndef CHIMU_DEFINED_H
#define CHIMU_DEFINED_H
typedef struct {
float phi;
float theta;
float psi;
} CHIMU_Euler;
typedef struct {
float x;
float y;
float z;
} CHIMU_Vector;
typedef struct {
float s;
CHIMU_Vector v;
} CHIMU_Quaternion;
typedef struct {
CHIMU_Euler euler;
CHIMU_Quaternion q;
} CHIMU_attitude_data;
#define FALSE (1==0)
#define TRUE (1==1)
typedef struct {
int cputemp;
int acc[3];
int rate[3];
int mag[3];
int spare1;
int euler[3];
} CHIMU_sensor_data;
extern uint8_t gCHIMU_SW_Exclaim;
extern char gCHIMU_SW_Major;
extern char gCHIMU_SW_Minor;
extern uint16_t gCHIMU_SW_SerialNumber;
#define CHIMU_RX_BUFFERSIZE 128
typedef struct {
unsigned char m_State; // Current state protocol parser is in
unsigned char m_Checksum; // Calculated CHIMU sentence checksum
unsigned char m_ReceivedChecksum; // Received CHIMU sentence checksum (if exists)
unsigned char m_Index; // Index used for command and data
unsigned char m_PayloadIndex;
unsigned char m_MsgID;
unsigned char m_MsgLen;
unsigned char m_TempDeviceID;
unsigned char m_DeviceID;
unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data
unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data
CHIMU_attitude_data m_attitude;
CHIMU_attitude_data m_attrates;
CHIMU_sensor_data m_sensor;
} CHIMU_PARSER_DATA;
/*---------------------------------------------------------------------------
Name: CHIMU_Init
---------------------------------------------------------------------------*/
void CHIMU_Init(CHIMU_PARSER_DATA *pstData);
/*---------------------------------------------------------------------------
Name: CHIMU_Parse
Abstract: Parse message input test mode, returns TRUE if new data.
---------------------------------------------------------------------------*/
unsigned char CHIMU_Parse(unsigned char btData, unsigned char bInputType, CHIMU_PARSER_DATA *pstData);
unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData);
CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude);
#endif // CHIMU_DEFINED