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

Merge remote branch 'paparazzi/CHIMU' into dev

Browse Source 
resolved conflicts:
	sw/airborne/modules/ins/endian_functions.c
	sw/airborne/modules/ins/endian_functions.h
	sw/airborne/modules/ins/imu_chimu.c
	sw/airborne/modules/ins/imu_chimu.h
This commit is contained in:
Felix Ruess
2011-04-11 22:07:43 +02:00
parent f88a75d620 a04935bee1
commit 2b55a3149f
22 changed files with 1224 additions and 311 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/AirborneCodeReorg/ChimuLisaFw.xml → conf/airframes/CDW/ChimuLisaFw.xml
+5 -17
View File
@@ -16,9 +16,6 @@
<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>
@@ -49,6 +46,10 @@
<modules>
<load name="ins_chimu_uart.xml">
<configure name="CHIMU_UART_NR" value="1"/>
</load>
<load name="light.xml">
<define name="LIGHT_LED_STROBE" value="3"/>
<define name="LIGHT_LED_NAV" value="4"/>
@@ -94,20 +95,7 @@
<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"/>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
</section>
conf/airframes/CDW/ChimuTinyFw.xml
+198
View File
@@ -0,0 +1,198 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
YAPA + XSens + XBee
-->
<airframe name="Yapa v1">
<servos>
<servo name="THROTTLE" no="8" min="1000" neutral="1000" max="2000"/>
<servo name="AILERON_LEFT" no="9" min="1000" neutral="1500" max="2000"/>
<servo name="AILERON_RIGHT" no="4" min="2000" neutral="1500" max="1000"/>
<servo name="ELEVATOR" no="5" min="2000" neutral="1500" max="1000"/>
<servo name="RUDDER" no="3" 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="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="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>
<!-- Differential Aileron Depending on Brake Value -->
<set servo="AILERON_LEFT" value="@ROLL"/>
<set servo="AILERON_RIGHT" value="@ROLL"/>
<!-- Differential Thurst -->
<set servo="THROTTLE" value="@THROTTLE"/>
<!-- Pitch with Brake-Trim Function -->
<set servo="ELEVATOR" value="@PITCH"/>
</command_laws>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
</section>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.7"/>
<define name="MAX_PITCH" value="0.7"/>
</section>
<section name="BAT">
<define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
<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="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"/> -->
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
<define name="GLIDE_AIRSPEED" value="10"/>
<define name="GLIDE_VSPEED" value="3."/>
<define name="GLIDE_PITCH" value="45" unit="deg"/>
</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.03"/>
<!-- outer loop saturation -->
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
<define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
<define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="-1.20000004768"/>
<define name="COURSE_DGAIN" value="0.3"/>
<define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
<define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="PITCH_PGAIN" value="-12000."/>
<define name="PITCH_DGAIN" value="0"/>
<define name="ELEVATOR_OF_ROLL" value="1000."/>
<define name="ROLL_ATTITUDE_GAIN" value="-11500"/>
<define name="ROLL_RATE_GAIN" value="-600."/>
</section>
<section name="AGGRESSIVE" prefix="AGR_">
<define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
<define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
<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="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_">
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
</section>
<modules>
<load name="ins_chimu_uart.xml">
<configure name="CHIMU_UART_NR" value="0"/>
</load>
<load name="gps_i2c.xml" />
</modules>
<firmware name="fixedwing">
<target name="ap" board="tiny_2.11">
<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"/>
<subsystem name="radio_control" type="ppm"/>
<subsystem name="gps" type="ublox_lea5h" >
<configure name="GPS_PORT" value="gps_i2c" />
</subsystem>
<!-- Communication -->
<subsystem name="telemetry" type="xbee_api">
<configure name="MODEM_BAUD" value="B57600"/>
</subsystem>
<!-- Actuators -->
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="navigation"/>
<!-- <subsystem name="gps" type="ublox_lea5h"/> -->
<!--<subsystem name="i2c"/>-->
</firmware>
</airframe>
conf/airframes/CDW/ChimuTinyFwSpi.xml
+192
View File
@@ -0,0 +1,192 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
YAPA + XSens + XBee
-->
<airframe name="Yapa v1">
<servos>
<servo name="THROTTLE" no="8" min="1000" neutral="1000" max="2000"/>
<servo name="AILERON_LEFT" no="9" min="1000" neutral="1500" max="2000"/>
<servo name="AILERON_RIGHT" no="4" min="2000" neutral="1500" max="1000"/>
<servo name="ELEVATOR" no="5" min="2000" neutral="1500" max="1000"/>
<servo name="RUDDER" no="3" 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="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="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>
<!-- Differential Aileron Depending on Brake Value -->
<set servo="AILERON_LEFT" value="@ROLL"/>
<set servo="AILERON_RIGHT" value="@ROLL"/>
<!-- Differential Thurst -->
<set servo="THROTTLE" value="@THROTTLE"/>
<!-- Pitch with Brake-Trim Function -->
<set servo="ELEVATOR" value="@PITCH"/>
</command_laws>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
</section>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.7"/>
<define name="MAX_PITCH" value="0.7"/>
</section>
<section name="BAT">
<define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
<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="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"/> -->
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
<define name="GLIDE_AIRSPEED" value="10"/>
<define name="GLIDE_VSPEED" value="3."/>
<define name="GLIDE_PITCH" value="45" unit="deg"/>
</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.03"/>
<!-- outer loop saturation -->
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
<define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
<define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="-1.20000004768"/>
<define name="COURSE_DGAIN" value="0.3"/>
<define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
<define name="ROLL_MAX_SETPOINT" value="0.9" unit="radians"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="PITCH_PGAIN" value="-12000."/>
<define name="PITCH_DGAIN" value="0"/>
<define name="ELEVATOR_OF_ROLL" value="1000."/>
<define name="ROLL_ATTITUDE_GAIN" value="-11500"/>
<define name="ROLL_RATE_GAIN" value="-600."/>
</section>
<section name="AGGRESSIVE" prefix="AGR_">
<define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
<define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
<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="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_">
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
</section>
<modules>
<load name="ins_chimu_spi.xml" />
</modules>
<firmware name="fixedwing">
<target name="ap" board="tiny_2.11">
<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"/>
<subsystem name="radio_control" type="ppm"/>
<subsystem name="gps" type="ublox_lea5h" />
<!-- Communication -->
<subsystem name="telemetry" type="xbee_api">
<configure name="MODEM_BAUD" value="B57600"/>
</subsystem>
<!-- Actuators -->
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="navigation"/>
<!-- <subsystem name="gps" type="ublox_lea5h"/> -->
<subsystem name="spi_slave_hs"/>
</firmware>
</airframe>
conf/airframes/AirborneCodeReorg/LisaFw.xml → conf/airframes/CDW/LisaFw.xml
View File
conf/airframes/AirborneCodeReorg/TinyFw.xml → conf/airframes/CDW/TinyFw.xml
View File
conf/autopilot/subsystems/fixedwing/spi_slave_hs.makefile
+10
View File
@@ -0,0 +1,10 @@
#generic spi driver
$(TARGET).CFLAGS += -DUSE_SPI
ifeq ($(ARCH), lpc21)
$(TARGET).CFLAGS += -DSSP_VIC_SLOT=9
else ifeq ($(ARCH), stm32)
endif
$(TARGET).srcs += mcu_periph/spi.c $(SRC_ARCH)/mcu_periph/spi_slave_hs_arch.c
conf/modules/gps_i2c.xml
+2
View File
@@ -14,6 +14,8 @@ ap.CFLAGS += -DGPS_CONFIGURE -DGPS_PORT_ID=GPS_PORT_DDC
<event fun="gps_i2cEvent()"/>
<makefile target="ap">
<file name="gps_i2c.c"/>
<define name="GPS_CONFIGURE" />
<define name="GPS_PORT_ID" value="GPS_PORT_DDC" />
</makefile>
</module>
conf/modules/ins_chimu.xml
-48
View File
@@ -1,48 +0,0 @@
# 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/modules/ins_chimu_spi.xml
+20
View File
@@ -0,0 +1,20 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ins">
<!-- <depend conflict="ins" -->
<header>
<file name="ins_module.h"/>
</header>
<init fun="ins_init()"/>
<periodic fun="ins_periodic_task()" freq="60"/>
<event fun="parse_ins_msg()"/>
<makefile>
<define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_module.h\\\""/>
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
<define name="USE_SPI" />
<define name="INS_LINK" value="SpiSlave"/>
<file name="ins_chimu_spi.c"/>
<file name="imu_chimu.c"/>
</makefile>
</module>
conf/modules/ins_chimu_uart.xml
+21
View File
@@ -0,0 +1,21 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ins">
<!-- <depend conflict="ins" -->
<header>
<file name="ins_module.h"/>
</header>
<init fun="ins_init()"/>
<!--<periodic fun="ins_periodic_task()" freq="60"/>-->
<event fun="parse_ins_msg()"/>
<makefile>
<define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_module.h\\\""/>
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
<define name="USE_UART$(CHIMU_UART_NR)"/>
<define name="INS_LINK" value="Uart$(CHIMU_UART_NR)"/>
<define name="UART$(CHIMU_UART_NR)_BAUD" value="B115200"/>
<file name="ins_chimu_uart.c"/>
<file name="imu_chimu.c"/>
</makefile>
</module>
sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c
+194
View File
@@ -0,0 +1,194 @@
/*
* $Id$
*
* 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.
*/
#include "spi_slave_hs_arch.h"
#include "mcu_periph/spi.h"
#include BOARD_CONFIG
#include "interrupt_hw.h"
#include "std.h"
#include "mcu.h"
#include "led.h"
#include "LPC21xx.h"
#include "ssp_hw.h"
#include "pprz_debug.h"
#include "armVIC.h"
/* High Speed SPI Slave Circular Buffer */
uint16_t spi_slave_hs_rx_insert_idx, spi_slave_hs_rx_extract_idx;
uint8_t spi_slave_hs_rx_buffer[SPI_SLAVE_HS_RX_BUFFER_SIZE];
uint8_t spi_slave_hs_tx_insert_idx, spi_slave_hs_tx_extract_idx;
uint8_t spi_slave_hs_tx_buffer[SPI_SLAVE_HS_TX_BUFFER_SIZE];
/* Prototypes */
// void spi_init( void ); // -> declared in spi.h
static void SSP_ISR(void) __attribute__((naked));
/* SSPCR0 settings */
#define SSP_DDS 0x07 << 0 /* data size : 8 bits */
//#define SSP_DDS 0x0F << 0 /* data size : 16 bits */
#define SSP_FRF 0x00 << 4 /* frame format : SPI */
#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */
#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */
#define SSP_SCR 0x00 << 8 /* serial clock rate : divide by 1 */
#define SSPCR0_VAL (SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR )
/* SSPCR1 settings */
#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */
#define SSP_SSE 0x00 << 1 /* SSP enable : enable later when init ready */
#define SSP_MS 0x01 << 2 /* master slave mode : slave */
#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */
#define SSPCR1_VAL (SSP_LBM | SSP_SSE | SSP_MS | SSP_SOD )
/* SSPCPSR settings
* min value as master: 2
* min value as slave: 12
*/
#if (PCLK == 15000000)
#define CPSDVSR 12
#else
#if (PCLK == 30000000)
#define CPSDVSR 24
#else
#if (PCLK == 60000000)
#define CPSDVSR 28
#else
#error unknown PCLK frequency
#endif
#endif
#endif
#define SSP_PINSEL1_SCK (2<<2)
#define SSP_PINSEL1_MISO (2<<4)
#define SSP_PINSEL1_MOSI (2<<6)
#define SSP_PINSEL1_SSEL (2<<8)
#define SSP_Write(X) SSPDR=(X)
#define SSP_Read() SSPDR
#define SSP_Status() SSPSR
void spi_init(void) {
/* setup pins for SSP (SCK, MISO, MOSI) */
PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI | SSP_PINSEL1_SSEL;
/* setup SSP */
// Control Registers
SSPCR0 = SSPCR0_VAL;
SSPCR1 = SSPCR1_VAL;
// Clock Prescale Registers
SSPCPSR = CPSDVSR;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */
VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */
_VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
_VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */
// Enable SPI Slave
SetBit(SSPCR1, SSE);
// Enable Receive interrupt
SetBit(SSPIMSC, RXIM);
}
/*
* SSP Status:
*
* ROVR Read Overrun
* WCOL Write Collision (send new byte during a transfer in progress
* ABRT SSEL inactive before end of transfer
*
*
*/
static void SSP_ISR(void) {
ISR_ENTRY();
//LED_TOGGLE(3);
// If any TX bytes are pending
if (spi_slave_hs_tx_insert_idx != spi_slave_hs_tx_extract_idx)
{
uint8_t ret = spi_slave_hs_tx_buffer[spi_slave_hs_tx_extract_idx];
spi_slave_hs_tx_extract_idx = (spi_slave_hs_tx_extract_idx + 1)%SPI_SLAVE_HS_TX_BUFFER_SIZE;
SSP_Write(ret);
}
else
{
SSP_Write(0x00);
}
//do
{
uint16_t temp;
// calc next insert index & store character
temp = ( spi_slave_hs_rx_insert_idx + 1) % SPI_SLAVE_HS_RX_BUFFER_SIZE;
spi_slave_hs_rx_buffer[ spi_slave_hs_rx_insert_idx] = SSP_Read();
// check for more room in queue
if (temp != spi_slave_hs_rx_extract_idx)
spi_slave_hs_rx_insert_idx = temp; // update insert index
// else overrun
}
// while FIFO not empty
//while (SSPSR & RNE);
/*
// loop until not more interrupt sources
while (((iid = U0IIR) & UIIR_NO_INT) == 0)
while (U0LSR & ULSR_THRE)
{
// check if more data to send
if (uart0_tx_insert_idx != uart0_tx_extract_idx)
{
U0THR = uart0_tx_buffer[uart0_tx_extract_idx];
uart0_tx_extract_idx++;
uart0_tx_extract_idx %= UART0_TX_BUFFER_SIZE;
}
else
{
// no
uart0_tx_running = 0; // clear running flag
break;
}
}
*/
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
ISR_EXIT();
}
sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h
+77
View File
@@ -0,0 +1,77 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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.
*/
#ifndef SPI_SLAVE_HS_ARCH_H
#define SPI_SLAVE_HS_ARCH_H
/*
Highspeed SPI Slave Interface
SS on P0.20
Circular Buffer
*/
#include "std.h"
#define SpiEnable() { \
SetBit(SSPCR1, SSE); \
}
#define SpiDisable() { \
ClearBit(SSPCR1, SSE); \
}
#define SPI_SLAVE_HS_RX_BUFFER_SIZE 256
extern uint16_t spi_slave_hs_rx_insert_idx, spi_slave_hs_rx_extract_idx;
extern uint8_t spi_slave_hs_rx_buffer[SPI_SLAVE_HS_RX_BUFFER_SIZE];
#define SpiSlaveChAvailable() (spi_slave_hs_rx_insert_idx != spi_slave_hs_rx_extract_idx)
#define SpiSlaveGetch() ({\
uint8_t ret = spi_slave_hs_rx_buffer[spi_slave_hs_rx_extract_idx]; \
spi_slave_hs_rx_extract_idx = (spi_slave_hs_rx_extract_idx + 1)%SPI_SLAVE_HS_RX_BUFFER_SIZE; \
ret; \
})
#define SPI_SLAVE_HS_TX_BUFFER_SIZE 64
extern uint8_t spi_slave_hs_tx_insert_idx, spi_slave_hs_tx_extract_idx;
extern uint8_t spi_slave_hs_tx_buffer[SPI_SLAVE_HS_TX_BUFFER_SIZE];
#define SpiSlaveTransmit(data) {\
uint8_t temp = (spi_slave_hs_tx_insert_idx + 1) % SPI_SLAVE_HS_TX_BUFFER_SIZE; \
if (temp != spi_slave_hs_tx_extract_idx) /* there is room left */ \
{ \
spi_slave_hs_tx_buffer[spi_slave_hs_tx_insert_idx] = (uint8_t)data; \
spi_slave_hs_tx_insert_idx = temp; \
} \
}
#endif
sw/airborne/firmwares/fixedwing/main_ap.c
+39 -20
View File
@@ -384,6 +384,32 @@ static void navigation_task( void ) {
*/
static inline void attitude_loop( void ) {
#ifdef USE_GYRO
gyro_update();
#endif
#ifdef USE_INFRARED
infrared_update();
estimator_update_state_infrared();
#endif /* USE_INFRARED */
h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
v_ctl_throttle_slew();
ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
#if defined MCU_SPI_LINK
link_mcu_send();
#elif defined INTER_MCU && defined SINGLE_MCU
/**Directly set the flag indicating to FBW that shared buffer is available*/
inter_mcu_received_ap = TRUE;
#endif
}
void periodic_task_ap( void ) {
static uint8_t _60Hz = 0;
@@ -474,35 +500,18 @@ void periodic_task_ap( void ) {
#error "Only 20 and 60 allowed for CONTROL_RATE"
#endif
#if CONTROL_RATE == 20
if (!_20Hz)
#endif
{
#ifdef USE_GYRO
gyro_update();
#endif
#ifdef USE_INFRARED
infrared_update();
estimator_update_state_infrared();
#endif /* USE_INFRARED */
h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
v_ctl_throttle_slew();
ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
#if defined MCU_SPI_LINK
link_mcu_send();
#elif defined INTER_MCU && defined SINGLE_MCU
/**Directly set the flag indicating to FBW that shared buffer is available*/
inter_mcu_received_ap = TRUE;
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
attitude_loop();
#endif
}
modules_periodic_task();
}
@@ -621,6 +630,16 @@ void event_task_ap( void ) {
}
modules_event_task();
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
if (new_ins_attitude > 0)
{
attitude_loop();
//LED_TOGGLE(3);
new_ins_attitude = 0;
}
#endif
} /* event_task_ap() */
sw/airborne/firmwares/fixedwing/main_fbw.c
+22 -5
View File
@@ -92,14 +92,21 @@ static inline void set_failsafe_mode( void ) {
SetCommands(commands_failsafe);
}
volatile uint8_t fbw_new_actuators = 0;
#ifdef RADIO_CONTROL
static inline void handle_rc_frame( void ) {
fbw_mode = FBW_MODE_OF_PPRZ(radio_control.values[RADIO_MODE]);
if (fbw_mode == FBW_MODE_MANUAL)
{
SetCommandsFromRC(commands, radio_control.values);
fbw_new_actuators = 1;
}
}
#endif
/********** EVENT ************************************************************/
void event_task_fbw( void) {
@@ -129,11 +136,24 @@ void event_task_fbw( void) {
SetApOnlyCommands(ap_state->commands);
}
#endif
fbw_new_actuators = 1;
#ifdef SINGLE_MCU
inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
}
#ifdef ACTUATORS
if (fbw_new_actuators > 0)
{
SetActuatorsFromCommands(commands);
fbw_new_actuators = 0;
}
#endif
#ifdef MCU_SPI_LINK
if (link_mcu_received) {
link_mcu_received = FALSE;
@@ -162,7 +182,9 @@ void periodic_task_fbw( void ) {
#ifdef INTER_MCU
inter_mcu_periodic_task();
if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
{
set_failsafe_mode();
}
#endif
#ifdef DOWNLINK
@@ -173,9 +195,4 @@ void periodic_task_fbw( void ) {
electrical_periodic();
}
#ifdef ACTUATORS
SetActuatorsFromCommands(commands);
#endif
}
sw/airborne/modules/gps_i2c/gps_i2c.c
+19 -13
View File
@@ -20,10 +20,13 @@
*
*/
#include "modules/gps_i2c.h"
#include "gps_i2c.h"
#include "mcu_periph/i2c.h"
#include "subsystems/gps.h"
struct i2c_transaction i2c_gps_trans;
uint8_t gps_i2c_rx_buf[GPS_I2C_BUF_SIZE];
uint8_t gps_i2c_rx_insert_idx, gps_i2c_rx_extract_idx;
uint8_t gps_i2c_tx_buf[GPS_I2C_BUF_SIZE];
@@ -46,11 +49,10 @@ bool_t gps_i2c_done, gps_i2c_data_ready_to_transmit;
}
static uint8_t gps_i2c_status;
static uint16_t gps_i2c_nb_avail_bytes; /* size buffer =~ 12k */
static uint8_t data_buf_len;
//static uint16_t gps_i2c_nb_avail_bytes; /* size buffer =~ 12k */
//static uint8_t data_buf_len;
void
gps_i2c_init(void) {
void gps_i2c_init(void) {
gps_i2c_status = GPS_I2C_STATUS_IDLE;
gps_i2c_done = TRUE;
gps_i2c_data_ready_to_transmit = FALSE;
@@ -63,32 +65,34 @@ gps_i2c_init(void) {
#endif
}
void
gps_i2c_periodic(void) {
void gps_i2c_periodic(void) {
/*
if (gps_i2c_done && gps_i2c_status == GPS_I2C_STATUS_IDLE) {
i2c0_buf[0] = GPS_I2C_ADDR_NB_AVAIL_BYTES;
i2c0_transmit_no_stop(GPS_I2C_SLAVE_ADDR, 1, &gps_i2c_done);
gps_i2c_done = FALSE;
gps_i2c_status = GPS_I2C_STATUS_ASKING_NB_AVAIL_BYTES;
}
*/
}
void
gps_i2c_event(void) {
switch (gps_i2c_status) {
void gps_i2c_event(void) {
/*
* switch (gps_i2c_status) {
case GPS_I2C_STATUS_IDLE:
if (gps_i2c_data_ready_to_transmit) {
/* Copy data from our buffer to the i2c buffer */
// Copy data from our buffer to the i2c buffer
uint8_t data_size = Min(gps_i2c_tx_insert_idx-gps_i2c_tx_extract_idx, I2C0_BUF_LEN);
uint8_t i;
for(i = 0; i < data_size; i++, gps_i2c_tx_extract_idx++)
i2c0_buf[i] = gps_i2c_tx_buf[gps_i2c_tx_extract_idx];
/* Start i2c transmit */
// Start i2c transmit
i2c0_transmit(GPS_I2C_SLAVE_ADDR, data_size, &gps_i2c_done);
gps_i2c_done = FALSE;
/* Reset flag if finished */
// Reset flag if finished
if (gps_i2c_tx_extract_idx >= gps_i2c_tx_insert_idx) {
gps_i2c_data_ready_to_transmit = FALSE;
gps_i2c_tx_insert_idx = 0;
@@ -138,4 +142,6 @@ gps_i2c_event(void) {
default:
return;
}
*/
}
sw/airborne/modules/ins/endian_functions.c
-74
View File
@@ -1,74 +0,0 @@
/**********************************************************************************************
* 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
@@ -1,26 +0,0 @@
/**********************************************************************************************
* 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
+139 -90
View File
@@ -29,7 +29,6 @@
Public Functions:
CHIMU_Init Create component instance
CHIMU_Done Free component instance
CHIMU_Parse Parse the RX byte stream message
Applicable Documents:
@@ -41,14 +40,81 @@
#include "imu_chimu.h"
#include "string.h"
//#include "crc.h"
#include "endian_functions.h"
//#include "util.h"
#include "math.h"
/***************************************************************************
* Endianness Swapping Functions
*/
//---[Defines]------------------------------------------------------
static 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;
}
/***************************************************************************
* Cyclic Redundancy Checksum
*/
static unsigned long UpdateCRC(unsigned long CRC_acc, void *data, unsigned long data_len)
{
unsigned long i; // loop counter
#define POLY 0xEDB88320 // bit-reversed version of the poly 0x04C11DB7
// Create the CRC "dividend" for polynomial arithmetic (binary arithmetic
// with no carries)
unsigned char *CRC_input = (unsigned char*)data;
for (unsigned long j = data_len; j; --j)
{
CRC_acc = CRC_acc ^ *CRC_input++;
// "Divide" the poly into the dividend using CRC XOR subtraction
// CRC_acc holds the "remainder" of each divide
//
// Only complete this division for 8 bits since input is 1 byte
for (i = 0; i < 8; i++)
{
// Check if the MSB is set (if MSB is 1, then the POLY can "divide"
// into the "dividend")
if ((CRC_acc & 0x00000001) == 0x00000001)
{
// if so, shift the CRC value, and XOR "subtract" the poly
CRC_acc = CRC_acc >> 1;
CRC_acc ^= POLY;
}
else
{
// if not, just shift the CRC value
CRC_acc = CRC_acc >> 1;
}
}
}
// Return the final remainder (CRC value)
return CRC_acc;
}
void CHIMU_Checksum(unsigned char *data, unsigned char buflen)
{
data[buflen-1] = (unsigned char) (UpdateCRC(0xFFFFFFFF , data , (unsigned long) (buflen - 1) ) & 0xff) ;
}
/***************************************************************************
* CHIMU Protocol Definition
*/
// Lowlevel Protocol Decoding
#define CHIMU_STATE_MACHINE_START 0
#define CHIMU_STATE_MACHINE_HEADER2 1
#define CHIMU_STATE_MACHINE_LEN 2
@@ -57,9 +123,7 @@
#define CHIMU_STATE_MACHINE_PAYLOAD 5
#define CHIMU_STATE_MACHINE_XSUM 6
//---[DEFINES for Message List]---------------------------------------
//Message ID's that go TO the CHIMU
// Message ID's that go TO the CHIMU
#define MSG00_PING 0x00
#define MSG01_BIAS 0x01
#define MSG02_DACMODE 0x02
@@ -79,8 +143,7 @@
#define MSG10_UARTSETTINGS 0x10
#define MSG11_SERIALNUMBER 0x11
//Output message identifiers from the CHIMU unit
//---[Defines]------------------------------------------------------
// Output message identifiers from the CHIMU unit
#define CHIMU_Msg_0_Ping 0
#define CHIMU_Msg_1_IMU_Raw 1
#define CHIMU_Msg_2_IMU_FP 2
@@ -98,21 +161,9 @@
#define CHIMU_Msg_14_RefVector 14
#define CHIMU_Msg_15_SFCheck 15
//---[COM] defines
#define CHIMU_COM_ID_LOW 0x00
// Communication Definitions
#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
@@ -155,11 +206,9 @@ void CHIMU_Init(CHIMU_PARSER_DATA *pstData)
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(
@@ -168,7 +217,6 @@ unsigned char CHIMU_Parse(
CHIMU_PARSER_DATA *pstData) /* resulting data */
{
//long int i;
char bUpdate = FALSE;
switch (pstData->m_State) {
@@ -215,7 +263,7 @@ unsigned char CHIMU_Parse(
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))
if ( pstData->m_MsgID>CHIMU_COM_ID_HIGH)
{
pstData->m_State = CHIMU_STATE_MACHINE_START;
//BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL);
@@ -230,8 +278,7 @@ unsigned char CHIMU_Parse(
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_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;
@@ -263,6 +310,55 @@ unsigned char CHIMU_Parse(
// appropriate sentence data processor.
///////////////////////////////////////////////////////////////////////////////
static 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 *M_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 * M_PI);
}
return(ps);
}
static unsigned char BitTest (unsigned char input, unsigned char n)
{
//Test a bit in n and return TRUE or FALSE
if ( input & (1 << n)) return TRUE; else return FALSE;
}
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
@@ -278,11 +374,11 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
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++;
pstData->gCHIMU_SW_Exclaim = pPayloadData[CHIMU_index]; CHIMU_index++;
pstData->gCHIMU_SW_Major = pPayloadData[CHIMU_index]; CHIMU_index++;
pstData->gCHIMU_SW_Minor = pPayloadData[CHIMU_index]; CHIMU_index++;
pstData->gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index]<<8) & (0x0000FF00); CHIMU_index++;
pstData->gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++;
return TRUE;
break;
case CHIMU_Msg_1_IMU_Raw:
@@ -319,7 +415,7 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
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));
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]);
@@ -341,14 +437,13 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
pstData->m_attrates.euler.theta = pstData->m_sensor.rate[1];
pstData->m_attrates.euler.psi = pstData->m_sensor.rate[2];
/*
// TODO: Read configuration bits
pstData->gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++;
pstData->gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++;
pstData->gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++;
gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++;
gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++;
// TODO: Read configuration bits
gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++;
bC0_SPI_En = BitTest (gConfigInfo, 0);
/* bC0_SPI_En = BitTest (gConfigInfo, 0);
bC1_HWCentrip_En = BitTest (gConfigInfo, 1);
bC2_TempCal_En = BitTest (gConfigInfo, 2);
bC3_RateOut_En = BitTest (gConfigInfo, 3);
@@ -356,13 +451,12 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
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)
if(BitTest (pstData->gConfigInfo, 5) == 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
@@ -380,8 +474,6 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
{
//TODO: Log BIT that indicates IMU message incoming failed (maybe SPI error?)
}
//Led_Off(LED_RED);
return TRUE;
break;
@@ -402,49 +494,6 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
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);
return FALSE;
}
sw/airborne/modules/ins/imu_chimu.h
+29 -17
View File
@@ -66,41 +66,54 @@ typedef struct {
CHIMU_Quaternion q;
} CHIMU_attitude_data;
#ifndef FALSE
#define FALSE (1==0)
#endif
#ifndef TRUE
#define TRUE (1==1)
#endif
typedef struct {
int cputemp;
int acc[3];
int rate[3];
int mag[3];
int spare1;
int euler[3];
float cputemp;
float acc[3];
float rate[3];
float mag[3];
float spare1;
} 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_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;
<<<<<<< HEAD
unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data
=======
unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data
>>>>>>> paparazzi/CHIMU
unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data
CHIMU_attitude_data m_attitude;
CHIMU_attitude_data m_attrates;
CHIMU_sensor_data m_sensor;
// Ping data
uint8_t gCHIMU_SW_Exclaim;
uint8_t gCHIMU_SW_Major;
uint8_t gCHIMU_SW_Minor;
uint16_t gCHIMU_SW_SerialNumber;
// Config
uint8_t gCalStatus;
uint8_t gCHIMU_BIT;
uint8_t gConfigInfo;
} CHIMU_PARSER_DATA;
/*---------------------------------------------------------------------------
@@ -116,8 +129,7 @@ unsigned char CHIMU_Parse(unsigned char btData, unsigned char bInputType, CHIMU_
unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData);
CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude);
void CHIMU_Checksum(unsigned char *data, unsigned char buflen);
#endif // CHIMU_DEFINED
sw/airborne/modules/ins/ins_chimu_spi.c
+143
View File
@@ -0,0 +1,143 @@
/*
C code to connect a CHIMU using uart
*/
#include <stdbool.h>
// SPI
#include "mcu_periph/spi.h"
#include "mcu_periph/spi_slave_hs_arch.h"
// Output
#include "estimator.h"
// For centripedal corrections
#include "gps.h"
// Telemetry
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#include "mcu_periph/uart.h"
#include "messages.h"
#include "downlink.h"
#include "ins_module.h"
#include "imu_chimu.h"
CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
INS_FORMAT ins_pitch_neutral;
volatile uint8_t new_ins_attitude;
void ins_init( void )
{
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
CHIMU_Checksum(rate,12);
new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
CHIMU_Init(&CHIMU_DATA);
// Quat Filter
for (int i=0;i<7;i++)
{
InsSend1(quaternions[i]);
}
// Wait a bit (SPI send zero)
InsSend1(0);
InsSend1(0);
InsSend1(0);
InsSend1(0);
InsSend1(0);
// 50Hz data: attitude only
for (int i=0;i<12;i++)
{
InsSend1(rate[i]);
}
}
//float tempang = 0;
void parse_ins_msg( void )
{
while (InsLink(ChAvailable()))
{
uint8_t ch = InsLink(Getch());
if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
{
if(CHIMU_DATA.m_MsgID==0x03)
{
new_ins_attitude = 1;
// RunOnceEvery(25, LED_TOGGLE(3) );
// LED_TOGGLE(3);
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
if (CHIMU_DATA.m_attrates.euler.phi > M_PI)
{
CHIMU_DATA.m_attrates.euler.phi -= 2 * M_PI;
}
LED_TOGGLE(3);
/* if (CHIMU_DATA.m_attitude.euler.phi == tempang)
{
LED_ON(3);
}
else
{
LED_OFF(3);
}
tempang = CHIMU_DATA.m_attitude.euler.phi;
*/
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta);
}
else if(CHIMU_DATA.m_MsgID==0x02)
{
RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
}
}
}
}
//Frequency defined in conf *.xml
void ins_periodic_task( void )
{
// Send SW Centripetal Corrections
uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 };
// Fill X-speed
CHIMU_Checksum(centripedal,19);
for (int i=0;i<19;i++)
{
InsSend1(centripedal[i]);
}
// Downlink Send
}
sw/airborne/modules/ins/ins_chimu_uart.c
+107
View File
@@ -0,0 +1,107 @@
/*
C code to connect a CHIMU using uart
*/
#include <stdbool.h>
//#include "modules/ins/ins_chimu_uart.h"
// Output
#include "estimator.h"
// For centripedal corrections
#include "gps.h"
// Telemetry
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#include "mcu_periph/uart.h"
#include "messages.h"
#include "downlink.h"
#include "ins_module.h"
#include "imu_chimu.h"
CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
INS_FORMAT ins_pitch_neutral;
volatile uint8_t new_ins_attitude;
void ins_init( void )
{
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0xab, 0x76 }; // 50Hz attitude only + SPI
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0xab, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
CHIMU_Init(&CHIMU_DATA);
// Quat Filter
for (int i=0;i<7;i++)
{
InsUartSend1(quaternions[i]);
}
// 50Hz
for (int i=0;i<12;i++)
{
InsUartSend1(rate[i]);
}
}
float tempang = 0;
void parse_ins_msg( void )
{
while (InsLink(ChAvailable()))
{
uint8_t ch = InsLink(Getch());
if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
{
if(CHIMU_DATA.m_MsgID==0x03)
{
new_ins_attitude = 1;
// RunOnceEvery(25, LED_TOGGLE(3) );
// LED_TOGGLE(3);
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
if (CHIMU_DATA.m_attitude.euler.phi == tempang)
{
LED_ON(3);
}
else
{
LED_OFF(3);
}
tempang = CHIMU_DATA.m_attitude.euler.phi;
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
//EstimatorSetRate(ins_p,ins_q);
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
}
}
}
}
//Frequency defined in conf *.xml
void ins_periodic_task( void )
{
// Downlink Send
}
sw/airborne/modules/ins/ins_module.h
+7 -1
View File
@@ -62,7 +62,12 @@ extern INS_FORMAT ins_mx;
extern INS_FORMAT ins_my;
extern INS_FORMAT ins_mz;
extern INS_FORMAT ins_roll_neutral;
extern INS_FORMAT ins_pitch_neutral;
extern volatile uint8_t ins_msg_received;
extern volatile uint8_t new_ins_attitude;
extern void ins_init( void );
extern void ins_periodic_task( void );
@@ -79,7 +84,8 @@ void parse_ins_buffer( uint8_t );
#define InsBuffer() InsLink(ChAvailable())
#define ReadInsBuffer() { while (InsLink(ChAvailable())&&!ins_msg_received) parse_ins_buffer(InsLink(Getch())); }
#define InsUartSend1(c) InsLink(Transmit(c))
#define InsSend1(c) InsLink(Transmit(c))
#define InsUartSend1(c) InsSend1(c)
#define InsUartInitParam(_a,_b,_c) InsLink(InitParam(_a,_b,_c))
#define InsUartRunning InsLink(TxRunning)