Toying with various settings

This commit is contained in:
Developer
2017-04-01 12:56:19 +02:00
parent a5b6e4af5f
commit ea96621d43
5 changed files with 120 additions and 54 deletions
+85 -33
View File
@@ -39,7 +39,7 @@ NOTES:
<configure name="CPU_LED" value="1"/>
<module name="imu" type="mpu9250_spi">
<define name="USE_ADC_2"/>
<!-- <define name="USE_ADC_2"/>-->
<configure name="IMU_MPU9250_SPI_DEV" value="spi1"/>
<configure name="IMU_MPU9250_SPI_SLAVE_IDX" value="SPI_SLAVE2"/>
<define name="IMU_MPU9250_READ_MAG" value="FALSE"/>
@@ -94,6 +94,7 @@ NOTES:
<!-- <module name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN"/>
<define name="USE_MAGNETOMETER" value="TRUE"/>
<define name="USE_MAGNETOMETER_ONGROUND" value="FALSE"/>
</module>-->
<!--<module name="uart"/>--><!-- TODO Exteral HITL PC debugging e.g a photocam trigger etc -->
</target>
@@ -155,7 +156,8 @@ NOTES:
<define name="AHRS_ICQ_MAG_ID" value="MAG_HMC58XX_SENDER_ID" /> <!-- Meaning the external hmc-->
<configure name="USE_MAGNETOMETER" value="FALSE"/> <!-- First autoflight set to false better make sure it works and is callibrated if set to TRUE >;-)-->
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/> <!-- FALSE thus data from magneto only, testing, brrrr -->
<!--<define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN"/> --><!-- value="TRUE"/> -->
<!-- <define name="USE_MAGNETOMETER_ONGROUND" value="FALSE"/> -->
<!--<define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" value="TRUE"/> -->
</module>
<module name="ins" type="alt_float"/>
@@ -184,8 +186,9 @@ NOTES:
</firmware> -->
<!-- To test various things for the new board -->
<!--
<firmware name="test_progs">
<!--
<target name="test_module" board="px4fmu_4.0">
<module name="usb_serial_stm32_example1"/>
</target>
@@ -207,15 +210,16 @@ NOTES:
<configure name="IMU_MPU9250_SPI_SLAVE_IDX" value="SPI_SLAVE2"/>
</module>
<configure name="PERIODIC_FREQUENCY" value="120"/>
</target>
</target>
<target name="test_actuators_pwm_sin" board="px4fmu_4.0">
</target>
<target name="test_baro_board" board="px4fmu_4.0">
<configure name="BARO_LED" value="2"/>
</target>
-->
<target name="test_adc" board="px4fmu_4.0"/>
<!--
<target name="test_uart" board="px4fmu_4.0">
<define name="USE_UART1"/>
<define name="UART1_BAUD" value="B57600"/>
@@ -239,7 +243,8 @@ NOTES:
<!-- <target name="test_actuators_pwm" board="px4fmu_4.0">-->
<!-- <define name="SERVO_HZ" value="40"/> -->
<!-- </target>-->
<!--</firmware>-->
</firmware>
<!-- ************************* MODULES ************************* -->
<modules>
@@ -268,17 +273,19 @@ NOTES:
<configure name="AIRSPEED_ETS_I2C_DEV" value="i2c1"/>
</module>
<module name="current_sensor">
<!-- <module name="current_sensor">
<define name="USE_ADC_3"/>
<configure name="ADC_CURRENT_SENSOR" value="ADC_3"/>
<!-- <define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.3"/>--><!-- TODO determine -->
</module>
<configure name="ADC_CURRENT_SENSOR" value="ADC_3"/>-->
<!-- <define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.3"/>--><!-- TODO determine -->
<!-- </module>-->
<!-- <module name="adc_generic">
<configure name="ADC_CHANNEL_GENERIC1" value="ADC_2"/>
<configure name="ADC_CHANNEL_GENERIC2" value="ADC_3"/>
</module> -->
<!-- <module name="adc_generic">-->
<!-- SONar -->
<!-- Sonar -->
<!-- <configure name="ADC_CHANNEL_GENERIC1" value="ADC_1"/>-->
<!--<configure name="ADC_SONAR" value="ADC_1"/>-->
<!-- RPM measuring sensor -->
@@ -392,32 +399,44 @@ NOTES:
<!-- ************************* SENSORS ************************* -->
<section name="IMU" prefix="IMU_"> <!-- TODO -->
<!-- ***************** MAGNETO *****************-->
<!-- replace this with your own calibration (on the correct sensor?) -->
<define name="MAG_X_NEUTRAL" value="-179"/>
<define name="MAG_Y_NEUTRAL" value="-21"/>
<define name="MAG_Z_NEUTRAL" value="79"/>
<!-- TODO: Calibrate,
<!-- RE-CALIBRATE after your aircraft changes e.g. cabeling or something else !-->
<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="4.17334785618" integer="16"/>
<define name="MAG_Y_SENS" value="3.98885954135" integer="16"/>
<define name="MAG_Z_SENS" value="4.40442339014" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0"/>
<define name="BODY_TO_IMU_THETA" value="0"/>
<define name="BODY_TO_IMU_PSI" value="0"/>
<define name="BODY_TO_IMU_PHI" value="0.0"/>
<define name="BODY_TO_IMU_THETA" value="0.0"/>
<define name="BODY_TO_IMU_PSI" value="0.0"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<!-- NL -->
<define name="H_X" value="0.3892503" />
<define name="H_Y" value="0.0017972" />
<define name="H_Z" value="0.9211303" />
<!--TODO: start using geo_mag module, else replace the values with your local magnetic field -->
<!-- Local Magnetic field NL Testfield-->
<!--
<define name="H_X" value="0.382478"/>
<define name="H_Y" value="0.00563406"/>
<define name="H_Z" value="0.923948"/>
-->
<!-- Local Magnetic field DE Testfield -->
<define name="H_X" value="0.51562740288882"/>
<define name="H_Y" value="-0.05707735220832"/>
<define name="H_Z" value="0.85490967783446"/>
<!-- Some insights https://lists.nongnu.org/archive/html/paparazzi-devel/2013-10/msg00126.html -->
<!-- <define name="GRAVITY_HEURISTIC_FACTOR" value="2"/>--><!-- TODO test and determine best val for heavy vibrating airfame -->
</section>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0.0" unit="deg"/><!-- TODO: Determine when fixed in final -->
<define name="PITCH_NEUTRAL_DEFAULT" value="0.0" unit="deg"/><!-- TODO: Determine when fixed in final -->
<define name="ROLL_NEUTRAL_DEFAULT" value="0.0" unit="deg"/><!-- TODO: Determine when mechanical in final fixed position in airframe -->
<define name="PITCH_NEUTRAL_DEFAULT" value="0.0" unit="deg"/><!-- TODO: Determine when mechanical in final fixed position in airframe -->
</section>
<!-- TODO: Set correct values for Maxbotix LV1 (Model 1010) -->
@@ -431,9 +450,23 @@ NOTES:
<!-- ************************* GAINS ************************* -->
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="0.80"/>
<define name="COURSE_DGAIN" value="0.20"/>
<define name="COURSE_PRE_BANK_CORRECTION" value="1.0"/>
<!-- TODO: Tune value -->
<define name="COURSE_PGAIN" value="0.80"/>
<define name="COURSE_DGAIN" value="0.10"/>
<!--
The prebank is an adjustment to the roll setting which is performed when the aircraft is
trying to do a circle and when it is close to the circumference of the circle. This way
it does not fly straight into the circumference but instead it starts to make a roll as
the one needed to fly in circles.
There is a value in the airframe file COURSE_PRE_BANK_CORRECTION which can be used to
increase o decrease the effect. If set to 1 then the normal prebank is done.
If set to 0.5 then half of the additional roll is done. This causes the aircraft to not roll
enough in order to fly the intended circle and it ends up flying in a larger circle.
A value > 1 makes it fly a smaller circle.
https://github.com/paparazzi/paparazzi/blob/master/sw/airborne/modules/nav.c#L132
-->
<define name="COURSE_PRE_BANK_CORRECTION" value="0.8"/>
<define name="ROLL_MAX_SETPOINT" value="45" unit="deg"/><!-- Determine best val without scaring the sh*t out of us-->
<define name="PITCH_MAX_SETPOINT" value="50" unit="deg"/><!-- Determine best val without scaring the sh*t out of us-->
<define name="PITCH_MIN_SETPOINT" value="-50" unit="deg"/><!-- Determine best val without scaring the sh*t out of us-->
@@ -471,7 +504,12 @@ NOTES:
<define name="AUTO_PITCH_OF_AIRSPEED_PGAIN" value="0.0"/>
<define name="AUTO_PITCH_OF_AIRSPEED_IGAIN" value="0.0"/>
<define name="AUTO_PITCH_OF_AIRSPEED_DGAIN" value="0.0"/>
<define name="THROTTLE_SLEW_LIMITER" value="1" unit="m/s/s"/>
<define name="THROTTLE_SLEW_LIMITER" value="0.7" unit="m/s/s"/>
<!--
<define name="AUTO_GROUNDSPEED_SETPOINT" value="16." unit="m/s"/>
<define name="AUTO_GROUNDSPEED_PGAIN" value="0.7"/>
<define name="AUTO_GROUNDSPEED_IGAIN" value="0."/>
-->
</section>
<!-- ************************ AUTO RC COMMANDS ***************************** -->
@@ -492,15 +530,19 @@ NOTES:
<!-- hint TODO Fix <define name="MilliAmpereOfAdc(adc)" value="((float)adc) * (3.3f / 4096.0f) * (90.0f / 5.0f)" />-->
<!-- 100Amp = 2Volt -> 2482,42 tick/100Amp"(0.0402832*adc)" -->
<!-- A simple powerblock module from CN -->
<!-- <define name="VoltageOfAdc(adc)" value="10"/> --><!-- Quick send data test -->
<!-- <define name="VoltageOfAdc(adc)" value="11.8"/>--> <!-- Quick send data test -->
<!-- <define name="VoltageOfAdc(adc)" value="24 - (0.004043478f * adc)"/>-->
<define name="VoltageOfAdc(adc)" value="12-((float)adc * 0.003f )"/>
<define name="MILLIAMP_AT_FULL_THROTTLE" value="20000"/> <!-- TODO unknown value, measure, now used as avarage value if no current sensor is attatched-->
<!-- <define name="VOLTAGE_ADC_SCALE" value="0.004511719f"/> --><!-- war 0.018 0.008 R1 (Bat - ADC input) = 22k, R2 (ADC input - GND) = 2.2K--->
<!-- <define name="VOLTAGE_OFFSET" value="0" unit="V"/>
<define name="VoltageOfAdc(adc)" value="((float)adc * VOLTAGE_ADC_SCALE + VOLTAGE_OFFSET)"/>
<define name="DefaultVoltageOfAdc(adc)" value="(0.00384*adc)"/>-->
<!-- <define name="VoltageOfAdc(adc)" value="((float)adc * 0.0001f )"/>-->
<define name="MILLIAMP_AT_FULL_THROTTLE" value="30000"/> <!-- TODO unknown value, measure, now used as avarage value if no current sensor is attatched-->
<define name="MAX_BAT_LEVEL" value="12.6" unit="V"/><!-- 3S lipo 3x4.2 = 12.6 -->
<define name="LOW_BAT_LEVEL" value="10.8" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="10.2" unit="V"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<!-- <define name="MIN_BAT_LEVEL" value="9.0" unit="V"/>--><!-- Get rid if this one .. -->
<define name="MIN_BAT_LEVEL" value="9.0" unit="V"/><!-- Get rid if this one .. -->
</section>
<!-- ************************* MISC ************************* -->
@@ -554,6 +596,16 @@ NOTES:
<section name="GCS">
<define name="SPEECH_NAME" value="Tee 28"/>
<define name="AC_ICON" value="fixedwing"/> <!-- Just to make sure ;) -->
<define name="ALT_SHIFT_PLUS_PLUS" value="50"/>
<define name="ALT_SHIFT_PLUS" value="10"/>
<define name="ALT_SHIFT_MINUS" value="-10"/>
</section>
<section name="SIMU">
<define name="WEIGHT" value ="1."/>
<define name="YAW_RESPONSE_FACTOR" value =".9"/> <!--default 1.-->
<define name="PITCH_RESPONSE_FACTOR" value ="1."/> <!--default 1.-->
<define name="ROLL_RESPONSE_FACTOR" value ="15."/> <!--default 15-->
</section>
</airframe>
+6
View File
@@ -57,3 +57,9 @@ GPS_BAUD ?= B38400
# e.g. <servo driver="Ppm">
#
ACTUATORS ?= actuators_pwm
#
# default External Current and Volt Sensor configuration
#
# ADC_CURRENT_SENSOR ?= ADC_3
# ADC_VOLTAGE_SENSOR ?= ADC_2
@@ -3,6 +3,7 @@
<telemetry>
<process name="Ap">
<mode name="default">
<message name="ADC_GENERIC" period="0.25"/>
<message name="AIRSPEED" period="1"/>
<message name="ALIVE" period="5.1"/>
<message name="GPS" period="0.25"/>
+1 -1
View File
@@ -18,7 +18,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/superbitrf.xml settings/nps.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml modules/air_data.xml modules/geo_mag.xml modules/gps_ubx_ucenter.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml modules/radio_control_superbitrf_rc.xml modules/air_data.xml modules/geo_mag.xml modules/gps_ubx_ucenter.xml"
gui_color="#fffffe72b9fd"
/>
<aircraft
+27 -20
View File
@@ -9,6 +9,9 @@
#define EXT_CLK 24000000 //OK
#define AHB_CLK 168000000 //OK
//#define STM32F4 //to debug ADC on F4 does no work
//#define ADC_SAMPLE_TIME ADC_SMPR_SMP_56CYC //to debug ADC on F4 does no work
/* On PCB Multicolor LED */
/* Red */
//OK
@@ -110,7 +113,7 @@
#define RADIO_CONTROL_POWER_ON gpio_clear // yes, inverted
#define RADIO_CONTROL_POWER_OFF gpio_set
//A sat
#define PERIPHERAL3V3_ENABLE_PORT GPIOC //VDD_3V3_PERIPHERAL_EN
#define PERIPHERAL3V3_ENABLE_PIN GPIO5
#define PERIPHERAL3V3_ENABLE_ON gpio_set
@@ -218,11 +221,11 @@ When a read-operation of an RTD resistance data register occurs, DRDY returns hi
#endif
#define USE_AD_TIM3 1
// Internal ADC for battery
// Internal ADC for voltage level measurement
#ifndef USE_ADC_1
#define USE_ADC_1 1
#endif
#if USE_ADC_1
#define AD1_1_CHANNEL 4 //ADC12_IN4
#define ADC_1 AD1_1
@@ -230,10 +233,11 @@ When a read-operation of an RTD resistance data register occurs, DRDY returns hi
#define ADC_1_GPIO_PIN GPIO4
#endif
// External ADC for battery
// Sense voltage level via external sensor on ADC
#ifndef USE_ADC_2
#define USE_ADC_2 1
#endif
#if USE_ADC_2
#define AD1_2_CHANNEL 2 // ADC123_IN2 (--> IN2 corresponds to channel 2)
#define ADC_2 AD1_2 // ADC123 means it can be used by ADC 1 and 2 and 3 (the f4 supports 3 adc's), does not matter which. Each ADC can address 4 pins, so in this case we are using ADC 1, on its second pin.
@@ -241,11 +245,11 @@ When a read-operation of an RTD resistance data register occurs, DRDY returns hi
#define ADC_2_GPIO_PIN GPIO2
#endif
// external current sens
// Sense current via external sensor on ADC
#ifndef USE_ADC_3
#define USE_ADC_3 1
#endif
#if USE_ADC_3
#define AD1_3_CHANNEL 3 // ADC123_IN3
#define ADC_3 AD1_3
@@ -255,27 +259,33 @@ When a read-operation of an RTD resistance data register occurs, DRDY returns hi
#define MilliAmpereOfAdc(adc)((float)adc) * (3.3f / 4096.0f) * (90.0f / 5.0f)
/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
#ifndef ADC_CHANNEL_VSUPPLY
#define ADC_CHANNEL_VSUPPLY ADC_2
#endif
//#ifndef ADC_CHANNEL_VSUPPLY
#define ADC_CHANNEL_VSUPPLY ADC_1
//#endif
#if USE_ADC_2
#define DefaultVoltageOfAdc(adc) ((float(adc) * (0.012660247f))// (0.00827*adc) //0,012660247
#else
#define DefaultVoltageOfAdc(adc) (0.003214815 * adc) // scale internal vdd to 5V
#endif
//??FT = Five-volt tolerant. In order to sustain a voltage higher than VDD+0.3 the internal pull-up/pull-down resistors must be disabled.
//#define DefaultVoltageOfAdc(adc) (0.0045*adc)
//Set stock values
//#if USE_ADC_2
//#define DefaultVoltageOfAdc(adc) (0.0005f * (float)adc)//FIXME: Value...
//#else
#define DefaultVoltageOfAdc(adc) (0.004 * adc) // scale internal vdd to 5V
//#endif
/* I2C mapping */
#define I2C1_GPIO_PORT GPIOB
#define I2C1_GPIO_SCL GPIO8
#define I2C1_GPIO_SDA GPIO9
//Onboard Barometer
#ifndef USE_BARO_BOARD
#define USE_BARO_BOARD 1
#endif
/* Another Magnetometer on board a HMC5983 not the one in the IMU 9250*/
//TODO
//TODO:
#ifndef USE_MAGNETOMETER_B
#define USE_MAGNETOMETER_B 0
#endif
@@ -349,7 +359,7 @@ When a read-operation of an RTD resistance data register occurs, DRDY returns hi
#define PWM_SERVO_4_OC_BIT 0
#endif
/* -ESC Servo 5 */
/* -ESC or Servo 5 */
#if USE_PWM5
#define PWM_SERVO_5 4
#define PWM_SERVO_5_TIMER TIM4
@@ -362,7 +372,7 @@ When a read-operation of an RTD resistance data register occurs, DRDY returns hi
#define PWM_SERVO_5_OC_BIT 0
#endif
/* -ESC Servo 6 */
/* -ESC or Servo 6 */
#if USE_PWM6
#define PWM_SERVO_6 5
#define PWM_SERVO_6_TIMER TIM4
@@ -379,7 +389,6 @@ When a read-operation of an RTD resistance data register occurs, DRDY returns hi
#define PWM_TIM4_CHAN_MASK (PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT)
/* Buzzer (A.k.a. Alarm) */
#if USE_BUZZER
#define PWM_BUZZER
#define PWM_BUZZER_TIMER TIM2
@@ -393,6 +402,4 @@ When a read-operation of an RTD resistance data register occurs, DRDY returns hi
#define PWM_BUZZER_OC_BIT 0
#endif
#endif /* CONFIG_PX4FMU_4_0_H */