mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-07 00:53:41 +08:00
Merge CHNI
This commit is contained in:
Christophe De Wagter
@@ -0,0 +1,192 @@
|
||||
<!DOCTYPE airframe SYSTEM "airframe.dtd">
|
||||
|
||||
<!-- Reely Condor
|
||||
TWOG v1.0 Board (http://paparazzi.enac.fr/wiki/TWOG)
|
||||
Sparkfun Razor IMU with Premerlani Code
|
||||
wi.232 Radiomodems
|
||||
-->
|
||||
|
||||
|
||||
<airframe name="Reely Condor CHNI">
|
||||
|
||||
<servos>
|
||||
<servo name="THROTTLE" no="7" min="1200" neutral="1200" max="2000"/>
|
||||
<servo name="AILERON" no="4" min="2000" neutral="1500" max="1000"/>
|
||||
<servo name="ELEVATOR" no="3" max="2000" neutral="1500" min="1000"/>
|
||||
<servo name="CAM" no="2" max="2000" neutral="1500" min="1000"/>
|
||||
<servo name="TILT" no="6" max="2000" neutral="1501" min="1000"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
<axis name="THROTTLE" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<rc_commands>
|
||||
<set command="THROTTLE" value="@THROTTLE"/>
|
||||
<set command="ROLL" value="@ROLL"/>
|
||||
<set command="PITCH" value="@PITCH"/>
|
||||
</rc_commands>
|
||||
|
||||
<command_laws>
|
||||
<set servo="THROTTLE" value="@THROTTLE"/>
|
||||
<set servo="AILERON" value="@ROLL"/>
|
||||
<set servo="ELEVATOR" value="@PITCH"/>
|
||||
</command_laws>
|
||||
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
<define name="MAX_ROLL" value="1.0"/>
|
||||
<define name="MAX_PITCH" value="1.0"/>
|
||||
</section>
|
||||
|
||||
<!-- Configure the pinning of the ADC -->
|
||||
|
||||
<section name="adc" prefix="ADC_CHANNEL_">
|
||||
<define name="IMU_GROLL" value="ADC_0"/>
|
||||
<define name="IMU_GPITCH" value="ADC_1"/>
|
||||
<define name="IMU_GYAW" value="ADC_2"/>
|
||||
<define name="IMU_ACCX" value="ADC_5"/>
|
||||
<define name="IMU_ACCY" value="ADC_6"/>
|
||||
<define name="IMU_ACCZ" value="ADC_7"/>
|
||||
</section>
|
||||
|
||||
<!-- settings for the Analog IMU -->
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
</section>
|
||||
|
||||
<!-- 3S LiPo with 1050mAh, connected to an 20A ESC -->
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="20000" unit="mA" />
|
||||
<define name="VOLTAGE_ADC_A" value="0.02456533604651162791"/>
|
||||
<define name="VOLTAGE_ADC_B" value="0.24024993023255813953"/>
|
||||
<define name="VoltageOfAdc(adc)" value ="(VOLTAGE_ADC_A * adc + VOLTAGE_ADC_B)"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.0" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="9.5" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMU">
|
||||
<define name="YAW_RESPONSE_FACTOR" value="0.7"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="TELEMETRY_MODE_FBW" value="1"/>
|
||||
<define name="NOMINAL_AIRSPEED" value="12.5" 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="ALT_KALMAN_ENABLED" value="TRUE"/>
|
||||
<define name="TRIGGER_DELAY" value="1."/>
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="50."/>
|
||||
</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.07"/>
|
||||
<!-- outer loop saturation -->
|
||||
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
|
||||
|
||||
<!-- auto throttle inner loop -->
|
||||
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.55"/>
|
||||
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.30"/>
|
||||
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.80"/>
|
||||
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
|
||||
<define name="AUTO_THROTTLE_DASH_TRIM" value="-500"/>
|
||||
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="-0.025"/>
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
|
||||
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
|
||||
|
||||
<!-- auto pitch inner loop -->
|
||||
<define name="AUTO_PITCH_PGAIN" value="-0.05"/>
|
||||
<define name="AUTO_PITCH_IGAIN" value="0.075"/>
|
||||
<define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
|
||||
<define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
|
||||
|
||||
<define name="THROTTLE_SLEW" value="0.05"/>
|
||||
|
||||
</section>
|
||||
|
||||
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||
<define name="COURSE_PGAIN" value="-0.85"/>
|
||||
<define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
|
||||
<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="12000."/>
|
||||
<define name="AILERON_OF_THROTTLE" value="0.0"/>
|
||||
<define name="PITCH_PGAIN" value="-9000."/>
|
||||
<define name="PITCH_DGAIN" value="1.5"/>
|
||||
<define name="ELEVATOR_OF_ROLL" value="1250"/>
|
||||
|
||||
</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="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="0.8"/><!-- 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="DELAY_WITHOUT_GPS" value="1" 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>
|
||||
|
||||
<firmware name="fixedwing">
|
||||
<target name="sim" board="pc"/>
|
||||
<target name="ap" board="twog_1.0">
|
||||
<define name="AGR_CLIMB" />
|
||||
<define name="LOITER_TRIM" />
|
||||
<define name="ALT_KALMAN" />
|
||||
</target>
|
||||
<subsystem name="radio_control" type="ppm"/>
|
||||
<!-- Communication -->
|
||||
<subsystem name="telemetry" type="xbee_api">
|
||||
<param name="MODEM_BAUD" value="B9600"/>
|
||||
</subsystem>
|
||||
<!-- Actuators are automatically chosen according to the board-->
|
||||
<subsystem name="control"/>
|
||||
<!-- Sensors -->
|
||||
<subsystem name="attitude" type="analogimu"/>
|
||||
<subsystem name="gps" type="ublox_lea5h"/>
|
||||
<subsystem name="navigation"/>
|
||||
</firmware>
|
||||
<!-- Carefull: add the location after!! -->
|
||||
<makefile location="after">
|
||||
ap.CFLAGS += -DANALOGIMU_ROTATED
|
||||
ap.CFLAGS += -DANALOGIMU_ZERO_AVERAGE
|
||||
|
||||
ap.CFLAGS += -DGPS_USE_LATLONG
|
||||
|
||||
//ap.CFLAGS += -DWIND_INFO -DSTRONG_WIND
|
||||
|
||||
//sim.CFLAGS += -DWIND_INFO -DSTRONG_WIND
|
||||
</makefile>
|
||||
|
||||
</airframe>
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
# attitude via analog imu
|
||||
|
||||
|
||||
ifeq ($(ARCH), lpc21)
|
||||
ap.CFLAGS += -DANALOG_IMU -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -DUSE_ADC_4 -DUSE_ADC_5 -DUSE_ADC_6 -DUSE_ADC_7
|
||||
|
||||
ap.srcs += $(SRC_FIXEDWING)/subsystems/analogimu/dcm.c $(SRC_FIXEDWING)/subsystems/analogimu/arduimu.c $(SRC_FIXEDWING)/subsystems/analogimu/matrix.c $(SRC_FIXEDWING)/subsystems/analogimu/vector.c
|
||||
|
||||
ap.srcs += $(SRC_FIXEDWING)/subsystems/analogimu/analogimu.c $(SRC_FIXEDWING)/subsystems/analogimu/analogimu_util.c
|
||||
endif
|
||||
|
||||
# since there is currently no SITL sim for the Analog IMU, we use the infrared sim
|
||||
|
||||
ifeq ($(TARGET), sim)
|
||||
|
||||
sim.CFLAGS += -DIR_ROLL_NEUTRAL_DEFAULT=0
|
||||
|
||||
sim.CFLAGS += -DIR_PITCH_NEUTRAL_DEFAULT=0
|
||||
|
||||
$(TARGET).CFLAGS += -DUSE_INFRARED
|
||||
$(TARGET).srcs += subsystems/sensors/infrared.c
|
||||
|
||||
sim.srcs += $(SRC_ARCH)/sim_ir.c
|
||||
|
||||
sim.srcs += $(SRC_ARCH)/sim_analogimu.c
|
||||
|
||||
endif
|
||||
|
||||
jsbsim.srcs += $(SRC_ARCH)/jsbsim_ir.c
|
||||
@@ -0,0 +1,117 @@
|
||||
<!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="subsystems/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="ir">
|
||||
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ir_roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ir_pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="ir_lateral_correction" shortname="360_lat_corr" module="infrared" param="IR_LATERAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="ir_longitudinal_correction" shortname="360_log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_vertical_correction" shortname="360_vert_corr" param="IR_VERTICAL_CORRECTION"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_left" shortname="corr_left" param="IR_CORRECTION_LEFT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_right" shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_up" shortname="corr_up" param="IR_CORRECTION_UP"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_down" shortname="corr_down" param="IR_CORRECTION_DOWN"/>
|
||||
|
||||
</dl_settings>-->
|
||||
|
||||
<dl_settings NAME="IMU">
|
||||
<dl_setting MAX="45" MIN="-45" STEP="0.1" VAR="imu_roll_neutral" shortname="roll neutral" module="subsystems/analogimu/analogimu" param="IMU_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
|
||||
<dl_setting MAX="45" MIN="-45" STEP="0.1" VAR="imu_pitch_neutral" shortname="pitch neutral" module="subsystems/analogimu/analogimu" param="IMU_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
|
||||
|
||||
|
||||
<dl_settings NAME="attitude">
|
||||
<dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain" module="stabilization/stabilization_attitude"/>
|
||||
<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="guidance/guidance_v" 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"/>
|
||||
<!-- commented by poine - does anybody use that ?at all ? -->
|
||||
<!-- <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" param="H_CTL_COURSE_DGAIN"/>
|
||||
<dl_setting MAX="2" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pre_bank_correction" shortname="pre bank cor" param="H_CTL_COURSE_PRE_BANK_CORRECTION"/>
|
||||
<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="subsystems/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>
|
||||
@@ -0,0 +1,15 @@
|
||||
/** \file sim_ir.c
|
||||
* \brief Regroup functions to simulate autopilot/infrared.c
|
||||
*
|
||||
* Infrared soft simulation. OCaml binding.
|
||||
*/
|
||||
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "subsystems/sensors/infrared.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
#include <caml/mlvalues.h>
|
||||
|
||||
float imu_roll_neutral = RadOfDeg(IMU_ROLL_NEUTRAL_DEFAULT);
|
||||
float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
|
||||
@@ -65,6 +65,10 @@
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef ANALOG_IMU
|
||||
#include "subsystems/analogimu/analogimu.h"
|
||||
#endif
|
||||
|
||||
#if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY
|
||||
#warning "LOW_BATTERY deprecated. Renamed into CATASTROPHIC_BAT_LEVEL (in airframe file)"
|
||||
#define CATASTROPHIC_BAT_LEVEL LOW_BATTERY
|
||||
@@ -431,6 +435,13 @@ void periodic_task_ap( void ) {
|
||||
#error "Only 20 and 60 allowed for CONTROL_RATE"
|
||||
#endif
|
||||
|
||||
#ifdef ANALOG_IMU
|
||||
if (!_20Hz) {
|
||||
estimator_update_state_analog_imu();
|
||||
analog_imu_downlink();
|
||||
}
|
||||
#endif // ANALOG_IMU
|
||||
|
||||
#if CONTROL_RATE == 20
|
||||
if (!_20Hz)
|
||||
#endif
|
||||
@@ -485,6 +496,10 @@ void init_ap( void ) {
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef ANALOG_IMU
|
||||
analog_imu_init();
|
||||
#endif
|
||||
|
||||
/************* Links initialization ***************/
|
||||
#if defined MCU_SPI_LINK
|
||||
link_mcu_init();
|
||||
@@ -534,6 +549,11 @@ void init_ap( void ) {
|
||||
traffic_info_init();
|
||||
#endif
|
||||
|
||||
#ifdef ANALOG_IMU
|
||||
//wait 10secs for init
|
||||
sys_time_usleep(10000000);
|
||||
analog_imu_offset_set();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,257 @@
|
||||
/*
|
||||
* $Id: analogimu.c $
|
||||
*
|
||||
* Copyright (C) 2010 Oliver Riesener, Christoph Niemann
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
/** \file analogimu.c
|
||||
* \brief Analog IMU Routines
|
||||
*
|
||||
*/
|
||||
#if ! (defined SITL || defined HITL)
|
||||
|
||||
#include "led.h"
|
||||
#include "mcu_periph/adc.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
//#include "downlink.h"
|
||||
#include "estimator.h"
|
||||
//#include "ap_downlink.h"
|
||||
#include "sys_time.h"
|
||||
|
||||
#include "dcm.h"
|
||||
|
||||
#include "analogimu_util.h"
|
||||
|
||||
#include "analogimu.h"
|
||||
|
||||
#endif
|
||||
|
||||
#define NB_ADC 8
|
||||
#define ADC_NB_SAMPLES 16
|
||||
|
||||
// variables
|
||||
|
||||
uint16_t analog_imu_offset[NB_ADC] = {0,};
|
||||
|
||||
static struct adc_buf buf_adc[NB_ADC];
|
||||
int adc_average[16] = { 0 };
|
||||
|
||||
float imu_roll_neutral = RadOfDeg(IMU_ROLL_NEUTRAL_DEFAULT);
|
||||
float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
|
||||
|
||||
#if ! (defined SITL || defined HITL)
|
||||
|
||||
// functions
|
||||
/**
|
||||
* accel2ms2():
|
||||
*
|
||||
* \return accel[ACC_X], accel[ACC_Y], accel[ACC_Z]
|
||||
*/
|
||||
void accel2ms2( void ) {
|
||||
accel[ACC_X] = (float)(adc_average[ADC_ACCX])/10.19;
|
||||
accel[ACC_Y] = (float)(-adc_average[ADC_ACCY])/10.5;
|
||||
accel[ACC_Z] = (float)(adc_average[ADC_ACCZ])/10.4;//chni: needs to be adjusted for earth gravity
|
||||
}
|
||||
/**
|
||||
* gyro2rads():
|
||||
*
|
||||
* \return gyro[G_ROLL], gyro[G_PITCH], gyro[G_YAW]
|
||||
*/
|
||||
void gyro2rads( void ) {
|
||||
/** 150 grad/sec 10Bit, 3,3Volt, 1rad = 2Pi/1024 => Pi/512 */
|
||||
gyro[G_ROLL] = (float)(adc_average[ADC_ROLL]) / 61.3588;
|
||||
gyro[G_PITCH] = (float)(adc_average[ADC_PITCH]) / 57.96;
|
||||
gyro[G_YAW] = (float)(-adc_average[ADC_YAW]) / 60.1;
|
||||
}
|
||||
|
||||
void analog_imu_init( void ) {
|
||||
adc_buf_channel(ADC_CHANNEL_IMU_GROLL, &buf_adc[0], ADC_NB_SAMPLES);
|
||||
adc_buf_channel(ADC_CHANNEL_IMU_GPITCH, &buf_adc[1], ADC_NB_SAMPLES);
|
||||
adc_buf_channel(ADC_CHANNEL_IMU_GYAW, &buf_adc[2], ADC_NB_SAMPLES);
|
||||
adc_buf_channel(ADC_CHANNEL_IMU_ACCX, &buf_adc[5], ADC_NB_SAMPLES);
|
||||
adc_buf_channel(ADC_CHANNEL_IMU_ACCY, &buf_adc[6], ADC_NB_SAMPLES);
|
||||
adc_buf_channel(ADC_CHANNEL_IMU_ACCZ, &buf_adc[7], ADC_NB_SAMPLES);
|
||||
|
||||
#if NB_ADC != 8
|
||||
#error "8 ADCs expected !"
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
void analog_imu_offset_set( void ) {
|
||||
uint8_t i;
|
||||
for(i = 0; i < NB_ADC - 1; i++) {
|
||||
analog_raw[i] = buf_adc[i].sum / ADC_NB_SAMPLES;
|
||||
analog_imu_offset[i] = analog_raw[i];
|
||||
}
|
||||
analog_imu_offset[7] = analog_raw[7] + 528;// 553; // + Zero of z-acc (without gravity) needs to be adjusted
|
||||
}
|
||||
/**
|
||||
* analog_imu_update():
|
||||
*/
|
||||
void analog_imu_update( void ) {
|
||||
uint8_t i;
|
||||
for(i = 0; i < NB_ADC; i++) {
|
||||
analog_raw[i] = buf_adc[i].sum / ADC_NB_SAMPLES;
|
||||
}
|
||||
adc_average[ADC_ROLL] = analog_raw[0] - analog_imu_offset[0];
|
||||
adc_average[ADC_PITCH] = analog_raw[1] - analog_imu_offset[1];
|
||||
adc_average[ADC_YAW] = analog_raw[2] - analog_imu_offset[2];
|
||||
adc_average[ADC_ACCX] = analog_raw[5] - analog_imu_offset[5];
|
||||
adc_average[ADC_ACCY] = analog_raw[6] - analog_imu_offset[6];
|
||||
adc_average[ADC_ACCZ] = analog_raw[7] - analog_imu_offset[7];
|
||||
accel2ms2();
|
||||
gyro2rads();
|
||||
}
|
||||
|
||||
/** earth accelecation */
|
||||
volatile float g = 0.;
|
||||
|
||||
// functions
|
||||
|
||||
void analog_imu_downlink( void ) {
|
||||
//uint8_t id = 0;
|
||||
//float time = GET_CUR_TIME_FLOAT();
|
||||
//time *= 1000;//secs to msecs
|
||||
//int mx = 0;
|
||||
//int my = 0;
|
||||
//int mz = 0;
|
||||
//DOWNLINK_SEND_HB_FILTER( DefaultChannel,&time, &accel[ACC_X],&accel[ACC_Y],&accel[ACC_Z],&gyro[G_ROLL],&gyro[G_PITCH],&gyro[G_YAW],&heading,&mx,&my,&mz,&euler[EULER_ROLL],&euler[EULER_PITCH],&euler[EULER_YAW], &imu_roll_neutral, &imu_pitch_neutral );
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* matrix transpose is safe to call with c == a if m = n
|
||||
*
|
||||
* \param [out] *c pointer to destination array
|
||||
* \param [in] *a pointer to source array
|
||||
* \param [in] m dimention rows
|
||||
* \param [in] n dimention cols
|
||||
* \return result is in *c, or error if a==c and m != n
|
||||
*/
|
||||
void matrix_transpose(float *c, float *a, short m, short n)
|
||||
{
|
||||
if ( a != c ) { // quick version
|
||||
short i,j;
|
||||
// [i][j] ... [m][n]
|
||||
for ( i=0 ; i<m ; i++ ) {
|
||||
for( j=0 ; j<n ; j++ ) {
|
||||
// c[j][i] = a[i][j];
|
||||
*(c+(m*j)+i) = *(a+(n*i)+j);
|
||||
}
|
||||
}
|
||||
} else if ( m == n) { // save version
|
||||
short i,j;
|
||||
// [i][j] ... [m][n]
|
||||
for ( i=0 ; i<m ; i++ ) {
|
||||
for( j=0 ; j<n ; j++ ) {
|
||||
/* c[j][i] = a[i][j]; */
|
||||
float vc = *(c+(m*j)+i);
|
||||
float va = *(a+(n*i)+j);
|
||||
*(c+(m*j)+i) = va;
|
||||
*(a+(n*i)+j) = vc;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// error illegal !!
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Minimalistic version to get angles from acceleration
|
||||
*
|
||||
* \todo why has this function 3 callers ?
|
||||
* \return g, angle[ANG_ROLL], angle[ANG_PITCH]
|
||||
*/
|
||||
void accel2euler( void ) {
|
||||
// Calculate g ( ||g_vec|| )
|
||||
g = sqrt(accel[ACC_X] * accel[ACC_X] +
|
||||
accel[ACC_Y] * accel[ACC_Y] +
|
||||
accel[ACC_Z] * accel[ACC_Z]);
|
||||
if( g < 0.01 && g > -0.01 )
|
||||
{
|
||||
g=0.01;
|
||||
}else{
|
||||
//nothing
|
||||
}
|
||||
//values in radians
|
||||
#define NEW
|
||||
#ifdef OLD
|
||||
angle[ANG_PITCH] = -asinf( accel[ACC_X] / g );
|
||||
angle[ANG_ROLL] = asinf( accel[ACC_Y] / g );
|
||||
angle[ANG_YAW] = 0.0;
|
||||
#endif
|
||||
|
||||
#ifdef NEW
|
||||
|
||||
float a1 = accel[ACC_X] / -g;
|
||||
|
||||
if(a1 > 1.0 && a1 >= 0.0){
|
||||
a1 = 1.0;
|
||||
} else if(a1 < -1.0 && a1 < 0.0){
|
||||
a1 = -1.0;
|
||||
}
|
||||
|
||||
angle[ANG_PITCH] = asinf( a1 );
|
||||
|
||||
if(accel[ACC_Z] < 0 && angle[ANG_PITCH] > 0) angle[ANG_PITCH] = + 3.145/2 + (3.145/2 - angle[ANG_PITCH]);
|
||||
else if (accel[ACC_Z] < 0 && angle[ANG_PITCH] < 0) angle[ANG_PITCH] = -3.145/2 - (3.145/2 + angle[ANG_PITCH]);
|
||||
|
||||
|
||||
if( accel[ACC_Z] < 0.01 && accel[ACC_Z] > -0.01 )
|
||||
{
|
||||
accel[ACC_Z]=0.01;
|
||||
}else{
|
||||
//nothing
|
||||
}
|
||||
|
||||
|
||||
angle[ANG_ROLL] = atan2f( accel[ACC_Y], accel[ACC_Z] );
|
||||
//angle[ANG_PITCH] = -atan2f( accel[ACC_X] , accel[ACC_Z] );
|
||||
angle[ANG_YAW] = 0.0;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void estimator_update_state_analog_imu( void ) {
|
||||
#undef ANGLE_FROM_ACCEL
|
||||
#ifdef ANGLE_FROM_ACCEL
|
||||
estimator_phi = (float)(atan2f((float)((analog_raw[6]-510)),(float)(-(analog_raw[7]-510))));
|
||||
estimator_theta = (float)(atan2f((float)(-(analog_raw[5]-510)),(float)(-(analog_raw[7]-510))));
|
||||
#else
|
||||
|
||||
analog_imu_update();
|
||||
|
||||
Matrix_update();
|
||||
Normalize();
|
||||
Drift_correction();
|
||||
Euler_angles();
|
||||
|
||||
// return euler angles to phi and theta
|
||||
estimator_phi = euler[EULER_ROLL]-imu_roll_neutral;
|
||||
//estimator_phi = angle[ANG_ROLL];
|
||||
estimator_theta = euler[EULER_PITCH]-imu_pitch_neutral;
|
||||
//estimator_theta = angle[ANG_PITCH];
|
||||
estimator_psi = euler[EULER_YAW];
|
||||
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,67 @@
|
||||
/*
|
||||
* $Id: analogimu.h $
|
||||
*
|
||||
* Copyright (C) 2010 Oliver Riesener, Christoph Niemann
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
/** \file analogimu.h
|
||||
* \brief Analog IMU Interface
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef _ANALOGIMU_H_
|
||||
#define _ANALOGIMU_H_
|
||||
// types
|
||||
#define uint16_t short int
|
||||
|
||||
#include "std.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
extern float imu_roll_neutral;
|
||||
extern float imu_pitch_neutral;
|
||||
|
||||
|
||||
// ADC0 Slots
|
||||
#define ADC_YAW 1
|
||||
#define ADC_PITCH 2
|
||||
#define ADC_ROLL 3
|
||||
#define ADC_OFF 4
|
||||
|
||||
// ADC1 Slots
|
||||
#define ADC_ACCX (3+8)
|
||||
#define ADC_ACCY (4+8)
|
||||
#define ADC_ACCZ (5+8)
|
||||
#define ADC_VBAT (6+8)
|
||||
#define ADC_ALT (7+8)
|
||||
|
||||
//functions
|
||||
void analog_imu_init( void );
|
||||
void analog_imu_update( void );
|
||||
void analog_imu_downlink( void );
|
||||
void analogconversion( void );
|
||||
void matrix_transpose(float *c, float *a, short m, short n);
|
||||
void accel2euler( void );
|
||||
void accel2ms2( void );
|
||||
void estimator_update_state_analog_imu( void );
|
||||
void gyro2rads( void );
|
||||
void analog_imu_offset_set( void );
|
||||
|
||||
#endif // _ANALOGIMU_H_
|
||||
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* $Id: analogimu_util.c $
|
||||
*
|
||||
* Copyright (C) 2010 Christoph Niemann
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
/** \file analogimu_util.c
|
||||
* \brief Analog IMU Utilities
|
||||
*
|
||||
*/
|
||||
|
||||
#include "mcu_periph/adc.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
|
||||
#include "std.h"
|
||||
#include "subsystems/nav.h"
|
||||
#include "estimator.h"
|
||||
#include "autopilot.h"
|
||||
#include "generated/flight_plan.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "inter_mcu.h"
|
||||
|
||||
#include "analogimu_util.h"
|
||||
|
||||
|
||||
uint16_t analog_raw[NB_ADC];
|
||||
|
||||
// variables
|
||||
/** gyro rate in rad */
|
||||
volatile float gyro[G_LAST] = {0.};
|
||||
/** gyro rate offset in rad */
|
||||
volatile float gyro_to_zero[G_LAST] = {0.};
|
||||
/** acceleration in ms2 */
|
||||
volatile float accel[ACC_LAST] = {0.};
|
||||
/** angle in rad */
|
||||
volatile float angle[ANG_LAST] = {0.};
|
||||
/** magnet heading \todo heading ? */
|
||||
volatile float heading;
|
||||
|
||||
void analogimu_delay( void ) {
|
||||
volatile int i,j;
|
||||
for (i=0;i<1000;i++)
|
||||
for (j=0;j<1000;j++);
|
||||
}
|
||||
|
||||
bool_t analog_imu_reset( void ) {
|
||||
#ifndef ANALOGIMU_ZERO_AVERAGE
|
||||
gyro_to_zero[GO_ROLL] = gyro[G_ROLL];
|
||||
gyro_to_zero[GO_PITCH] = gyro[G_PITCH];
|
||||
gyro_to_zero[GO_YAW] = gyro[G_YAW];
|
||||
#else
|
||||
/** temp_value for calculating the average */
|
||||
volatile float gyro_to_zero_temp[G_LAST] = {0.};
|
||||
|
||||
gyro_to_zero_temp[GO_ROLL] = gyro[G_ROLL];
|
||||
gyro_to_zero_temp[GO_PITCH] = gyro[G_PITCH];
|
||||
gyro_to_zero_temp[GO_YAW] = gyro[G_YAW];
|
||||
|
||||
analogimu_delay();
|
||||
|
||||
gyro_to_zero_temp[GO_ROLL] += gyro[G_ROLL];
|
||||
gyro_to_zero_temp[GO_PITCH] += gyro[G_PITCH];
|
||||
gyro_to_zero_temp[GO_YAW] += gyro[G_YAW];
|
||||
|
||||
analogimu_delay();
|
||||
|
||||
gyro_to_zero_temp[GO_ROLL] += gyro[G_ROLL];
|
||||
gyro_to_zero_temp[GO_PITCH] += gyro[G_PITCH];
|
||||
gyro_to_zero_temp[GO_YAW] += gyro[G_YAW];
|
||||
|
||||
gyro_to_zero_temp[GO_ROLL] /= 3;
|
||||
gyro_to_zero_temp[GO_PITCH] /= 3;
|
||||
gyro_to_zero_temp[GO_YAW] /= 3;
|
||||
|
||||
gyro_to_zero[GO_ROLL] = gyro_to_zero_temp[G_ROLL];
|
||||
gyro_to_zero[GO_PITCH] = gyro_to_zero_temp[G_PITCH];
|
||||
gyro_to_zero[GO_YAW] = gyro_to_zero_temp[G_YAW];
|
||||
#endif
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,66 @@
|
||||
/*
|
||||
* $Id: analogimu_imu.h $
|
||||
*
|
||||
* Copyright (C) 2010 Christoph Niemann
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
/** \file analogimu_util.h
|
||||
* \brief Analog IMU Utilities
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef _ANALOGIMU_UTIL_H_
|
||||
#define _ANALOGIMU_UTIL_H_
|
||||
|
||||
#include "std.h"
|
||||
|
||||
// defines
|
||||
/** defines for gyro[] indicies */
|
||||
enum gyro_idx_t { G_ROLL, G_PITCH, G_YAW, G_LAST };
|
||||
/** defines for accel[] indicies */
|
||||
enum accel_idx_t { ACC_X, ACC_Y, ACC_Z, ACC_LAST };
|
||||
/** defines for stick[] indicies */
|
||||
enum stick_idx_t { STICK_ROLL, STICK_PITCH, STICK_YAW, STICK_THRUST,
|
||||
STICK_SWR, STICK_SWL, STICK_CH6, STICK_CH7, STICK_LAST };
|
||||
/** defines for angle[] indicies */
|
||||
enum angle_idx_t { ANG_ROLL, ANG_PITCH, ANG_YAW, ANG_LAST };
|
||||
/** defines for gyro_to_zero[] indicies */
|
||||
enum gyro_to_zero_idx_t { GO_ROLL, GO_PITCH, GO_YAW, GO_LAST };
|
||||
|
||||
// variables
|
||||
extern volatile float gyro[];
|
||||
/** acceleration in ms2 */
|
||||
extern volatile float accel[];
|
||||
/** angle in rad */
|
||||
extern volatile float angle[];
|
||||
/** magnet heading \todo heading ? */
|
||||
extern volatile float heading;
|
||||
/** analog_raw[] analog downlink arry */
|
||||
extern uint16_t analog_raw[];
|
||||
|
||||
extern volatile float gyro_to_zero[];
|
||||
|
||||
extern bool_t analog_imu_reset( void );
|
||||
|
||||
void analogimu_delay( void );
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,282 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "wiring.h"
|
||||
#include "vector.h"
|
||||
#include "matrix.h"
|
||||
#include "read_adc.h"
|
||||
#include "arduimu.h"
|
||||
|
||||
#include "dcm.h"
|
||||
|
||||
// Released under Creative Commons License
|
||||
// Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun).
|
||||
// Version 1.0 for flat board updated by Doug Weibel and Jose Julio
|
||||
// Version 1.7 includes support for SCP1000 absolute pressure sensor
|
||||
|
||||
// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
|
||||
// Positive pitch : nose up
|
||||
// Positive roll : right wing down
|
||||
// Positive yaw : clockwise
|
||||
|
||||
// olri #include <avr/eeprom.h>
|
||||
// olri #include <Wire.h>
|
||||
|
||||
//**********************************************************************
|
||||
// This section contains USER PARAMETERS !!!
|
||||
//
|
||||
//**********************************************************************
|
||||
|
||||
// *** NOTE! Hardware version - Can be used for v1 (daughterboards) or v2 (flat)
|
||||
#define BOARD_VERSION 2 // 1 For V1 and 2 for V2
|
||||
|
||||
// Ublox gps is recommended!
|
||||
#define GPS_PROTOCOL 1 // 1 - Ublox, 2 - EM406, 3 - NMEA We have only tested with Ublox
|
||||
|
||||
// Enable Air Start uses Remove Before Fly flag - connection to pin 6 on ArduPilot
|
||||
#define ENABLE_AIR_START 1 // 1 if using airstart/groundstart signaling, 0 if not
|
||||
#define GROUNDSTART_PIN 8 // Pin number used for ground start signal (recommend 10 on v1 and 8 on v2 hardware)
|
||||
|
||||
/*Min Speed Filter for Yaw drift Correction*/
|
||||
#define SPEEDFILT 2 // >1 use min speed filter for yaw drift cancellation, 0=do not use speed filter
|
||||
|
||||
/*For debugging propurses*/
|
||||
#define PRINT_DEBUG 0 //Will print Debug messages
|
||||
|
||||
//OUTPUTMODE=1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only data
|
||||
#define OUTPUTMODE 1
|
||||
|
||||
#define PRINT_DCM 0 //Will print the whole direction cosine matrix
|
||||
#define PRINT_ANALOGS 0 //Will print the analog raw data
|
||||
#define PRINT_EULER 0 //Will print the Euler angles Roll, Pitch and Yaw
|
||||
#define PRINT_GPS 1 //Will print GPS data
|
||||
|
||||
// *** NOTE! To use ArduIMU with ArduPilot you must select binary output messages
|
||||
#define PRINT_BINARY 1 //Will print binary message and suppress ASCII messages (above)
|
||||
|
||||
// *** NOTE! Performance reporting is only supported for Ublox. Set to 0 for others
|
||||
#define PERFORMANCE_REPORTING 1 //Will include performance reports in the binary output ~ 1/2 min
|
||||
|
||||
/* Support for optional magnetometer (1 enabled, 0 dissabled) */
|
||||
#define USE_MAGNETOMETER 0 // use 1 if you want to make yaw gyro drift corrections using the optional magnetometer
|
||||
|
||||
/* Support for optional barometer (1 enabled, 0 dissabled) */
|
||||
#define USE_BAROMETER 0 // use 1 if you want to get altitude using the optional absolute pressure sensor
|
||||
#define ALT_MIX 50 // For binary messages: GPS or barometric altitude. 0 to 100 = % of barometric. For example 75 gives 25% GPS alt and 75% baro
|
||||
|
||||
//**********************************************************************
|
||||
// End of user parameters
|
||||
//**********************************************************************
|
||||
|
||||
#define SOFTWARE_VER "1.7"
|
||||
|
||||
// ADC : Voltage reference 3.3v / 10bits(1024 steps) => 3.22mV/ADC step
|
||||
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 3.22mV/ADC step => 330/3.22 = 102.48
|
||||
// Tested value : 101
|
||||
//#define GRAVITY 101 //this equivalent to 1G in the raw data coming from the accelerometer
|
||||
//#define GRAVITY 9.81
|
||||
|
||||
// olri #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
|
||||
|
||||
// olri #define ToRad(x) (x*0.01745329252) // *pi/180
|
||||
// olri #define ToDeg(x) (x*57.2957795131) // *180/pi
|
||||
|
||||
// LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/º/s, 3.22mV/ADC step => 1.03
|
||||
// Tested values : 0.96,0.96,0.94
|
||||
// #define Gyro_Gain_X 0.92 //X axis Gyro gain
|
||||
// #define Gyro_Gain_Y 0.92 //Y axis Gyro gain
|
||||
// #define Gyro_Gain_Z 0.94 //Z axis Gyro gain
|
||||
// olri #define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
// olri #define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
// olri #define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
|
||||
//olri #define Kp_ROLLPITCH 0.015
|
||||
// olri #define Ki_ROLLPITCH 0.000010
|
||||
// olri #define Kp_YAW 1.2
|
||||
//#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution!
|
||||
// olri #define Ki_YAW 0.00005
|
||||
|
||||
/*UBLOX Maximum payload length*/
|
||||
#define UBX_MAXPAYLOAD 56
|
||||
|
||||
#define ADC_WARM_CYCLES 75
|
||||
|
||||
//olri #define FALSE 0
|
||||
// olri #define TRUE 1
|
||||
|
||||
|
||||
//float G_Dt=0.02; // Integration time (DCM algorithm)
|
||||
//chni: changed to 50ms, according to HSB-Data
|
||||
float G_Dt=0.05;
|
||||
|
||||
long timeNow=0; // Hold the milliseond value for now
|
||||
long timer=0; //general purpuse timer
|
||||
long timer_old;
|
||||
long timer24=0; //Second timer used to print values
|
||||
boolean groundstartDone = false; // Used to not repeat ground start
|
||||
|
||||
float AN[8]; //array that store the 6 ADC filtered data
|
||||
float AN_OFFSET[8]; //Array that stores the Offset of the gyros
|
||||
|
||||
float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
|
||||
float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
|
||||
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
|
||||
float Omega_P[3]= {0,0,0};//Omega Proportional correction
|
||||
float Omega_I[3]= {0,0,0};//Omega Integrator
|
||||
float Omega[3]= {0,0,0};
|
||||
|
||||
//Magnetometer variables
|
||||
int magnetom_x;
|
||||
int magnetom_y;
|
||||
int magnetom_z;
|
||||
float MAG_Heading;
|
||||
|
||||
// Euler angles
|
||||
float roll;
|
||||
float pitch;
|
||||
float yaw;
|
||||
|
||||
float errorRollPitch[3]= {0,0,0};
|
||||
float errorYaw[3]= {0,0,0};
|
||||
float errorCourse=180;
|
||||
float COGX=0; //Course overground X axis
|
||||
float COGY=1; //Course overground Y axis
|
||||
|
||||
unsigned int cycleCount=0;
|
||||
byte gyro_sat=0;
|
||||
|
||||
float DCM_Matrix[3][3]= {
|
||||
{
|
||||
1,0,0 }
|
||||
,{
|
||||
0,1,0 }
|
||||
,{
|
||||
0,0,1 }
|
||||
};
|
||||
float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
|
||||
|
||||
|
||||
float Temporary_Matrix[3][3]={
|
||||
{
|
||||
0,0,0 }
|
||||
,{
|
||||
0,0,0 }
|
||||
,{
|
||||
0,0,0 }
|
||||
};
|
||||
|
||||
//GPS
|
||||
|
||||
//GPS stuff
|
||||
union long_union {
|
||||
int32_t dword;
|
||||
uint8_t byte[4];
|
||||
} longUnion;
|
||||
|
||||
union int_union {
|
||||
int16_t word;
|
||||
uint8_t byte[2];
|
||||
} intUnion;
|
||||
|
||||
/*Flight GPS variables*/
|
||||
int gpsFix=1; //This variable store the status of the GPS
|
||||
int gpsFixnew=0; //used to flag when new gps data received - used for binary output message flags
|
||||
int gps_fix_count = 5; //used to count 5 good fixes at ground startup
|
||||
long lat=0; // store the Latitude from the gps to pass to output
|
||||
long lon=0; // Store the Longitude from the gps to pass to output
|
||||
long alt_MSL=0; //This is the altitude in millimeters
|
||||
long iTOW=0; //GPS Millisecond Time of Week
|
||||
long alt=0; //Height above Ellipsoid in millimeters
|
||||
float speed_3d=0; //Speed (3-D)
|
||||
float ground_speed=0;// This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots
|
||||
float ground_course=90;//This is the runaway direction of you "plane" in degrees
|
||||
float gc_offset = 0; // Force yaw output to ground course when fresh data available (only implemented for ublox&binary message)
|
||||
byte numSV=0; //Number of Sats used.
|
||||
float ecefVZ=0; //Vertical Speed in m/s
|
||||
unsigned long GPS_timer=0;
|
||||
|
||||
#if GPS_PROTOCOL == 1
|
||||
// GPS UBLOX
|
||||
//byte ck_a=0; // Packet checksum
|
||||
//byte ck_b=0;
|
||||
byte UBX_step=0;
|
||||
byte UBX_class=0;
|
||||
byte UBX_id=0;
|
||||
byte UBX_payload_length_hi=0;
|
||||
byte UBX_payload_length_lo=0;
|
||||
byte UBX_payload_counter=0;
|
||||
byte UBX_buffer[UBX_MAXPAYLOAD];
|
||||
byte UBX_ck_a=0;
|
||||
byte UBX_ck_b=0;
|
||||
#endif
|
||||
|
||||
//ADC variables
|
||||
volatile uint8_t MuxSel=0;
|
||||
volatile uint8_t analog_reference = DEFAULT;
|
||||
volatile uint16_t analog_buffer[8];
|
||||
volatile uint8_t analog_count[8];
|
||||
|
||||
|
||||
#if BOARD_VERSION == 1
|
||||
uint8_t sensors[6] = {0,2,1,3,5,4}; // Use these two lines for Hardware v1 (w/ daughterboards)
|
||||
int SENSOR_SIGN[]= {1,-1,1,-1,1,-1,-1,-1,-1}; //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
|
||||
#endif
|
||||
|
||||
#if BOARD_VERSION == 2
|
||||
uint8_t sensors[6] = {6,7,3,0,1,2}; // For Hardware v2 flat
|
||||
int SENSOR_SIGN[] = {1,-1,-1,1,-1,1,-1,-1,-1};
|
||||
#endif
|
||||
|
||||
// Performance Monitoring variables
|
||||
// Data collected and reported for ~1/2 minute intervals
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
int mainLoop_count = 0; //Main loop cycles since last report
|
||||
int G_Dt_max = 0.0; //Max main loop cycle time in milliseconds
|
||||
byte gyro_sat_count = 0;
|
||||
byte adc_constraints = 0;
|
||||
byte renorm_sqrt_count = 0;
|
||||
byte renorm_blowup_count = 0;
|
||||
byte gps_payload_error_count = 0;
|
||||
byte gps_checksum_error_count = 0;
|
||||
byte gps_pos_fix_count = 0;
|
||||
byte gps_nav_fix_count = 0;
|
||||
byte gps_messages_sent = 0;
|
||||
long perf_mon_timer = 0;
|
||||
#endif
|
||||
unsigned int imu_health = 65012;
|
||||
|
||||
//**********************************************************************
|
||||
// This section contains SCP1000_D11 PARAMETERS !!!
|
||||
//**********************************************************************
|
||||
#if USE_BAROMETER == 1
|
||||
#define SCP_MODE (9) // 9 = high speed mode, 10 = high resolution mode
|
||||
#define PRESSURE_ADDR (0x11U) // IIC address of the SCP1000
|
||||
// ************ #define START_ALTITUDE (217U) // default initial altitude in m above sea level
|
||||
|
||||
// When we have to manage data transfers via IIC directly we need to use the following addresses
|
||||
// IIC address of the SCP1000 device forms the Top 7 bits of the address with the R/W bit as the LSB
|
||||
#define READ_PRESSURE_ADDR (PRESSURE_ADDR<<1 | 1)
|
||||
#define WRITE_PRESSURE_ADDR (PRESSURE_ADDR<<1)
|
||||
|
||||
// SCP1000 Register addresses
|
||||
#define SNS_ADDR_POPERATION (0x03U) // OPERATON register
|
||||
#define SNS_ADDR_PSTATUS (0x07U) // STATUS register
|
||||
#define SNS_ADDR_PPRESSURE (0x80U) // DATARD16 Register (pressure)
|
||||
#define SNS_ADDR_DATARD8 (0x7FU) // DAYARD8 Register
|
||||
#define SNS_ADDR_PTEMP (0x81U) // TEMPOUT Register (temperature)
|
||||
|
||||
#ifndef TRUE
|
||||
#define TRUE (0x01)
|
||||
#endif
|
||||
#ifndef FALSE
|
||||
#define FALSE (0x00)
|
||||
#endif
|
||||
|
||||
int temp_unfilt = 0;
|
||||
int temperature = 0;
|
||||
unsigned long press = 0;
|
||||
unsigned long press_filt = 0;
|
||||
unsigned long press_gnd = 0;
|
||||
long ground_alt = 0; // Ground altitude in millimeters
|
||||
long press_alt = 0; // Pressure altitude in millimeters
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,130 @@
|
||||
#include "wiring.h"
|
||||
/* This file was automatically generated. Do not edit! */
|
||||
void debug_print(char string[]);
|
||||
void startup_air(void);
|
||||
void debug_handler(byte message);
|
||||
void startup_ground(void);
|
||||
//void loop();
|
||||
#if USE_BAROMETER == 1
|
||||
extern long press_alt;
|
||||
extern long ground_alt;
|
||||
extern unsigned long press_gnd;
|
||||
extern unsigned long press_filt;
|
||||
extern unsigned long press;
|
||||
extern int temperature;
|
||||
extern int temp_unfilt;
|
||||
#endif
|
||||
extern unsigned int imu_health;
|
||||
extern long perf_mon_timer;
|
||||
extern byte gps_messages_sent;
|
||||
extern byte gps_nav_fix_count;
|
||||
extern byte gps_pos_fix_count;
|
||||
extern byte gps_checksum_error_count;
|
||||
extern byte gps_payload_error_count;
|
||||
extern byte renorm_blowup_count;
|
||||
extern byte renorm_sqrt_count;
|
||||
extern byte adc_constraints;
|
||||
extern byte gyro_sat_count;
|
||||
extern int G_Dt_max;
|
||||
extern int SENSOR_SIGN[];
|
||||
extern volatile uint8_t analog_count[8];
|
||||
extern volatile uint16_t analog_buffer[8];
|
||||
extern volatile uint8_t analog_reference;
|
||||
extern volatile uint8_t MuxSel;
|
||||
#if GPS_PROTOCOL == 1
|
||||
extern byte UBX_ck_b;
|
||||
extern byte UBX_ck_a;
|
||||
extern byte UBX_buffer[UBX_MAXPAYLOAD];
|
||||
extern byte UBX_payload_counter;
|
||||
extern byte UBX_payload_length_lo;
|
||||
extern byte UBX_payload_length_hi;
|
||||
extern byte UBX_id;
|
||||
extern byte UBX_class;
|
||||
extern byte UBX_step;
|
||||
extern byte ck_b;
|
||||
extern byte ck_a;
|
||||
#endif
|
||||
extern unsigned long GPS_timer;
|
||||
extern float ecefVZ;
|
||||
extern byte numSV;
|
||||
extern float gc_offset;
|
||||
extern float ground_course;
|
||||
extern float ground_speed;
|
||||
extern float speed_3d;
|
||||
extern long alt;
|
||||
extern long iTOW;
|
||||
extern long alt_MSL;
|
||||
extern long lon;
|
||||
extern long lat;
|
||||
extern int gps_fix_count;
|
||||
extern int gpsFixnew;
|
||||
extern int gpsFix;
|
||||
extern float Temporary_Matrix[3][3];
|
||||
extern float Update_Matrix[3][3];
|
||||
extern float DCM_Matrix[3][3];
|
||||
extern byte gyro_sat;
|
||||
extern unsigned int cycleCount;
|
||||
extern float COGY;
|
||||
extern float COGX;
|
||||
extern float errorCourse;
|
||||
extern float errorYaw[3];
|
||||
extern float errorRollPitch[3];
|
||||
extern float yaw;
|
||||
extern float pitch;
|
||||
extern float roll;
|
||||
extern float MAG_Heading;
|
||||
extern int magnetom_z;
|
||||
extern int magnetom_y;
|
||||
extern int magnetom_x;
|
||||
extern float Omega[3];
|
||||
extern float Omega_I[3];
|
||||
extern float Omega_P[3];
|
||||
extern float Omega_Vector[3];
|
||||
extern float Gyro_Vector[3];
|
||||
extern float Accel_Vector[3];
|
||||
extern float AN_OFFSET[8];
|
||||
extern float AN[8];
|
||||
extern boolean groundstartDone;
|
||||
extern long timer24;
|
||||
extern long timer_old;
|
||||
extern long timer;
|
||||
extern long timeNow;
|
||||
extern float G_Dt;
|
||||
#define FALSE 0
|
||||
#define TRUE 1
|
||||
//#define GRAVITY 101 //this equivalent to 1G in the raw data coming from the accelerometer
|
||||
//chni:
|
||||
#define GRAVITY 9.9074 // typical 9.81 sensor depend, hier Z Sensor Wert unserer IMU in Normallage
|
||||
|
||||
//#define Kp_ROLLPITCH 0.2
|
||||
#define Kp_ROLLPITCH 0.015
|
||||
//#define Kp_YAW 1.2
|
||||
#define Kp_YAW 1.2 //High yaw drift correction gain - use with caution!
|
||||
#define Ki_YAW 0.00005
|
||||
#define Ki_ROLLPITCH 0.000010
|
||||
#define SPEEDFILT 2 // >1 use min speed filter for yaw drift cancellation, 0=do not use speed filter
|
||||
#define ToRad(x) (x*0.01745329252) // *pi/180
|
||||
#define ToDeg(x) (x*57.2957795131) // *180/pi
|
||||
#define Accel_Scale(x) x*(GRAVITY/9.81) //Scaling the raw data of the accel to actual acceleration in meters for seconds square
|
||||
|
||||
#define USE_DEFEKT
|
||||
#ifdef USE_DEFEKT
|
||||
#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
#else
|
||||
#define Gyro_Scaled_X(x) x*(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
#define Gyro_Scaled_Y(x) x*(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
#define Gyro_Scaled_Z(x) x*(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
|
||||
#endif
|
||||
|
||||
#define Gyro_Gain_X 0.92 //X axis Gyro gain
|
||||
#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
|
||||
#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
|
||||
//#define Gyro_Gain_X 1.0 //X axis Gyro gain
|
||||
//#define Gyro_Gain_Y 1.0 //Y axis Gyro gain
|
||||
//#define Gyro_Gain_Z 1.0 //Z axis Gyro gain
|
||||
|
||||
void dcm_init( int i );
|
||||
|
||||
extern float adc[11];
|
||||
@@ -0,0 +1,339 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "wiring.h"
|
||||
#include "vector.h"
|
||||
#include "matrix.h"
|
||||
#include "read_adc.h"
|
||||
#include "arduimu.h"
|
||||
|
||||
#ifdef ANALOG_IMU
|
||||
#include "analogimu.h"
|
||||
#include "analogimu_util.h"
|
||||
#endif // ANALOG_IMU
|
||||
|
||||
#include "dcm.h"
|
||||
/**
|
||||
* Geschätzte Winkel in der Eulerdarstellung
|
||||
* Estimated angles as Euler
|
||||
*/
|
||||
float euler[EULER_LAST] = {0.};
|
||||
|
||||
/**************************************************/
|
||||
void Normalize(void)
|
||||
{
|
||||
float error=0;
|
||||
float temporary[3][3];
|
||||
float renorm=0;
|
||||
boolean problem=FALSE;
|
||||
|
||||
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
|
||||
|
||||
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
|
||||
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
|
||||
|
||||
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
|
||||
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
|
||||
|
||||
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
|
||||
|
||||
renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???SQT:1,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",ERR:");
|
||||
Serial.print (error);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???PRB:1,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",ERR:");
|
||||
Serial.print (error);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
|
||||
|
||||
renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???SQT:2,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",ERR:");
|
||||
Serial.print (error);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???PRB:2,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",ERR:");
|
||||
Serial.print (error);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
|
||||
|
||||
renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???SQT:3,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",ERR:");
|
||||
Serial.print (error);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("???PRB:3,RNM:");
|
||||
Serial.print (renorm);
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
|
||||
|
||||
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
|
||||
DCM_Matrix[0][0]= 1.0f;
|
||||
DCM_Matrix[0][1]= 0.0f;
|
||||
DCM_Matrix[0][2]= 0.0f;
|
||||
DCM_Matrix[1][0]= 0.0f;
|
||||
DCM_Matrix[1][1]= 1.0f;
|
||||
DCM_Matrix[1][2]= 0.0f;
|
||||
DCM_Matrix[2][0]= 0.0f;
|
||||
DCM_Matrix[2][1]= 0.0f;
|
||||
DCM_Matrix[2][2]= 1.0f;
|
||||
problem = FALSE;
|
||||
}
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
extern short gps_course;
|
||||
extern short gps_gspeed;
|
||||
extern short gps_climb;
|
||||
extern short gps_mode;
|
||||
|
||||
void Drift_correction(void)
|
||||
{
|
||||
//Compensation the Roll, Pitch and Yaw drift.
|
||||
static float Scaled_Omega_P[3];
|
||||
static float Scaled_Omega_I[3];
|
||||
float Accel_magnitude;
|
||||
float Accel_weight;
|
||||
float Integrator_magnitude;
|
||||
// hwarm
|
||||
ground_course=gps_course/10.-180.;
|
||||
ground_speed=gps_gspeed/100.;
|
||||
float ground_climb=gps_climb/100.;
|
||||
speed_3d= sqrt(ground_speed*ground_speed+ground_climb*ground_climb);
|
||||
//*****Roll and Pitch***************
|
||||
|
||||
// Calculate the magnitude of the accelerometer vector
|
||||
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
|
||||
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
|
||||
// Dynamic weighting of accelerometer info (reliability filter)
|
||||
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
|
||||
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
|
||||
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
|
||||
imu_health += tempfloat;
|
||||
imu_health = constrain(imu_health,129,65405);
|
||||
#endif
|
||||
|
||||
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
|
||||
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
|
||||
|
||||
//*****YAW***************
|
||||
|
||||
#if USE_MAGNETOMETER==1
|
||||
// We make the gyro YAW drift correction based on compass magnetic heading
|
||||
mag_heading_x = cos(MAG_Heading);
|
||||
mag_heading_y = sin(MAG_Heading);
|
||||
errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
|
||||
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
||||
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
|
||||
#else // Use GPS Ground course to correct yaw gyro drift
|
||||
if(gps_mode==3 && ground_speed>= 0.5) //hwarm
|
||||
{
|
||||
|
||||
COGX = cos(ToRad(ground_course));
|
||||
COGY = sin(ToRad(ground_course));
|
||||
errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
|
||||
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
||||
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
|
||||
}
|
||||
#endif
|
||||
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
|
||||
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
|
||||
if (Integrator_magnitude > ToRad(300)) {
|
||||
Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("!!!INT:1,MAG:");
|
||||
Serial.print (ToDeg(Integrator_magnitude));
|
||||
|
||||
Serial.print (",TOW:");
|
||||
Serial.print (iTOW);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
/**************************************************/
|
||||
void Accel_adjust(void)
|
||||
{
|
||||
#ifndef ANALOGIMU_ROTATED
|
||||
Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
|
||||
Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
|
||||
#else
|
||||
Accel_Vector[0] -= Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_x = GPS_speed*GyroZ (ok, wenn x beim rollen nach rechts negativ)
|
||||
Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[0]); // Centrifugal force on Acc_z = GPS_speed*GyroX (ok, wenn nase hoch positiv)
|
||||
#endif
|
||||
}
|
||||
/**************************************************/
|
||||
|
||||
void Matrix_update(void)
|
||||
{
|
||||
/* chni: Gyro_Vector[0]=Gyro_Scaled_X(read_adc(3)); //gyro x roll
|
||||
Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(4)); //gyro y pitch
|
||||
Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(5)); //gyro Z yaw
|
||||
|
||||
Accel_Vector[0]=read_adc(0); // acc x
|
||||
Accel_Vector[1]=read_adc(1); // acc y
|
||||
Accel_Vector[2]=read_adc(2); // acc z */
|
||||
|
||||
/* chni: Offset is set dynamic on Ground*/
|
||||
Gyro_Vector[0]= -gyro_to_zero[G_ROLL] + gyro[G_ROLL];
|
||||
Gyro_Vector[1]= -gyro_to_zero[G_PITCH] + gyro[G_PITCH];
|
||||
Gyro_Vector[2]= -gyro_to_zero[G_PITCH] + gyro[G_YAW];
|
||||
|
||||
Accel_Vector[0] = + 0.0 + accel[ACC_X];
|
||||
Accel_Vector[1] = - 0.286 + accel[ACC_Y];
|
||||
Accel_Vector[2] = accel[ACC_Z];
|
||||
|
||||
|
||||
|
||||
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
|
||||
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
|
||||
#define USE_GPS
|
||||
#ifdef USE_GPS
|
||||
if (gps_mode==3) Accel_adjust(); //Remove centrifugal acceleration.
|
||||
#endif
|
||||
|
||||
#define OUTPUTMODE 1
|
||||
#if OUTPUTMODE==1 // With corrected data (drift correction)
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
|
||||
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
|
||||
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
|
||||
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
|
||||
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
|
||||
Update_Matrix[2][2]=0;
|
||||
#else // Uncorrected data (no drift correction)
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
|
||||
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
|
||||
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
|
||||
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
|
||||
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
|
||||
Update_Matrix[2][2]=0;
|
||||
#endif
|
||||
|
||||
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
|
||||
|
||||
for(int x=0; x<3; x++) //Matrix Addition (update)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Euler_angles(void)
|
||||
{
|
||||
//#define OUTPUTMODE 2
|
||||
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
|
||||
euler[EULER_ROLL] = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
|
||||
//euler[EULER_PITCH] = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
|
||||
//todo: chni:ordentlich lösen!
|
||||
euler[EULER_PITCH] = -asin((Accel_Vector[0])/9.81); // asin(acc_x)
|
||||
euler[EULER_YAW] = 0;
|
||||
#else
|
||||
//pitch = -asin(DCM_Matrix[2][0]);
|
||||
//roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
|
||||
//yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
|
||||
#ifndef ANALOGIMU_ROTATED
|
||||
euler[EULER_PITCH] = -asin(DCM_Matrix[2][0]);
|
||||
euler[EULER_ROLL] = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
|
||||
#else
|
||||
euler[EULER_ROLL] = -asin(DCM_Matrix[2][0]);
|
||||
euler[EULER_PITCH] = -atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
|
||||
#endif
|
||||
|
||||
euler[EULER_YAW] = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
|
||||
euler[EULER_YAW] += M_PI; // Rotating the angle 180deg to fit for PPRZ
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -0,0 +1,15 @@
|
||||
void Euler_angles(void);
|
||||
void Matrix_update(void);
|
||||
void Accel_adjust(void);
|
||||
void Drift_correction(void);
|
||||
void Normalize(void);
|
||||
|
||||
/** defines for euler vector */
|
||||
enum euler_idx_t { EULER_ROLL, EULER_PITCH, EULER_YAW, EULER_LAST };
|
||||
|
||||
#define M_PI 3.14159265358979323846
|
||||
|
||||
// variables
|
||||
|
||||
/** output vector with angles in rad */
|
||||
extern float euler[EULER_LAST];
|
||||
@@ -0,0 +1,24 @@
|
||||
/**************************************************/
|
||||
//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
|
||||
|
||||
#include "matrix.h"
|
||||
|
||||
void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
|
||||
{
|
||||
float op[3];
|
||||
for(int x=0; x<3; x++)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
for(int w=0; w<3; w++)
|
||||
{
|
||||
op[w]=a[x][w]*b[w][y];
|
||||
}
|
||||
mat[x][y]=0;
|
||||
mat[x][y]=op[0]+op[1]+op[2];
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,2 @@
|
||||
|
||||
void Matrix_Multiply(float a[3][3],float b[3][3],float mat[3][3]);
|
||||
@@ -0,0 +1,2 @@
|
||||
/* This file was automatically generated. Do not edit! */
|
||||
float read_adc(char i);
|
||||
@@ -0,0 +1,42 @@
|
||||
#include "vector.h"
|
||||
|
||||
//Computes the dot product of two vectors
|
||||
float Vector_Dot_Product(float vector1[3],float vector2[3])
|
||||
{
|
||||
float op=0;
|
||||
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
op+=vector1[c]*vector2[c];
|
||||
}
|
||||
|
||||
return op;
|
||||
}
|
||||
|
||||
//Computes the cross product of two vectors
|
||||
void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
|
||||
{
|
||||
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
|
||||
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
|
||||
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
|
||||
}
|
||||
|
||||
//Multiply the vector by a scalar.
|
||||
void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
|
||||
{
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
vectorOut[c]=vectorIn[c]*scale2;
|
||||
}
|
||||
}
|
||||
|
||||
void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
|
||||
{
|
||||
for(int c=0; c<3; c++)
|
||||
{
|
||||
vectorOut[c]=vectorIn1[c]+vectorIn2[c];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
|
||||
void Vector_Add(float vectorOut[3],float vectorIn1[3],float vectorIn2[3]);
|
||||
void Vector_Scale(float vectorOut[3],float vectorIn[3],float scale2);
|
||||
void Vector_Cross_Product(float vectorOut[3],float v1[3],float v2[3]);
|
||||
float Vector_Dot_Product(float vector1[3],float vector2[3]);
|
||||
@@ -0,0 +1,135 @@
|
||||
|
||||
/*
|
||||
wiring.h - Partial implementation of the Wiring API for the ATmega8.
|
||||
Part of Arduino - http://www.arduino.cc/
|
||||
|
||||
Copyright (c) 2005-2006 David A. Mellis
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This library 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
|
||||
Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General
|
||||
Public License along with this library; if not, write to the
|
||||
Free Software Foundation, Inc., 59 Temple Place, Suite 330,
|
||||
Boston, MA 02111-1307 USA
|
||||
|
||||
$Id: wiring.h 804 2009-12-18 16:05:52Z dmellis $
|
||||
*/
|
||||
|
||||
#ifndef Wiring_h
|
||||
#define Wiring_h
|
||||
|
||||
// olri #include <avr/io.h>
|
||||
// olri #include "binary.h"
|
||||
#include <inttypes.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"{
|
||||
#endif
|
||||
|
||||
#define HIGH 0x1
|
||||
#define LOW 0x0
|
||||
|
||||
#define INPUT 0x0
|
||||
#define OUTPUT 0x1
|
||||
|
||||
#define true 0x1
|
||||
#define false 0x0
|
||||
|
||||
#define PI 3.1415926535897932384626433832795
|
||||
#define HALF_PI 1.5707963267948966192313216916398
|
||||
#define TWO_PI 6.283185307179586476925286766559
|
||||
#define DEG_TO_RAD 0.017453292519943295769236907684886
|
||||
#define RAD_TO_DEG 57.295779513082320876798154814105
|
||||
|
||||
#define SERIAL 0x0
|
||||
#define DISPLAY 0x1
|
||||
|
||||
#define LSBFIRST 0
|
||||
#define MSBFIRST 1
|
||||
|
||||
#define CHANGE 1
|
||||
#define FALLING 2
|
||||
#define RISING 3
|
||||
|
||||
#define INTERNAL 3
|
||||
#define DEFAULT 1
|
||||
#define EXTERNAL 0
|
||||
|
||||
// undefine stdlib's abs if encountered
|
||||
#ifdef abs
|
||||
#undef abs
|
||||
#endif
|
||||
|
||||
#define min(a,b) ((a)<(b)?(a):(b))
|
||||
#define max(a,b) ((a)>(b)?(a):(b))
|
||||
#define abs(x) ((x)>0?(x):-(x))
|
||||
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
|
||||
#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
|
||||
#define radians(deg) ((deg)*DEG_TO_RAD)
|
||||
#define degrees(rad) ((rad)*RAD_TO_DEG)
|
||||
#define sq(x) ((x)*(x))
|
||||
|
||||
#define interrupts() sei()
|
||||
#define noInterrupts() cli()
|
||||
|
||||
#define clockCyclesPerMicrosecond() ( F_CPU / 1000000L )
|
||||
#define clockCyclesToMicroseconds(a) ( (a) / clockCyclesPerMicrosecond() )
|
||||
#define microsecondsToClockCycles(a) ( (a) * clockCyclesPerMicrosecond() )
|
||||
|
||||
#define lowByte(w) ((uint8_t) ((w) & 0xff))
|
||||
#define highByte(w) ((uint8_t) ((w) >> 8))
|
||||
|
||||
#define bitRead(value, bit) (((value) >> (bit)) & 0x01)
|
||||
#define bitSet(value, bit) ((value) |= (1UL << (bit)))
|
||||
#define bitClear(value, bit) ((value) &= ~(1UL << (bit)))
|
||||
#define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit))
|
||||
|
||||
typedef unsigned int word;
|
||||
|
||||
#define bit(b) (1UL << (b))
|
||||
|
||||
//chni: typedef uint8_t boolean;
|
||||
typedef uint8_t byte;
|
||||
|
||||
void init(void);
|
||||
|
||||
void pinMode(uint8_t, uint8_t);
|
||||
void digitalWrite(uint8_t, uint8_t);
|
||||
int digitalRead(uint8_t);
|
||||
int analogRead(uint8_t);
|
||||
void analogReference(uint8_t mode);
|
||||
void analogWrite(uint8_t, int);
|
||||
|
||||
void beginSerial(long);
|
||||
void serialWrite(unsigned char);
|
||||
int serialAvailable(void);
|
||||
int serialRead(void);
|
||||
void serialFlush(void);
|
||||
|
||||
unsigned long millis(void);
|
||||
unsigned long micros(void);
|
||||
void delay(unsigned long);
|
||||
void delayMicroseconds(unsigned int us);
|
||||
unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout);
|
||||
|
||||
void shiftOut(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder, byte val);
|
||||
|
||||
void attachInterrupt(uint8_t, void (*)(void), int mode);
|
||||
void detachInterrupt(uint8_t);
|
||||
|
||||
void setup(void);
|
||||
void loop(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} // extern "C"
|
||||
#endif
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user