mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-01 04:46:51 +08:00
Adapt ArduIMU to new i2c, fix gps.
This commit is contained in:
Martin Mueller
@@ -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>
|
||||||
@@ -1,14 +1,14 @@
|
|||||||
<!DOCTYPE module SYSTEM "module.dtd">
|
<!DOCTYPE module SYSTEM "module.dtd">
|
||||||
|
|
||||||
<module name="ArduIMU">
|
<module name="ins_ArduIMU" dir="ins">
|
||||||
<header>
|
<header>
|
||||||
<file name="ArduIMU.h"/>
|
<file name="ins_arduimu.h"/>
|
||||||
</header>
|
</header>
|
||||||
<init fun="ArduIMU_init()"/>
|
<init fun="ArduIMU_init()"/>
|
||||||
<periodic fun="ArduIMU_periodic()" freq="15" autorun="TRUE"/> <!-- 15 ist soll -->
|
<periodic fun="ArduIMU_periodic()" freq="15" autorun="TRUE"/> <!-- 15 ist soll -->
|
||||||
<periodic fun="ArduIMU_periodicGPS()" freq="8" autorun="TRUE"/> <!-- 8 ist soll -->
|
<periodic fun="ArduIMU_periodicGPS()" freq="8" autorun="TRUE"/> <!-- 8 ist soll -->
|
||||||
<makefile target="ap">
|
<makefile target="ap">
|
||||||
<file name="ArduIMU.c"/>
|
<file name="ins_arduimu.c"/>
|
||||||
</makefile>
|
</makefile>
|
||||||
<!--makefile target="sim">
|
<!--makefile target="sim">
|
||||||
<file name="sim_MPPT.c"/>
|
<file name="sim_MPPT.c"/>
|
||||||
@@ -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>
|
||||||
@@ -48,17 +48,22 @@
|
|||||||
|
|
||||||
|
|
||||||
extern uint8_t gps_mode; /* Receiver status */
|
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 uint16_t gps_week; /* weeks */
|
||||||
extern uint32_t gps_itow; /* ms */
|
extern uint32_t gps_itow; /* ms */
|
||||||
extern int32_t gps_alt; /* cm */
|
extern int32_t gps_alt; /* cm */
|
||||||
|
extern uint16_t gps_speed_3d; /* cm/s */
|
||||||
extern uint16_t gps_gspeed; /* cm/s */
|
extern uint16_t gps_gspeed; /* cm/s */
|
||||||
extern int16_t gps_climb; /* m/s */
|
extern int16_t gps_climb; /* m/s */
|
||||||
extern int16_t gps_course; /* decideg */
|
extern int16_t gps_course; /* decideg */
|
||||||
extern int32_t gps_utm_east, gps_utm_north; /** cm */
|
extern int32_t gps_utm_east, gps_utm_north; /** cm */
|
||||||
extern uint8_t gps_utm_zone;
|
extern uint8_t gps_utm_zone;
|
||||||
extern int32_t gps_lat, gps_lon; /* 1e7 deg */
|
extern int32_t gps_lat, gps_lon; /* 1e7 deg */
|
||||||
|
extern int32_t gps_hmsl;
|
||||||
extern uint16_t gps_PDOP;
|
extern uint16_t gps_PDOP;
|
||||||
extern uint32_t gps_Pacc, gps_Sacc;
|
extern uint32_t gps_Pacc, gps_Sacc;
|
||||||
|
extern int32_t gps_ecefVZ;
|
||||||
extern uint8_t gps_numSV;
|
extern uint8_t gps_numSV;
|
||||||
extern uint8_t gps_configuring;
|
extern uint8_t gps_configuring;
|
||||||
|
|
||||||
|
|||||||
@@ -81,21 +81,26 @@ uint32_t gps_t0_itow;
|
|||||||
uint32_t gps_t0_frac;
|
uint32_t gps_t0_frac;
|
||||||
#endif
|
#endif
|
||||||
int32_t gps_alt;
|
int32_t gps_alt;
|
||||||
|
uint16_t gps_speed_3d;
|
||||||
uint16_t gps_gspeed;
|
uint16_t gps_gspeed;
|
||||||
int16_t gps_climb;
|
int16_t gps_climb;
|
||||||
int16_t gps_course;
|
int16_t gps_course;
|
||||||
int32_t gps_utm_east, gps_utm_north;
|
int32_t gps_utm_east, gps_utm_north;
|
||||||
uint8_t gps_utm_zone;
|
uint8_t gps_utm_zone;
|
||||||
uint8_t gps_mode;
|
uint8_t gps_mode;
|
||||||
|
uint8_t gps_status_flags;
|
||||||
|
uint8_t gps_sol_flags;
|
||||||
volatile bool_t gps_msg_received;
|
volatile bool_t gps_msg_received;
|
||||||
bool_t gps_pos_available;
|
bool_t gps_pos_available;
|
||||||
uint8_t ubx_id, ubx_class;
|
uint8_t ubx_id, ubx_class;
|
||||||
uint16_t ubx_len;
|
uint16_t ubx_len;
|
||||||
int32_t gps_lat, gps_lon;
|
int32_t gps_lat, gps_lon;
|
||||||
|
int32_t gps_hmsl;
|
||||||
uint16_t gps_reset;
|
uint16_t gps_reset;
|
||||||
|
|
||||||
uint16_t gps_PDOP;
|
uint16_t gps_PDOP;
|
||||||
uint32_t gps_Pacc, gps_Sacc;
|
uint32_t gps_Pacc, gps_Sacc;
|
||||||
|
int32_t gps_ecefVZ;
|
||||||
uint8_t gps_numSV;
|
uint8_t gps_numSV;
|
||||||
|
|
||||||
#define UTM_HEM_NORTH 0
|
#define UTM_HEM_NORTH 0
|
||||||
@@ -250,11 +255,14 @@ void parse_gps_msg( void ) {
|
|||||||
if (ubx_class == UBX_NAV_ID) {
|
if (ubx_class == UBX_NAV_ID) {
|
||||||
if (ubx_id == UBX_NAV_STATUS_ID) {
|
if (ubx_id == UBX_NAV_STATUS_ID) {
|
||||||
gps_mode = UBX_NAV_STATUS_GPSfix(ubx_msg_buf);
|
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
|
#ifdef GPS_USE_LATLONG
|
||||||
/* Computes from (lat, long) in the referenced UTM zone */
|
/* Computes from (lat, long) in the referenced UTM zone */
|
||||||
} else if (ubx_id == UBX_NAV_POSLLH_ID) {
|
} else if (ubx_id == UBX_NAV_POSLLH_ID) {
|
||||||
gps_lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf);
|
gps_lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf);
|
||||||
gps_lon = UBX_NAV_POSLLH_LON(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);
|
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
|
||||||
|
|
||||||
@@ -273,6 +281,7 @@ void parse_gps_msg( void ) {
|
|||||||
gps_utm_zone = UBX_NAV_POSUTM_ZONE(ubx_msg_buf);
|
gps_utm_zone = UBX_NAV_POSUTM_ZONE(ubx_msg_buf);
|
||||||
#endif
|
#endif
|
||||||
} else if (ubx_id == UBX_NAV_VELNED_ID) {
|
} 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_gspeed = UBX_NAV_VELNED_GSpeed(ubx_msg_buf);
|
||||||
gps_climb = - UBX_NAV_VELNED_VEL_D(ubx_msg_buf);
|
gps_climb = - UBX_NAV_VELNED_VEL_D(ubx_msg_buf);
|
||||||
gps_course = UBX_NAV_VELNED_Heading(ubx_msg_buf) / 10000;
|
gps_course = UBX_NAV_VELNED_Heading(ubx_msg_buf) / 10000;
|
||||||
@@ -290,6 +299,7 @@ void parse_gps_msg( void ) {
|
|||||||
gps_mode = UBX_NAV_SOL_GPSfix(ubx_msg_buf);
|
gps_mode = UBX_NAV_SOL_GPSfix(ubx_msg_buf);
|
||||||
gps_PDOP = UBX_NAV_SOL_PDOP(ubx_msg_buf);
|
gps_PDOP = UBX_NAV_SOL_PDOP(ubx_msg_buf);
|
||||||
gps_Pacc = UBX_NAV_SOL_Pacc(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_Sacc = UBX_NAV_SOL_Sacc(ubx_msg_buf);
|
||||||
gps_numSV = UBX_NAV_SOL_numSV(ubx_msg_buf);
|
gps_numSV = UBX_NAV_SOL_numSV(ubx_msg_buf);
|
||||||
gps_week = UBX_NAV_SOL_week(ubx_msg_buf);
|
gps_week = UBX_NAV_SOL_week(ubx_msg_buf);
|
||||||
|
|||||||
@@ -1,211 +0,0 @@
|
|||||||
/*
|
|
||||||
C Datei für die Einbindung eines ArduIMU's
|
|
||||||
Autoren@ZHAW: 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@@ -0,0 +1,218 @@
|
|||||||
|
/*
|
||||||
|
C Datei für die Einbindung eines ArduIMU's
|
||||||
|
Autoren@ZHAW: 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));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
@@ -9,8 +9,8 @@
|
|||||||
|
|
||||||
extern float ArduIMU_data[NB_DATA];
|
extern float ArduIMU_data[NB_DATA];
|
||||||
|
|
||||||
extern float arduimu_roll_neutral;
|
extern float ins_roll_neutral;
|
||||||
extern float arduimu_pitch_neutral;
|
extern float ins_pitch_neutral;
|
||||||
|
|
||||||
//mixer
|
//mixer
|
||||||
extern float pitch_of_throttle_gain;
|
extern float pitch_of_throttle_gain;
|
||||||
Reference in New Issue
Block a user