[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6169] Adapt ArduIMU to new i2c, fix gps.
|
From: |
Martin Mueller |
|
Subject: |
[paparazzi-commits] [6169] Adapt ArduIMU to new i2c, fix gps. |
|
Date: |
Mon, 18 Oct 2010 22:36:14 +0000 |
Revision: 6169
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6169
Author: mmm
Date: 2010-10-18 22:36:14 +0000 (Mon, 18 Oct 2010)
Log Message:
-----------
Adapt ArduIMU to new i2c, fix gps.
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/gps.h
paparazzi3/trunk/sw/airborne/gps_ubx.c
Added Paths:
-----------
paparazzi3/trunk/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
paparazzi3/trunk/conf/modules/ins_arduimu.xml
paparazzi3/trunk/conf/settings/tuning_ins.xml
paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c
paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.h
Removed Paths:
-------------
paparazzi3/trunk/conf/modules/ArduIMU.xml
paparazzi3/trunk/sw/airborne/modules/ins/ArduIMU.c
paparazzi3/trunk/sw/airborne/modules/ins/ArduIMU.h
Added: paparazzi3/trunk/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
(rev 0)
+++ paparazzi3/trunk/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
2010-10-18 22:36:14 UTC (rev 6169)
@@ -0,0 +1,207 @@
+<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
+
+<!-- Funjet Multiplex (http://www.multiplex-rc.de/), Jeti ECO 25
+ Tiny 2.11 board (http://paparazzi.enac.fr/wiki/index.php/Tiny_v2)
+ XBee modem
+ LEA 5H
+-->
+
+<airframe name="Funjet mm 1">
+
+ <firmware name="fixedwing">
+ <target name="sim" board="pc">
+ <define name="AGR_CLIMB"/>
+ <define name="LOITER_TRIM"/>
+ <define name="ALT_KALMAN"/>
+ <define name="WIND_INFO"/>
+ <define name="WIND_INFO_RET"/>
+ </target>
+
+ <target name="ap" board="tiny_2.11">
+ <define name="AGR_CLIMB"/>
+ <define name="LOITER_TRIM"/>
+ <define name="ALT_KALMAN"/>
+ <define name="WIND_INFO"/>
+ <define name="WIND_INFO_RET"/>
+ <define name="USE_I2C0"/>
+ </target>
+
+ <subsystem name="radio_control" type="ppm"/>
+
+ <!-- Communication -->
+ <subsystem name="telemetry" type="xbee_api">
+ <param name="MODEM_BAUD" value="B57600"/>
+ </subsystem>
+
+ <!-- Actuators are automatically chosen according to board-->
+ <subsystem name="control"/>
+ <!-- Sensors -->
+ <!--subsystem name="attitude" type="infrared"/-->
+ <subsystem name="gps" type="ublox_lea5h">
+ <param name="GPS_BAUD" value="B38400"/>
+ </subsystem>
+ <subsystem name="navigation"/>
+ <subsystem name="i2c"/>
+ </firmware>
+
+ <firmware name="setup">
+ <target name="tunnel" board="tiny_2.11"/>
+ <target name="usb_tunnel_0" board="tiny_2.11"/>
+ <target name="usb_tunnel_1" board="tiny_2.11"/>
+ <target name="setup_actuators" board="tiny_2.11"/>
+ </firmware>
+
+
+ <!-- modules -->
+ <modules>
+ <load name="ins_arduimu.xml"/>
+ </modules>
+
+ <!-- commands section -->
+ <servos>
+ <servo name="MOTOR" no="0" min="1000" neutral="1000" max="2000"/>
+ <servo name="AILEVON_LEFT" no="2" min="1900" neutral="1442" max="1100"/>
+ <servo name="AILEVON_RIGHT" no="6" min="1100" neutral="1549" max="1900"/>
+ <servo name="HATCH" no="7" min="1070" neutral="1070" max="2200"/>
+ </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"/>
+ </commands>
+
+ <rc_commands>
+ <set command="THROTTLE" value="@THROTTLE"/>
+ <set command="ROLL" value="@ROLL"/>
+ <set command="PITCH" value="@PITCH"/>
+ <set command="HATCH" value="@CALIB"/>
+ </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"/>
+ <set servo="HATCH" value="@HATCH"/>
+ </command_laws>
+
+ <section name="AUTO1" prefix="AUTO1_">
+ <define name="MAX_ROLL" value="0.85"/>
+ <define name="MAX_PITCH" value="0.6"/>
+ </section>
+
+ <section name="adc" prefix="ADC_CHANNEL_">
+ <define name="IR1" value="ADC_1"/>
+ <define name="IR2" value="ADC_2"/>
+ <define name="IR_TOP" value="ADC_0"/>
+ <define name="IR_NB_SAMPLES" value="16"/>
+ </section>
+
+ <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="BAT">
+ <!--define name="MILLIAMP_AT_FULL_THROTTLE" value="10000."/-->
+ <define name="ADC_CHANNEL_CURRENT" value="ADC_4"/>
+ <define name="MilliAmpereOfAdc(adc)" value="(88*adc)"/>
+ <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+ <!-- 0.0247311828 -->
+ <!-- 0.02432905 -->
+ <define name="VoltageOfAdc(adc)" value="(0.02454*adc)"/>
+ </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="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/> -->
+<!-- <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
+ <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
+
+ <define name="TRIGGER_DELAY" value="1."/>
+ <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.25"/>
+ <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.85"/>
+ <define name="AUTO_THROTTLE_LOITER_TRIM" value="1000"/>
+ <define name="AUTO_THROTTLE_DASH_TRIM" value="-1200"/>
+ <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"/>
+
+ <!--define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
+ <define name="ROLL_RATE_GAIN" value="-1500"/-->
+
+ </section>
+
+ <section name="NAV">
+ <define name="NAV_PITCH" value="0."/>
+ <define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
+ </section>
+
+ <section name="AGGRESSIVE" prefix="AGR_">
+ <define name="BLEND_START" value="50"/><!-- Altitude Error to Initiate
Aggressive Climb CANNOT BE ZERO!!-->
+ <define name="BLEND_END" value="15"/><!-- Altitude Error to Blend
Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
+ <define name="CLIMB_THROTTLE" value="0.9"/><!-- Gaz for Aggressive Climb
-->
+ <define name="CLIMB_PITCH" value="0.35"/><!-- Pitch for Aggressive Climb
-->
+ <define name="DESCENT_THROTTLE" value="0.05"/><!-- Gaz for Aggressive
Decent -->
+ <define name="DESCENT_PITCH" value="-0.35"/><!-- 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"/>
+ <define name="HOME_RADIUS" value="100" unit="m"/>
+</section>
+
+</airframe>
Deleted: paparazzi3/trunk/conf/modules/ArduIMU.xml
===================================================================
--- paparazzi3/trunk/conf/modules/ArduIMU.xml 2010-10-18 22:19:40 UTC (rev
6168)
+++ paparazzi3/trunk/conf/modules/ArduIMU.xml 2010-10-18 22:36:14 UTC (rev
6169)
@@ -1,17 +0,0 @@
-<!DOCTYPE module SYSTEM "module.dtd">
-
-<module name="ArduIMU">
- <header>
- <file name="ArduIMU.h"/>
- </header>
- <init fun="ArduIMU_init()"/>
- <periodic fun="ArduIMU_periodic()" freq="15" autorun="TRUE"/>
<!-- 15 ist soll -->
- <periodic fun="ArduIMU_periodicGPS()" freq="8" autorun="TRUE"/> <!--
8 ist soll -->
- <makefile target="ap">
- <file name="ArduIMU.c"/>
- </makefile>
- <!--makefile target="sim">
- <file name="sim_MPPT.c"/>
- </makefile-->
-</module>
-
Copied: paparazzi3/trunk/conf/modules/ins_arduimu.xml (from rev 6168,
paparazzi3/trunk/conf/modules/ArduIMU.xml)
===================================================================
--- paparazzi3/trunk/conf/modules/ins_arduimu.xml
(rev 0)
+++ paparazzi3/trunk/conf/modules/ins_arduimu.xml 2010-10-18 22:36:14 UTC
(rev 6169)
@@ -0,0 +1,17 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ins_ArduIMU" dir="ins">
+ <header>
+ <file name="ins_arduimu.h"/>
+ </header>
+ <init fun="ArduIMU_init()"/>
+ <periodic fun="ArduIMU_periodic()" freq="15" autorun="TRUE"/>
<!-- 15 ist soll -->
+ <periodic fun="ArduIMU_periodicGPS()" freq="8" autorun="TRUE"/> <!--
8 ist soll -->
+ <makefile target="ap">
+ <file name="ins_arduimu.c"/>
+ </makefile>
+ <!--makefile target="sim">
+ <file name="sim_MPPT.c"/>
+ </makefile-->
+</module>
+
Added: paparazzi3/trunk/conf/settings/tuning_ins.xml
===================================================================
--- paparazzi3/trunk/conf/settings/tuning_ins.xml
(rev 0)
+++ paparazzi3/trunk/conf/settings/tuning_ins.xml 2010-10-18 22:36:14 UTC
(rev 6169)
@@ -0,0 +1,98 @@
+<!DOCTYPE settings SYSTEM "settings.dtd">
+
+<!-- A conf to use to tune a new A/C -->
+
+<settings>
+ <dl_settings>
+ <dl_settings NAME="flight params">
+ <dl_setting MAX="1000" MIN="0" STEP="10" VAR="flight_altitude"
shortname="altitude"/>
+ <dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_east"/>
+ <dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_north"/>
+ </dl_settings>
+
+ <dl_settings NAME="mode">
+ <dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode" module="autopilot"/>
+ <dl_setting MAX="1" MIN="0" STEP="1" VAR="alt_kalman_enabled"
shortname="alt_kalman" module="estimator"/>
+ <dl_setting MAX="0" MIN="0" STEP="1" VAR="estimator_flight_time"
shortname="flight time"/>
+ <dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
+ <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
+ <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
+ <dl_setting MAX="2" MIN="0" STEP="1"
VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
+ <dl_setting MAX="1" MIN="0" STEP="1"
VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
+ <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx"
handler="Reset" shortname="GPS reset"/>
+
+ <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="nav"
handler="SetNavRadius">
+ <strip_button icon="circle-right.png" name="Circle right" value="1"/>
+ <strip_button icon="circle-left.png" name="Circle left" value="-1"/>
+ <key_press key="greater" value="1"/>
+ <key_press key="less" value="-1"/>
+ </dl_setting>
+ </dl_settings>
+
+ <dl_settings NAME="control">
+ <dl_settings NAME="ins">
+ <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral"
shortname="roll_neutral" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
+ <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral"
shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+ </dl_settings>
+
+
+ <dl_settings NAME="attitude">
+ <dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain"
shortname="roll_pgain" module="fw_h_ctl"/>
+ <dl_setting MAX="1" MIN="0" STEP="0.05" VAR="h_ctl_roll_max_setpoint"
shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT"/>
+ <dl_setting MAX="000" MIN="-25000" STEP="250" VAR="h_ctl_pitch_pgain"
shortname="pitch_pgain" param="H_CTL_PITCH_PGAIN"/>
+ <dl_setting MAX="0" MIN="-50000" STEP="10" VAR="h_ctl_pitch_dgain"
shortname="pitch_dgain" param="H_CTL_PITCH_DGAIN"/>
+ <dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_elevator_of_roll"
shortname="elevator_of_roll" param="H_CTL_ELEVATOR_OF_ROLL"/>
+ <dl_setting MAX="5000" MIN="0" STEP="100"
VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle"/>
+
+
+ <dl_setting MAX="0" MIN="-15000" STEP="250"
VAR="h_ctl_roll_attitude_gain" shortname="roll attitude pgain"
param="H_CTL_ROLL_ATTITUDE_GAIN"/>
+ <dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_rate_gain"
shortname="roll rate gain" param="H_CTL_ROLL_RATE_GAIN"/>
+
+ </dl_settings>
+
+ <dl_settings name="alt">
+ <dl_setting MAX="0" MIN="-0.2" STEP="0.01" VAR="v_ctl_altitude_pgain"
shortname="alt_pgain" param="V_CTL_ALTITUDE_PGAIN"/>
+ </dl_settings>
+
+ <dl_settings name="auto_throttle">
+ <dl_setting MAX="1" MIN="0.0" STEP="0.05"
VAR="v_ctl_auto_throttle_cruise_throttle" shortname="cruise throttle"
module="fw_v_ctl" handler="SetCruiseThrottle"
param="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE">
+ <strip_button name="Loiter" value="0.1"/>
+ <strip_button name="Cruise" value="0"/>
+ <strip_button name="Dash" value="1"/>
+ </dl_setting>
+
+
+ <dl_setting MAX="0.00" MIN="-0.05" STEP="0.005"
VAR="v_ctl_auto_throttle_pgain" shortname="throttle_pgain"
param="V_CTL_AUTO_THROTTLE_PGAIN"/>
+ <dl_setting MAX="1" MIN="0.0" STEP="0.05"
VAR="v_ctl_auto_throttle_igain" shortname="throttle_igain"
param="V_CTL_AUTO_THROTTLE_IGAIN"/>
+ <dl_setting MAX="2" MIN="0.0" STEP="0.1"
VAR="v_ctl_auto_throttle_dgain" shortname="throttle_dgain"/>
+ <dl_setting MAX="0" MIN="-4000" STEP="100"
VAR="v_ctl_auto_throttle_dash_trim" shortname="dash trim"/>
+ <dl_setting MIN="0" MAX="3000" STEP="100"
VAR="v_ctl_auto_throttle_loiter_trim" shortname="loiter trim"
param="V_CTL_AUTO_THROTTLE_LOITER_TRIM"/>
+ <dl_setting MAX="1" MIN="0" STEP="0.01"
VAR="v_ctl_auto_throttle_climb_throttle_increment" shortname="throttle_incr"
param="V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT"/>
+ <dl_setting MAX="1" MIN="0" STEP="0.01"
VAR="v_ctl_auto_throttle_pitch_of_vz_pgain" shortname="pitch_of_vz"
param="V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN"/>
+ <dl_setting MAX="10" MIN="-10" STEP="0.1"
VAR="v_ctl_auto_throttle_pitch_of_vz_dgain" shortname="pitch_of_vz (d)"/>
+ </dl_settings>
+
+ <dl_settings name="auto_pitch">
+ <dl_setting MAX="-0.01" MIN="-0.1" STEP="0.01"
VAR="v_ctl_auto_pitch_pgain" shortname="pgain" param="V_CTL_AUTO_PITCH_PGAIN"/>
+ <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_pitch_igain"
shortname="igain" param="V_CTL_AUTO_PITCH_IGAIN"/>
+ </dl_settings>
+
+ <dl_settings name="nav">
+ <dl_setting MAX="-0.1" MIN="-3" STEP="0.05" VAR="h_ctl_course_pgain"
shortname="course pgain" param="H_CTL_COURSE_PGAIN"/>
+ <dl_setting MAX="2" MIN="0" STEP="0.1" VAR="h_ctl_course_dgain"
shortname="course dgain"/>
+ <dl_setting MAX="2" MIN="0.1" STEP="0.05"
VAR="h_ctl_course_pre_bank_correction" shortname="pre bank cor"/>
+ <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="nav_glide_pitch_trim"
shortname="glide pitch trim" param="NAV_GLIDE_PITCH_TRIM"/>
+ <dl_setting MAX="1" MIN="0.02" STEP="0.01" VAR="h_ctl_roll_slew"
shortname="roll slew"/>
+ <dl_setting MAX="500" MIN="-500" STEP="5" VAR="nav_radius"/>
+ <dl_setting MAX="359" MIN="0" STEP="5" VAR="nav_course"/>
+ <dl_setting MAX="2" MIN="1" STEP="1" VAR="nav_mode"/>
+ <dl_setting MAX="5" MIN="-5" STEP="0.5" VAR="nav_climb"/>
+ <dl_setting MAX="15" MIN="-15" STEP="1" VAR="fp_pitch"/>
+ <dl_setting MAX="50" MIN="-50" STEP="5" VAR="nav_shift" module="nav"
handler="IncreaseShift" shortname="inc. shift"/>
+ <dl_setting MAX="50" MIN="5" STEP="0.5"
VAR="nav_ground_speed_setpoint" shortname="ground speed"/>
+ <dl_setting MAX="0." MIN="-0.2" STEP="0.01"
VAR="nav_ground_speed_pgain" shortname="ground speed pgain"/>
+ <dl_setting MAX="500" MIN="50" STEP="5" VAR="nav_survey_shift"/>
+ </dl_settings>
+ </dl_settings>
+ </dl_settings>
+</settings>
Modified: paparazzi3/trunk/sw/airborne/gps.h
===================================================================
--- paparazzi3/trunk/sw/airborne/gps.h 2010-10-18 22:19:40 UTC (rev 6168)
+++ paparazzi3/trunk/sw/airborne/gps.h 2010-10-18 22:36:14 UTC (rev 6169)
@@ -48,17 +48,22 @@
extern uint8_t gps_mode; /* Receiver status */
+extern uint8_t gps_status_flags;
+extern uint8_t gps_sol_flags;
extern uint16_t gps_week; /* weeks */
extern uint32_t gps_itow; /* ms */
extern int32_t gps_alt; /* cm */
+extern uint16_t gps_speed_3d; /* cm/s */
extern uint16_t gps_gspeed; /* cm/s */
extern int16_t gps_climb; /* m/s */
extern int16_t gps_course; /* decideg */
extern int32_t gps_utm_east, gps_utm_north; /** cm */
extern uint8_t gps_utm_zone;
extern int32_t gps_lat, gps_lon; /* 1e7 deg */
+extern int32_t gps_hmsl;
extern uint16_t gps_PDOP;
extern uint32_t gps_Pacc, gps_Sacc;
+extern int32_t gps_ecefVZ;
extern uint8_t gps_numSV;
extern uint8_t gps_configuring;
Modified: paparazzi3/trunk/sw/airborne/gps_ubx.c
===================================================================
--- paparazzi3/trunk/sw/airborne/gps_ubx.c 2010-10-18 22:19:40 UTC (rev
6168)
+++ paparazzi3/trunk/sw/airborne/gps_ubx.c 2010-10-18 22:36:14 UTC (rev
6169)
@@ -81,21 +81,26 @@
uint32_t gps_t0_frac;
#endif
int32_t gps_alt;
+uint16_t gps_speed_3d;
uint16_t gps_gspeed;
int16_t gps_climb;
int16_t gps_course;
int32_t gps_utm_east, gps_utm_north;
uint8_t gps_utm_zone;
uint8_t gps_mode;
+uint8_t gps_status_flags;
+uint8_t gps_sol_flags;
volatile bool_t gps_msg_received;
bool_t gps_pos_available;
uint8_t ubx_id, ubx_class;
uint16_t ubx_len;
int32_t gps_lat, gps_lon;
+int32_t gps_hmsl;
uint16_t gps_reset;
uint16_t gps_PDOP;
uint32_t gps_Pacc, gps_Sacc;
+int32_t gps_ecefVZ;
uint8_t gps_numSV;
#define UTM_HEM_NORTH 0
@@ -250,11 +255,14 @@
if (ubx_class == UBX_NAV_ID) {
if (ubx_id == UBX_NAV_STATUS_ID) {
gps_mode = UBX_NAV_STATUS_GPSfix(ubx_msg_buf);
+ gps_status_flags = UBX_NAV_STATUS_Flags(ubx_msg_buf);
+ gps_sol_flags = UBX_NAV_SOL_Flags(ubx_msg_buf);
#ifdef GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
} else if (ubx_id == UBX_NAV_POSLLH_ID) {
gps_lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf);
gps_lon = UBX_NAV_POSLLH_LON(ubx_msg_buf);
+ gps_hmsl = UBX_NAV_POSLLH_HMSL(ubx_msg_buf);
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7),
nav_utm_zone0);
@@ -273,6 +281,7 @@
gps_utm_zone = UBX_NAV_POSUTM_ZONE(ubx_msg_buf);
#endif
} else if (ubx_id == UBX_NAV_VELNED_ID) {
+ gps_speed_3d = UBX_NAV_VELNED_Speed(ubx_msg_buf);
gps_gspeed = UBX_NAV_VELNED_GSpeed(ubx_msg_buf);
gps_climb = - UBX_NAV_VELNED_VEL_D(ubx_msg_buf);
gps_course = UBX_NAV_VELNED_Heading(ubx_msg_buf) / 10000;
@@ -290,6 +299,7 @@
gps_mode = UBX_NAV_SOL_GPSfix(ubx_msg_buf);
gps_PDOP = UBX_NAV_SOL_PDOP(ubx_msg_buf);
gps_Pacc = UBX_NAV_SOL_Pacc(ubx_msg_buf);
+ gps_ecefVZ = UBX_NAV_SOL_ECEFVZ(ubx_msg_buf);
gps_Sacc = UBX_NAV_SOL_Sacc(ubx_msg_buf);
gps_numSV = UBX_NAV_SOL_numSV(ubx_msg_buf);
gps_week = UBX_NAV_SOL_week(ubx_msg_buf);
Deleted: paparazzi3/trunk/sw/airborne/modules/ins/ArduIMU.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ArduIMU.c 2010-10-18 22:19:40 UTC
(rev 6168)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ArduIMU.c 2010-10-18 22:36:14 UTC
(rev 6169)
@@ -1,211 +0,0 @@
-/*
-C Datei für die Einbindung eines ArduIMU's
address@hidden: schmiemi
- chaneren
-*/
-
-
-#include <stdbool.h>
-#include "ArduIMU.h"
-#include "main_fbw.h"
-#include "i2c.h"
-
-// test
-#include "estimator.h"
-
-// für das Senden von GPS-Daten an den ArduIMU
-#include "gps.h"
-int32_t GPS_Data[13];
-static volatile bool_t gps_i2c_done;
-
-
-// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write
-// einzugebende Adresse im ArduIMU ist 0000 1011
-//da ArduIMU das Read/Write Bit selber anfügt.
-#define ArduIMU_SLAVE_ADDR 0x22
-
-#ifndef DOWNLINK_DEVICE
-#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
-#endif
-
-#include "uart.h"
-#include "messages.h"
-#include "downlink.h"
-
-static volatile bool_t ArduIMU_i2c_done;
-static int16_t recievedData[NB_DATA];
-float ArduIMU_data[NB_DATA];
-int8_t messageNr;
-int8_t imu_daten_angefordert;
-int8_t gps_daten_abgespeichert;
-int8_t gps_daten_versendet_msg1;
-int8_t gps_daten_versendet_msg2;
-
-float arduimu_roll_neutral;
-float arduimu_pitch_neutral;
-float pitch_of_throttle_gain;
-float throttle_slew;
-
-void ArduIMU_init( void ) {
- ArduIMU_i2c_done = TRUE;
- gps_i2c_done = TRUE;
- i2c0_buf[0] = 0;
- i2c0_buf[1] = 0;
- messageNr = 0;
- imu_daten_angefordert = FALSE;
- gps_daten_abgespeichert = FALSE;
- gps_daten_versendet_msg1 = FALSE;
- gps_daten_versendet_msg2 = FALSE;
- arduimu_roll_neutral = ARDUIMU_ROLL_NEUTRAL;
- arduimu_pitch_neutral = ARDUIMU_PITCH_NEUTRAL;
- pitch_of_throttle_gain = PITCH_OF_THROTTLE_GAIN;
- throttle_slew = V_CTL_THROTTLE_SLEW;
-}
-
-
-void ArduIMU_periodicGPS( void ) {
-
- if ( gps_daten_versendet_msg1 == TRUE && gps_daten_versendet_msg2 ==
TRUE ) {
- gps_daten_abgespeichert = FALSE;
- }
-
- if( imu_daten_angefordert == TRUE ){
- IMU_Daten_verarbeiten();
- }
-
- if ( gps_daten_abgespeichert == FALSE ) {
- //posllh
- GPS_Data [0] = gps_itow;
- GPS_Data [1] = gps_lon;
- GPS_Data [2] = gps_lat;
- GPS_Data [3] = gps_alt; //höhe über elipsoid
- GPS_Data [4] = gps_hmsl; //höhe über sea level
- //velend
- GPS_Data [5] = gps_speed; //speed 3D
- GPS_Data [6] = gps_gspeed; //ground speed
- GPS_Data [7] = gps_course * 100000; //Kurs
- //status
- GPS_Data [8] = gps_mode; //fix
- GPS_Data [9] = gps_stauts_flag; //flags
- //sol
- GPS_Data [10] = gps_mode; //fix
- GPS_Data [11] = gps_sol_flags; //flags
- GPS_Data [12] = gps_ecefVZ; //ecefVZ
- GPS_Data [13] = gps_numSV;
-
- gps_daten_abgespeichert = TRUE;
- }
-
- if(messageNr == 0) {
-
- //test für 32bit in byte packete abzupacken:
- //GPS_Data [0] = -1550138773;
-
- i2c0_buf[0] = 0; //message Nr = 0 -->
itow bis ground speed
- i2c0_buf[1] = (uint8_t) GPS_Data[0]; //itow
- i2c0_buf[2] = (uint8_t) (GPS_Data[0] >>8);
- i2c0_buf[3] = (uint8_t) (GPS_Data[0] >>16);
- i2c0_buf[4] = (uint8_t) (GPS_Data[0] >>24);
- i2c0_buf[5] = (uint8_t) GPS_Data[1]; //lon
- i2c0_buf[6] = (uint8_t) (GPS_Data[1] >>8);
- i2c0_buf[7] = (uint8_t) (GPS_Data[1] >>16);
- i2c0_buf[8] = (uint8_t) (GPS_Data[1] >>24);
- i2c0_buf[9] = (uint8_t) GPS_Data[2]; //lat
- i2c0_buf[10] = (uint8_t) (GPS_Data[2] >>8);
- i2c0_buf[11] = (uint8_t) (GPS_Data[2] >>16);
- i2c0_buf[12] = (uint8_t) (GPS_Data[2] >>24);
- i2c0_buf[13] = (uint8_t) GPS_Data[3]; //height
- i2c0_buf[14] = (uint8_t) (GPS_Data[3] >>8);
- i2c0_buf[15] = (uint8_t) (GPS_Data[3] >>16);
- i2c0_buf[16] = (uint8_t) (GPS_Data[3] >>24);
- i2c0_buf[17] = (uint8_t) GPS_Data[4]; //hmsl
- i2c0_buf[18] = (uint8_t) (GPS_Data[4] >>8);
- i2c0_buf[19] = (uint8_t) (GPS_Data[4] >>16);
- i2c0_buf[20] = (uint8_t) (GPS_Data[4] >>24);
- i2c0_buf[21] = (uint8_t) GPS_Data[5]; //speed
- i2c0_buf[22] = (uint8_t) (GPS_Data[5] >>8);
- i2c0_buf[23] = (uint8_t) (GPS_Data[5] >>16);
- i2c0_buf[24] = (uint8_t) (GPS_Data[5] >>24);
- i2c0_buf[25] = (uint8_t) GPS_Data[6]; //gspeed
- i2c0_buf[26] = (uint8_t) (GPS_Data[6] >>8);
- i2c0_buf[27] = (uint8_t) (GPS_Data[6] >>16);
- i2c0_buf[28] = (uint8_t) (GPS_Data[6] >>24);
- i2c0_transmit(ArduIMU_SLAVE_ADDR, 28, &gps_i2c_done);
-
- gps_daten_versendet_msg1 = TRUE;
- messageNr =1;
- }
- else {
-
- i2c0_buf[0] = 1; //message Nr = 1 --> ground
course, ecefVZ, numSV, Fix, flags, fix, flags
- i2c0_buf[1] = GPS_Data[7]; //ground course
- i2c0_buf[2] = (GPS_Data[7] >>8);
- i2c0_buf[3] = (GPS_Data[7] >>16);
- i2c0_buf[4] = (GPS_Data[7] >>24);
- i2c0_buf[5] = GPS_Data[12]; //ecefVZ
- i2c0_buf[6] = (GPS_Data[12] >>8);
- i2c0_buf[7] = (GPS_Data[12] >>16);
- i2c0_buf[8] = (GPS_Data[12] >>24);
- i2c0_buf[9] = GPS_Data[13]; //numSV
- i2c0_buf[10] = GPS_Data[8]; //status gps fix
- i2c0_buf[11] = GPS_Data[9]; //status flags
- i2c0_buf[12] = GPS_Data[10]; //sol gps fix
- i2c0_buf[13] = GPS_Data[11]; //sol flags
- i2c0_transmit(ArduIMU_SLAVE_ADDR, 13, &gps_i2c_done);
-
- gps_daten_versendet_msg2 = TRUE;
- messageNr = 0;
- }
-
- //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &gps_mode , &gps_numSV
,&gps_alt , &gps_hmsl , &gps_itow, &gps_speed);
-}
-
-void ArduIMU_periodic( void ) {
-//Frequenz des Aufrufs wird in conf/modules/ArduIMU.xml festgelegt.
-
- //I2C-Nachricht anfordern an Slave Adresse, erwartete Anzahl Byte,
Status
- if (imu_daten_angefordert == TRUE) {
- IMU_Daten_verarbeiten();
- }
- i2c0_receive(ArduIMU_SLAVE_ADDR, 12, &ArduIMU_i2c_done);
- imu_daten_angefordert = TRUE;
- /*
- Buffer O: Roll
- Buffer 1: Pitch
- Buffer 2: Yaw
- Buffer 3: Beschleunigung X-Achse
- Buffer 4: Beschleunigung Y-Achse
- Buffer 5: Beschleunigung Z-Achse
- */
-
-
- //Nachricht zum GCS senden
- // DOWNLINK_SEND_ArduIMU(DefaultChannel, &ArduIMU_data[0],
&ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4],
&ArduIMU_data[5]);
-
- // DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &airspeed_mode ,
&altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert,
&amsys_baro_scaliert);
-}
-
-void IMU_Daten_verarbeiten( void ) {
- //Empfangene Byts zusammenfügen und bereitstellen
- recievedData[0] = (i2c0_buf[1]<<8) | i2c0_buf[0];
- recievedData[1] = (i2c0_buf[3]<<8) | i2c0_buf[2];
- recievedData[2] = (i2c0_buf[5]<<8) | i2c0_buf[4];
- recievedData[3] = (i2c0_buf[7]<<8) | i2c0_buf[6];
- recievedData[4] = (i2c0_buf[9]<<8) | i2c0_buf[8];
- recievedData[5] = (i2c0_buf[11]<<8) | i2c0_buf[10];
-
- //Floats zurück transformieren. Transformation ist auf ArduIMU
programmiert.
- ArduIMU_data[0] = (float) (recievedData[0] / (float)100);
- ArduIMU_data[1] = (float) (recievedData[1] / (float)100);
- ArduIMU_data[2] = (float) (recievedData[2] / (float)100);
- ArduIMU_data[3] = (float) (recievedData[3] / (float)100);
- ArduIMU_data[4] = (float) (recievedData[4] / (float)100);
- ArduIMU_data[5] = (float) (recievedData[5] / (float)100);
-
- // test
- estimator_phi = (float)ArduIMU_data[0]*0.01745329252 -
arduimu_roll_neutral;
- estimator_theta = (float)ArduIMU_data[1]*0.01745329252 -
arduimu_pitch_neutral;
- imu_daten_angefordert = FALSE;
-}
-
-
Deleted: paparazzi3/trunk/sw/airborne/modules/ins/ArduIMU.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ArduIMU.h 2010-10-18 22:19:40 UTC
(rev 6168)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ArduIMU.h 2010-10-18 22:36:14 UTC
(rev 6169)
@@ -1,24 +0,0 @@
-
-
-#ifndef ArduIMU_H
-#define ArduIMU_H
-
-#include <inttypes.h>
-
-#define NB_DATA 6
-
-extern float ArduIMU_data[NB_DATA];
-
-extern float arduimu_roll_neutral;
-extern float arduimu_pitch_neutral;
-
-//mixer
-extern float pitch_of_throttle_gain;
-extern float throttle_slew;
-
-void ArduIMU_init( void );
-void ArduIMU_periodic( void );
-void ArduIMU_periodicGPS( void );
-void IMU_Daten_verarbeiten( void );
-
-#endif // ArduIMU_H
Copied: paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c (from rev 6168,
paparazzi3/trunk/sw/airborne/modules/ins/ArduIMU.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c 2010-10-18
22:36:14 UTC (rev 6169)
@@ -0,0 +1,218 @@
+/*
+C Datei für die Einbindung eines ArduIMU's
address@hidden: schmiemi
+ chaneren
+*/
+
+
+#include <stdbool.h>
+#include "ins_arduimu.h"
+#include "main_fbw.h"
+#include "i2c.h"
+
+// test
+#include "estimator.h"
+
+// für das Senden von GPS-Daten an den ArduIMU
+#include "gps.h"
+int32_t GPS_Data[13];
+
+#ifndef ARDUIMU_I2C_DEV
+#define ARDUIMU_I2C_DEV i2c0
+#endif
+
+// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write
+// einzugebende Adresse im ArduIMU ist 0000 1011
+//da ArduIMU das Read/Write Bit selber anfügt.
+#define ArduIMU_SLAVE_ADDR 0x22
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+struct i2c_transaction ardu_gps_trans;
+struct i2c_transaction ardu_ins_trans;
+
+static int16_t recievedData[NB_DATA];
+float ArduIMU_data[NB_DATA];
+int8_t messageNr;
+int8_t imu_daten_angefordert;
+int8_t gps_daten_abgespeichert;
+int8_t gps_daten_versendet_msg1;
+int8_t gps_daten_versendet_msg2;
+
+float ins_roll_neutral;
+float ins_pitch_neutral;
+//float pitch_of_throttle_gain;
+float throttle_slew;
+
+void ArduIMU_init( void ) {
+ ardu_gps_trans.buf[0] = 0;
+ ardu_gps_trans.buf[1] = 0;
+ messageNr = 0;
+ imu_daten_angefordert = FALSE;
+ gps_daten_abgespeichert = FALSE;
+ gps_daten_versendet_msg1 = FALSE;
+ gps_daten_versendet_msg2 = FALSE;
+ ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+ ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+// pitch_of_throttle_gain = PITCH_OF_THROTTLE_GAIN;
+ throttle_slew = V_CTL_THROTTLE_SLEW;
+}
+
+
+void ArduIMU_periodicGPS( void ) {
+
+ if ( gps_daten_versendet_msg1 == TRUE && gps_daten_versendet_msg2 ==
TRUE ) {
+ gps_daten_abgespeichert = FALSE;
+ }
+
+ if( imu_daten_angefordert == TRUE ){
+ IMU_Daten_verarbeiten();
+ }
+
+ if ( gps_daten_abgespeichert == FALSE ) {
+ //posllh
+ GPS_Data [0] = gps_itow;
+ GPS_Data [1] = gps_lon;
+ GPS_Data [2] = gps_lat;
+ GPS_Data [3] = gps_alt; //höhe über elipsoid
+ GPS_Data [4] = gps_hmsl; //höhe über sea level
+ //velend
+ GPS_Data [5] = gps_speed_3d; //speed 3D
+ GPS_Data [6] = gps_gspeed; //ground speed
+ GPS_Data [7] = gps_course * 100000; //Kurs
+ //status
+ GPS_Data [8] = gps_mode; //fix
+ GPS_Data [9] = gps_status_flags; //flags
+ //sol
+ GPS_Data [10] = gps_mode; //fix
+ GPS_Data [11] = gps_sol_flags; //flags
+ GPS_Data [12] = gps_ecefVZ; //ecefVZ
+ GPS_Data [13] = gps_numSV;
+
+ gps_daten_abgespeichert = TRUE;
+ }
+
+ if(messageNr == 0) {
+
+ //test für 32bit in byte packete abzupacken:
+ //GPS_Data [0] = -1550138773;
+
+ ardu_gps_trans.buf[0] = 0; //message Nr =
0 --> itow bis ground speed
+ ardu_gps_trans.buf[1] = (uint8_t) GPS_Data[0]; //itow
+ ardu_gps_trans.buf[2] = (uint8_t) (GPS_Data[0] >>8);
+ ardu_gps_trans.buf[3] = (uint8_t) (GPS_Data[0] >>16);
+ ardu_gps_trans.buf[4] = (uint8_t) (GPS_Data[0] >>24);
+ ardu_gps_trans.buf[5] = (uint8_t) GPS_Data[1]; //lon
+ ardu_gps_trans.buf[6] = (uint8_t) (GPS_Data[1] >>8);
+ ardu_gps_trans.buf[7] = (uint8_t) (GPS_Data[1] >>16);
+ ardu_gps_trans.buf[8] = (uint8_t) (GPS_Data[1] >>24);
+ ardu_gps_trans.buf[9] = (uint8_t) GPS_Data[2]; //lat
+ ardu_gps_trans.buf[10] = (uint8_t) (GPS_Data[2] >>8);
+ ardu_gps_trans.buf[11] = (uint8_t) (GPS_Data[2] >>16);
+ ardu_gps_trans.buf[12] = (uint8_t) (GPS_Data[2] >>24);
+ ardu_gps_trans.buf[13] = (uint8_t) GPS_Data[3]; //height
+ ardu_gps_trans.buf[14] = (uint8_t) (GPS_Data[3] >>8);
+ ardu_gps_trans.buf[15] = (uint8_t) (GPS_Data[3] >>16);
+ ardu_gps_trans.buf[16] = (uint8_t) (GPS_Data[3] >>24);
+ ardu_gps_trans.buf[17] = (uint8_t) GPS_Data[4]; //hmsl
+ ardu_gps_trans.buf[18] = (uint8_t) (GPS_Data[4] >>8);
+ ardu_gps_trans.buf[19] = (uint8_t) (GPS_Data[4] >>16);
+ ardu_gps_trans.buf[20] = (uint8_t) (GPS_Data[4] >>24);
+ ardu_gps_trans.buf[21] = (uint8_t) GPS_Data[5]; //speed
+ ardu_gps_trans.buf[22] = (uint8_t) (GPS_Data[5] >>8);
+ ardu_gps_trans.buf[23] = (uint8_t) (GPS_Data[5] >>16);
+ ardu_gps_trans.buf[24] = (uint8_t) (GPS_Data[5] >>24);
+ ardu_gps_trans.buf[25] = (uint8_t) GPS_Data[6]; //gspeed
+ ardu_gps_trans.buf[26] = (uint8_t) (GPS_Data[6] >>8);
+ ardu_gps_trans.buf[27] = (uint8_t) (GPS_Data[6] >>16);
+ ardu_gps_trans.buf[28] = (uint8_t) (GPS_Data[6] >>24);
+ I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 28);
+
+ gps_daten_versendet_msg1 = TRUE;
+ messageNr =1;
+ }
+ else {
+
+ ardu_gps_trans.buf[0] = 1; //message Nr = 1 -->
ground course, ecefVZ, numSV, Fix, flags, fix, flags
+ ardu_gps_trans.buf[1] = GPS_Data[7]; //ground course
+ ardu_gps_trans.buf[2] = (GPS_Data[7] >>8);
+ ardu_gps_trans.buf[3] = (GPS_Data[7] >>16);
+ ardu_gps_trans.buf[4] = (GPS_Data[7] >>24);
+ ardu_gps_trans.buf[5] = GPS_Data[12]; //ecefVZ
+ ardu_gps_trans.buf[6] = (GPS_Data[12] >>8);
+ ardu_gps_trans.buf[7] = (GPS_Data[12] >>16);
+ ardu_gps_trans.buf[8] = (GPS_Data[12] >>24);
+ ardu_gps_trans.buf[9] = GPS_Data[13]; //numSV
+ ardu_gps_trans.buf[10] = GPS_Data[8]; //status gps fix
+ ardu_gps_trans.buf[11] = GPS_Data[9]; //status flags
+ ardu_gps_trans.buf[12] = GPS_Data[10]; //sol gps fix
+ ardu_gps_trans.buf[13] = GPS_Data[11]; //sol flags
+ I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 13);
+
+ gps_daten_versendet_msg2 = TRUE;
+ messageNr = 0;
+ }
+
+ //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &gps_mode , &gps_numSV
,&gps_alt , &gps_hmsl , &gps_itow, &gps_speed_3d);
+}
+
+void ArduIMU_periodic( void ) {
+//Frequenz des Aufrufs wird in conf/modules/ArduIMU.xml festgelegt.
+
+ //I2C-Nachricht anfordern an Slave Adresse, erwartete Anzahl Byte,
Status
+ if (imu_daten_angefordert == TRUE) {
+ IMU_Daten_verarbeiten();
+ }
+ I2CReceive(ARDUIMU_I2C_DEV, ardu_ins_trans, ArduIMU_SLAVE_ADDR, 12);
+
+ imu_daten_angefordert = TRUE;
+ /*
+ Buffer O: Roll
+ Buffer 1: Pitch
+ Buffer 2: Yaw
+ Buffer 3: Beschleunigung X-Achse
+ Buffer 4: Beschleunigung Y-Achse
+ Buffer 5: Beschleunigung Z-Achse
+ */
+
+
+ //Nachricht zum GCS senden
+ // DOWNLINK_SEND_ArduIMU(DefaultChannel, &ArduIMU_data[0],
&ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4],
&ArduIMU_data[5]);
+
+ // DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &airspeed_mode ,
&altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert,
&amsys_baro_scaliert);
+}
+
+void IMU_Daten_verarbeiten( void ) {
+ //Empfangene Byts zusammenfügen und bereitstellen
+ recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0];
+ recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2];
+ recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4];
+ recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6];
+ recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8];
+ recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
+
+ //Floats zurück transformieren. Transformation ist auf ArduIMU
programmiert.
+ ArduIMU_data[0] = (float) (recievedData[0] / (float)100);
+ ArduIMU_data[1] = (float) (recievedData[1] / (float)100);
+ ArduIMU_data[2] = (float) (recievedData[2] / (float)100);
+ ArduIMU_data[3] = (float) (recievedData[3] / (float)100);
+ ArduIMU_data[4] = (float) (recievedData[4] / (float)100);
+ ArduIMU_data[5] = (float) (recievedData[5] / (float)100);
+
+ // test
+ estimator_phi = (float)ArduIMU_data[0]*0.01745329252 -
ins_roll_neutral;
+ estimator_theta = (float)ArduIMU_data[1]*0.01745329252 -
ins_pitch_neutral;
+ imu_daten_angefordert = FALSE;
+
+ {
+ float psi=0;
+ RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel,
&estimator_phi, &estimator_theta, &psi));
+ }
+}
+
Copied: paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.h (from rev 6168,
paparazzi3/trunk/sw/airborne/modules/ins/ArduIMU.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.h 2010-10-18
22:36:14 UTC (rev 6169)
@@ -0,0 +1,24 @@
+
+
+#ifndef ArduIMU_H
+#define ArduIMU_H
+
+#include <inttypes.h>
+
+#define NB_DATA 6
+
+extern float ArduIMU_data[NB_DATA];
+
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+
+//mixer
+extern float pitch_of_throttle_gain;
+extern float throttle_slew;
+
+void ArduIMU_init( void );
+void ArduIMU_periodic( void );
+void ArduIMU_periodicGPS( void );
+void IMU_Daten_verarbeiten( void );
+
+#endif // ArduIMU_H
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6169] Adapt ArduIMU to new i2c, fix gps.,
Martin Mueller <=