mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-01 12:57:27 +08:00
Merge branch 'manta_pprz_tracking' into gps_merge
This commit is contained in:
Andre Devitt
@@ -0,0 +1,32 @@
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="GYRO_P_NEUTRAL" value="93"/>
|
||||
<define name="GYRO_Q_NEUTRAL" value="131"/>
|
||||
<define name="GYRO_R_NEUTRAL" value="-25"/>
|
||||
<define name="GYRO_P_SENS" value="5.0" integer="16"/>
|
||||
<define name="GYRO_Q_SENS" value="5.0" integer="16"/>
|
||||
<define name="GYRO_R_SENS" value="5.0" integer="16"/>
|
||||
<define name="GYRO_PQ_SENS" value="0.0" integer="16"/>
|
||||
<define name="GYRO_PR_SENS" value="0.0" integer="16"/>
|
||||
<define name="GYRO_QR_SENS" value="0.0" integer="16"/>
|
||||
|
||||
<define name="ACCEL_X_NEUTRAL" value="4"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="-14"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="-5"/>
|
||||
<define name="ACCEL_X_SENS" value="39" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="39" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="39" integer="16"/>
|
||||
<define name="ACCEL_XY_SENS" value="0.0" integer="16"/>
|
||||
<define name="ACCEL_XZ_SENS" value="0.0" integer="16"/>
|
||||
<define name="ACCEL_YZ_SENS" value="0.0" integer="16"/>
|
||||
|
||||
<define name="MAG_X_NEUTRAL" value="-21"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="10"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="12"/>
|
||||
<define name="MAG_X_SENS" value="3.17378921476" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="3.14663275967" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="3.26531022727" integer="16"/>
|
||||
<define name="MAG_XY_SENS" value="0.0" integer="16"/>
|
||||
<define name="MAG_XZ_SENS" value="0.0" integer="16"/>
|
||||
<define name="MAG_YZ_SENS" value="0.0" integer="16"/>
|
||||
|
||||
</section>
|
||||
@@ -38,7 +38,7 @@
|
||||
|
||||
|
||||
<section name="SUPERVISION" prefix="SUPERVISION_">
|
||||
<define name="MIN_MOTOR" value="1090"/>
|
||||
<define name="MIN_MOTOR" value="1150"/>
|
||||
<define name="MAX_MOTOR" value="2000"/>
|
||||
<define name="TRIM_A" value="0"/>
|
||||
<define name="TRIM_E" value="0"/>
|
||||
@@ -51,40 +51,13 @@
|
||||
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
|
||||
</section>
|
||||
|
||||
<include href="conf/airframes/esden/calib/aspirin_012.xml"/>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="GYRO_P_NEUTRAL" value="27"/>
|
||||
<define name="GYRO_Q_NEUTRAL" value="-10"/>
|
||||
<define name="GYRO_R_NEUTRAL" value="20"/>
|
||||
<define name="GYRO_P_SENS" value="5.0" integer="16"/>
|
||||
<define name="GYRO_Q_SENS" value="5.0" integer="16"/>
|
||||
<define name="GYRO_R_SENS" value="5.0" integer="16"/>
|
||||
<define name="GYRO_PQ_SENS" value="0.0" integer="16"/>
|
||||
<define name="GYRO_PR_SENS" value="0.0" integer="16"/>
|
||||
<define name="GYRO_QR_SENS" value="0.0" integer="16"/>
|
||||
|
||||
<define name="ACCEL_X_NEUTRAL" value="4"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="-14"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="-5"/>
|
||||
<define name="ACCEL_X_SENS" value="39" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="39" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="39" integer="16"/>
|
||||
<define name="ACCEL_XY_SENS" value="0.0" integer="16"/>
|
||||
<define name="ACCEL_XZ_SENS" value="0.0" integer="16"/>
|
||||
<define name="ACCEL_YZ_SENS" value="0.0" integer="16"/>
|
||||
|
||||
<define name="MAG_X_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="0"/>
|
||||
<define name="MAG_X_SENS" value="1." integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="1." integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="1." integer="16"/>
|
||||
<define name="MAG_XY_SENS" value="0.0" integer="16"/>
|
||||
<define name="MAG_XZ_SENS" value="0.0" integer="16"/>
|
||||
<define name="MAG_YZ_SENS" value="0.0" integer="16"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="RadOfDeg( 0. )"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="RadOfDeg( 0. )"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="RadOfDeg( 45. )"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="RadOfDeg( 225. )"/>
|
||||
|
||||
</section>
|
||||
|
||||
@@ -137,6 +110,7 @@
|
||||
<define name="REF_MAX_R" value="RadOfDeg(180.)"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
|
||||
|
||||
<!-- Old settings with exponential throttle motor controllers
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="-250"/>
|
||||
<define name="PHI_DGAIN" value="-200"/>
|
||||
@@ -154,7 +128,33 @@
|
||||
<define name="PHI_DDGAIN" value=" 200"/>
|
||||
<define name="THETA_DDGAIN" value=" 200"/>
|
||||
<define name="PSI_DDGAIN" value=" 200"/>
|
||||
-->
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="-1900"/>
|
||||
<define name="PHI_DGAIN" value="-380"/>
|
||||
<define name="PHI_IGAIN" value="-200"/>
|
||||
|
||||
<define name="THETA_PGAIN" value="-1900"/>
|
||||
<define name="THETA_DGAIN" value="-380"/>
|
||||
<define name="THETA_IGAIN" value="-200"/>
|
||||
|
||||
<define name="PSI_PGAIN" value="-2000"/>
|
||||
<define name="PSI_DGAIN" value="-400"/>
|
||||
<define name="PSI_IGAIN" value="-10"/>
|
||||
|
||||
<!-- feedforward -->
|
||||
<define name="PHI_DDGAIN" value=" 300"/>
|
||||
<define name="THETA_DDGAIN" value=" 300"/>
|
||||
<define name="PSI_DDGAIN" value=" 300"/>
|
||||
|
||||
</section>
|
||||
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<define name="PROPAGATE_FREQUENCY" value="512"/>
|
||||
<define name="H_X" value=" 0.51562740288882"/>
|
||||
<define name="H_Y" value="-0.05707735220832"/>
|
||||
<define name="H_Z" value=" 0.85490967783446"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
@@ -178,7 +178,6 @@
|
||||
<define name="INV_M" value ="0.15"/>
|
||||
</section>
|
||||
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="PGAIN" value="-100"/>
|
||||
<define name="DGAIN" value="-100"/>
|
||||
@@ -224,7 +223,8 @@
|
||||
<subsystem name="gps" type="ublox">
|
||||
<configure name="GPS_BAUD" value="B57600"/>
|
||||
</subsystem>
|
||||
<subsystem name="ahrs" type="cmpl"/>
|
||||
<!--subsystem name="ahrs" type="cmpl"/-->
|
||||
<subsystem name="ahrs" type="ic"/>
|
||||
</firmware>
|
||||
|
||||
<firmware name="lisa_l_test_progs">
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
</header>
|
||||
<init fun="ArduIMU_init()"/>
|
||||
<periodic fun="ArduIMU_periodic()" freq="60"/>
|
||||
<periodic fun="ArduIMU_periodicGPS()" freq="8"/>
|
||||
<periodic fun="ArduIMU_periodicGPS()" freq="4"/>
|
||||
<event fun="ArduIMU_event()"/>
|
||||
<makefile target="ap">
|
||||
<file name="ins_arduimu_basic.c"/>
|
||||
|
||||
@@ -4,67 +4,27 @@
|
||||
|
||||
<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" group="circle"/>
|
||||
<strip_button icon="circle-left.png" name="Circle left" value="-1" group="circle"/>
|
||||
<key_press key="greater" value="1"/>
|
||||
<key_press key="less" value="-1"/>
|
||||
</dl_setting>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="control">
|
||||
<dl_settings NAME="infrared">
|
||||
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" module="subsystems/sensors/infrared" />
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.lateral_correction" shortname="lat_corr" param="IR_LATERAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.longitudinal_correction" shortname="log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.vertical_correction" shortname="vert_corr" param="IR_VERTICAL_CORRECTION"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_left" shortname="corr_left" param="IR_CORRECTION_LEFT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_right" shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_up" shortname="corr_up" param="IR_CORRECTION_UP"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_down" shortname="corr_down" param="IR_CORRECTION_DOWN"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
|
||||
<dl_settings NAME="attitude">
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" var="use_airspeed_ratio" values="FALSE|TRUE"/>
|
||||
<dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="fw_h_ctl"/>
|
||||
<dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
|
||||
<dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN"/>
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_igain" shortname="roll_igain" module="fw_h_ctl_a" handler="SetRollIGain"/>
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_igain" shortname="roll_igain" param="H_CTL_ROLL_IGAIN" handler="SetRollIGain"/>
|
||||
<dl_setting MAX="0" MIN="-55000" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain" param="H_CTL_PITCH_PGAIN"/>
|
||||
<dl_setting MAX="0" MIN="-55000" STEP="250" VAR="h_ctl_pitch_dgain" shortname="pitch_dgain" param="H_CTL_PITCH_DGAIN"/>
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_igain" shortname="pitch_igain" module="fw_h_ctl_a" handler="SetPitchIGain"/>
|
||||
<dl_setting MAX=".3" MIN="0." STEP="0.001" VAR="h_ctl_pitch_of_roll" shortname="pitch_of_roll" param="H_CTL_PITCH_OF_ROLL" module="fw_h_ctl_a"/>
|
||||
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle"/>
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_igain" shortname="pitch_igain" param="H_CTL_PITCH_IGAIN" handler="SetPitchIGain"/>
|
||||
<dl_setting MAX=".3" MIN="0." STEP="0.001" VAR="h_ctl_pitch_of_roll" shortname="pitch_of_roll" param="H_CTL_PITCH_OF_ROLL"/>
|
||||
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle" module="stabilization/stabilization_adaptive"/>
|
||||
|
||||
<dl_setting MAX="60" MIN="0" STEP="1" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT" unit="rad" alt_unit="deg" alt_unit_coef="0.0174533"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" var="use_airspeed_ratio" values="FALSE|TRUE"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="feedforward">
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffa" module="fw_h_ctl_a" shortname="roll_Kffa"/>
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffd" module="fw_h_ctl_a" shortname="roll_Kffd"/>
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffa" module="fw_h_ctl_a" shortname="pitch_Kffa"/>
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffd" module="fw_h_ctl_a" shortname="pitch_Kffd"/>
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffa" shortname="roll_Kffa" param="H_CTL_ROLL_KFFA"/>
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffd" shortname="roll_Kffd" param="H_CTL_ROLL_KFFD"/>
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffa" shortname="pitch_Kffa" param="H_CTL_PITCH_KFFA"/>
|
||||
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffd" shortname="pitch_Kffd" param="H_CTL_PITCH_KFFD"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="alt">
|
||||
@@ -77,11 +37,11 @@
|
||||
<strip_button name="AS" value="1" group="speed_mode"/>
|
||||
<strip_button name="GS" value="2" group="speed_mode"/>
|
||||
</dl_setting>
|
||||
<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"/>
|
||||
<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"/>
|
||||
|
||||
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_pgain" shortname="pitch_p" param="V_CTL_AUTO_PITCH_PGAIN"/>
|
||||
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_igain" shortname="pitch_i" param="V_CTL_AUTO_PITCH_IGAIN"/>
|
||||
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_dgain" shortname="pitch_d" param="V_CTL_AUTO_PITCH_DGAIN" module="fw_v_ctl_n"/>
|
||||
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_dgain" shortname="pitch_d" param="V_CTL_AUTO_PITCH_DGAIN" module="guidance/guidance_v_n"/>
|
||||
|
||||
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_throttle_pgain" shortname="throttle_p" param="V_CTL_AUTO_THROTTLE_PGAIN"/>
|
||||
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_throttle_igain" shortname="throttle_i" param="V_CTL_AUTO_THROTTLE_IGAIN"/>
|
||||
@@ -114,13 +74,10 @@
|
||||
<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="20" MIN="-20" STEP="1" VAR="fp_pitch"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="0.1" VAR="fp_throttle"/>
|
||||
<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_setting MAX="0.3" MIN="0" STEP="0.01" VAR="cte_igain"/>
|
||||
<dl_setting MAX="20" MIN="0" STEP="1" VAR="cte_max"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
|
||||
@@ -198,7 +198,7 @@ static inline void v_ctl_set_pitch ( void ) {
|
||||
|
||||
if (v_ctl_auto_pitch_igain < 0.) {
|
||||
v_ctl_auto_pitch_sum_err += err*(1./60.);
|
||||
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);
|
||||
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / (-v_ctl_auto_pitch_igain));
|
||||
}
|
||||
|
||||
// PI loop + feedforward ctl
|
||||
@@ -223,7 +223,7 @@ static inline void v_ctl_set_throttle( void ) {
|
||||
|
||||
if (v_ctl_auto_throttle_igain < 0.) {
|
||||
v_ctl_auto_throttle_sum_err += err*(1./60.);
|
||||
BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain);
|
||||
BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / (-v_ctl_auto_throttle_igain));
|
||||
}
|
||||
|
||||
// PID loop + feedforward ctl
|
||||
|
||||
@@ -31,29 +31,9 @@ float read_adc(int select)
|
||||
float temp;
|
||||
if (SENSOR_SIGN[select]<0){
|
||||
temp = (AN_OFFSET[select]-AN[select]);
|
||||
if (abs(temp)>900) {
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("!!!ADC:1,VAL:");
|
||||
Serial.print (temp);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
adc_constraints++;
|
||||
#endif
|
||||
}
|
||||
return constrain(temp,-900,900); //Throw out nonsensical values
|
||||
} else {
|
||||
temp = (AN[select]-AN_OFFSET[select]);
|
||||
if (abs(temp)>900) {
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.print("!!!ADC:2,VAL:");
|
||||
Serial.print (temp);
|
||||
Serial.println("***");
|
||||
#endif
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
adc_constraints++;
|
||||
#endif
|
||||
}
|
||||
return constrain(temp,-900,900);
|
||||
}
|
||||
}
|
||||
@@ -61,8 +41,8 @@ float read_adc(int select)
|
||||
//Activating the ADC interrupts.
|
||||
void Analog_Init(void)
|
||||
{
|
||||
ADCSRA|=(1<<ADIE)|(1<<ADEN);
|
||||
ADCSRA|= (1<<ADSC);
|
||||
ADCSRA|=(1<<ADIE)|(1<<ADEN);
|
||||
ADCSRA|= (1<<ADSC);
|
||||
}
|
||||
|
||||
//
|
||||
@@ -80,8 +60,8 @@ ISR(ADC_vect)
|
||||
high = ADCH;
|
||||
|
||||
if(analog_count[MuxSel]<63) {
|
||||
analog_buffer[MuxSel] += (high << 8) | low; // cumulate analog values
|
||||
analog_count[MuxSel]++;
|
||||
analog_buffer[MuxSel] += (high << 8) | low; // cumulate analog values
|
||||
analog_count[MuxSel]++;
|
||||
}
|
||||
MuxSel++;
|
||||
MuxSel &= 0x07; //if(MuxSel >=8) MuxSel=0;
|
||||
|
||||
@@ -21,27 +21,13 @@ void Normalize(void)
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
#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.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.println("***");
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
|
||||
@@ -53,25 +39,11 @@ void Normalize(void)
|
||||
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.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.println("***");
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
|
||||
@@ -83,23 +55,11 @@ void Normalize(void)
|
||||
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.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.println("***");
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
|
||||
@@ -170,23 +130,18 @@ void Drift_correction(void)
|
||||
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.println("***");
|
||||
#endif
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
void Accel_adjust(void)
|
||||
{
|
||||
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
|
||||
}
|
||||
/**************************************************/
|
||||
|
||||
/**************************************************/
|
||||
void Matrix_update(void)
|
||||
{
|
||||
Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
|
||||
@@ -202,7 +157,6 @@ void Matrix_update(void)
|
||||
|
||||
Accel_adjust(); //Remove centrifugal acceleration.
|
||||
|
||||
#if OUTPUTMODE==1
|
||||
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
|
||||
@@ -212,17 +166,6 @@ void Matrix_update(void)
|
||||
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
|
||||
|
||||
@@ -235,16 +178,11 @@ void Matrix_update(void)
|
||||
}
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
void Euler_angles(void)
|
||||
{
|
||||
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
|
||||
roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
|
||||
pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
|
||||
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]);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@@ -1,24 +1,28 @@
|
||||
|
||||
//*****I2C Output************************************************************
|
||||
|
||||
void requestEvent(){
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.println("Sending IMU Data");
|
||||
#endif
|
||||
|
||||
void fill_I2C_message() {
|
||||
// Message Array : Roll; Pitch; YaW; GyroX; GyroY; GyroZ; ACCX; ACCY; ACCZ
|
||||
// Float Number is multipited with 10000 and converted to an Integer, for sending via I2C.
|
||||
// Resolution for angles: 12, rates: 12, accel:10 (from pprza BFP math lib)
|
||||
I2C_Message_ar[0] = int(roll*(1<<12));
|
||||
I2C_Message_ar[1] = int(pitch*(1<<12));
|
||||
I2C_Message_ar[2] = int(yaw*(1<<12));
|
||||
I2C_Message_ar[3] = int(Gyro_Vector[0]*(1<<12));
|
||||
I2C_Message_ar[4] = int(Gyro_Vector[1]*(1<<12));
|
||||
I2C_Message_ar[5] = int(Gyro_Vector[2]*(1<<12));
|
||||
I2C_Message_ar[3] = int(Omega_Vector[0]*(1<<12));
|
||||
I2C_Message_ar[4] = int(Omega_Vector[1]*(1<<12));
|
||||
I2C_Message_ar[5] = int(Omega_Vector[2]*(1<<12));
|
||||
I2C_Message_ar[6] = int((9.81*Accel_Vector[0]/GRAVITY)*(1<<10));
|
||||
I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10));
|
||||
I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10));
|
||||
|
||||
}
|
||||
|
||||
void requestEvent(){
|
||||
#if PRINT_DEBUG != 0
|
||||
Serial.println("Sending IMU Data");
|
||||
#endif
|
||||
|
||||
fill_I2C_message();
|
||||
|
||||
byte* pointer;
|
||||
pointer = (byte*) &I2C_Message_ar;
|
||||
Wire.send(pointer, 18);
|
||||
@@ -51,6 +55,7 @@ void receiveEvent(int howMany){
|
||||
void printdata(void){
|
||||
|
||||
#if PRINT_I2C_MSG == 1
|
||||
fill_I2C_message();
|
||||
Serial.print("Time ");
|
||||
Serial.print(millis());
|
||||
Serial.print("; Roll ");
|
||||
@@ -89,8 +94,7 @@ void printdata(void){
|
||||
Serial.print (",AN4:");
|
||||
Serial.print(read_adc(4));
|
||||
Serial.print (",AN5:");
|
||||
Serial.print(read_adc(5));
|
||||
Serial.print (",");
|
||||
Serial.println(read_adc(5));
|
||||
#endif
|
||||
|
||||
#if PRINT_DCM == 1
|
||||
@@ -111,8 +115,7 @@ void printdata(void){
|
||||
Serial.print (",EX7:");
|
||||
Serial.print(convert_to_dec(DCM_Matrix[2][1]));
|
||||
Serial.print (",EX8:");
|
||||
Serial.print(convert_to_dec(DCM_Matrix[2][2]));
|
||||
Serial.print (",");
|
||||
Serial.println(convert_to_dec(DCM_Matrix[2][2]));
|
||||
#endif
|
||||
|
||||
#if PRINT_EULER == 1
|
||||
@@ -123,8 +126,7 @@ void printdata(void){
|
||||
Serial.print(",YAW:");
|
||||
Serial.print(ToDeg(yaw));
|
||||
Serial.print(",IMUH:");
|
||||
Serial.print((imu_health>>8)&0xff);
|
||||
Serial.print (",");
|
||||
Serial.println((imu_health>>8)&0xff);
|
||||
#endif
|
||||
|
||||
#if PRINT_GPS == 1
|
||||
@@ -141,6 +143,7 @@ void printdata(void){
|
||||
gps_messages_sent++;
|
||||
#endif
|
||||
}
|
||||
Serial.println("");
|
||||
#endif
|
||||
|
||||
}
|
||||
@@ -174,7 +177,7 @@ void printPerfData(long time)
|
||||
Serial.print(gps_messages_sent,DEC);
|
||||
Serial.print(",imu:");
|
||||
Serial.print((imu_health>>8),DEC);
|
||||
Serial.print(",***");
|
||||
Serial.println(",***");
|
||||
|
||||
// Reset counters
|
||||
mainLoop_count = 0;
|
||||
|
||||
+9
-10
@@ -30,9 +30,6 @@
|
||||
/*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_I2C_MSG 0 //Will print the I2C output buffer
|
||||
#define PRINT_DCM 0 //Will print the whole direction cosine matrix
|
||||
#define PRINT_ANALOGS 0 //Will print the analog raw data
|
||||
@@ -49,7 +46,7 @@ int I2C_Message_ar[9];
|
||||
|
||||
|
||||
// *** NOTE! To use ArduIMU with ArduPilot you must select binary output messages (change to 1 here)
|
||||
#define PRINT_BINARY 0 //Will print binary message and suppress ASCII messages (above)
|
||||
#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
|
||||
@@ -70,10 +67,9 @@ int I2C_Message_ar[9];
|
||||
#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
|
||||
#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
|
||||
#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
|
||||
@@ -84,7 +80,7 @@ int I2C_Message_ar[9];
|
||||
//#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution!
|
||||
#define Ki_YAW 0.00005
|
||||
|
||||
#define ADC_WARM_CYCLES 75
|
||||
#define ADC_WARM_CYCLES 100
|
||||
|
||||
#define FALSE 0
|
||||
#define TRUE 1
|
||||
@@ -268,7 +264,10 @@ void loop() //Main Loop
|
||||
{
|
||||
timeNow = millis();
|
||||
|
||||
if((timeNow-timer)>=20) // Main loop runs at 50Hz
|
||||
// 20 -> Main loop runs at 50Hz
|
||||
// 5 -> Main loop runs at 200Hz
|
||||
// Max measured duty time around 3ms
|
||||
if((timeNow-timer)>=5)
|
||||
{
|
||||
timer_old = timer;
|
||||
timer = timeNow;
|
||||
|
||||
@@ -132,7 +132,7 @@ void ArduIMU_event( void ) {
|
||||
estimator_p = arduimu_rates.p;
|
||||
ardu_ins_trans.status = I2CTransDone;
|
||||
|
||||
RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
|
||||
//RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
|
||||
RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
|
||||
RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user