Skip to content

Commit

Permalink
Some cosmetic changes; added mkk exaple setup file
Browse files Browse the repository at this point in the history
  • Loading branch information
@softsr
softsr committed Feb 6, 2013
1 parent 6ad46fa commit 00790cd
Show file tree
Hide file tree
Showing 7 changed files with 318 additions and 30 deletions.
240 changes: 240 additions & 0 deletions conf/airframes/krooz_1_0_quad_mkk.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,240 @@
<!DOCTYPE airframe SYSTEM "airframe.dtd">

<airframe name="Hexarotor Krooz 1.0 mkk">

<firmware name="rotorcraft">
<target name="ap" board="krooz_1.0">
</target>

<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target>

<subsystem name="radio_control" type="ppm"/>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="krooz_v1.0"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="euler"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="hff"/>
<subsystem name="actuators" type="mkk">
<configure name="MKK_I2C_SCL_TIME" value="150"/>
</subsystem>
<subsystem name="motor_mixing"/>

<define name="NO_RC_THRUST_LIMIT"/>
<define name="USE_RC_FP_BLOCK_SWITCHING"/>
<define name="USE_ATTITUDE_REF" value="0"/>
</firmware>

<!-- Using Geo Mag module -->
<modules main_freq="512">
<!--<load name="gps_ubx_ucenter.xml"/>
<load name="geo_mag.xml"/>-->
</modules>

<servos driver="Pwm">
<servo name="FRONT" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="BACK" no="1" min="1000" neutral="1000" max="2000"/>
<servo name="RIGHT" no="2" min="1000" neutral="1000" max="2000"/>
<servo name="LEFT" no="3" min="1000" neutral="1000" max="2000"/>
</servos>

<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>

<section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
<define name="NB" value="4"/>
<define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
</section>

<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="FRONT" value="motor_mixing.commands[SERVO_FRONT]"/>
<set servo="BACK" value="motor_mixing.commands[SERVO_BACK]"/>
<set servo="RIGHT" value="motor_mixing.commands[SERVO_RIGHT]"/>
<set servo="LEFT" value="motor_mixing.commands[SERVO_LEFT]"/>
</command_laws>

<section name="VERSION" prefix="VERSION_">
<define name="MAJOR" value="1"/>
<define name="MINOR" value="2"/>
</section>

<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="512"/>
<define name="ROLL_COEF" value="{ 0, MOTOR_MIXING_SCALE, 0, -MOTOR_MIXING_SCALE }"/>
<define name="PITCH_COEF" value="{ MOTOR_MIXING_SCALE, 0, -MOTOR_MIXING_SCALE, 0 }"/>
<define name="YAW_COEF" value="{ -MOTOR_MIXING_SCALE, MOTOR_MIXING_SCALE, -MOTOR_MIXING_SCALE, MOTOR_MIXING_SCALE }"/>
<define name="THRUST_COEF" value="{ MOTOR_MIXING_SCALE, MOTOR_MIXING_SCALE, MOTOR_MIXING_SCALE, MOTOR_MIXING_SCALE }"/>
</section>

<section name="IMU" prefix="IMU_">
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<define name="GYRO_P_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_Q_SENS" value=" 4.41" integer="16"/>
<define name="GYRO_R_SENS" value=" 4.41" integer="16"/>

<!-- replace this with your own calibration -->
<define name="ACCEL_X_NEUTRAL" value="-24"/>
<define name="ACCEL_Y_NEUTRAL" value="122"/>
<define name="ACCEL_Z_NEUTRAL" value="-130"/>
<define name="ACCEL_X_SENS" value="4.90555515454" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.90893349017" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.75238978393" integer="16"/>

<define name="MAG_X_NEUTRAL" value="-136"/>
<define name="MAG_Y_NEUTRAL" value="-103"/>
<define name="MAG_Z_NEUTRAL" value="6"/>
<define name="MAG_X_SENS" value="3.18492472198" integer="16"/>
<define name="MAG_Y_SENS" value="3.42471474617" integer="16"/>
<define name="MAG_Z_SENS" value="3.42088446936" integer="16"/>

<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>

<section name="AHRS" prefix="AHRS_">
<define name="H_X" value="0.3586845"/>
<define name="H_Y" value="0.0168651"/>
<define name="H_Z" value="0.933303"/>
</section>

<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="20." integer="16"/>
</section>

<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>

<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>

<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>

<!-- feedforward -->
<define name="DDGAIN_P" value="300"/>
<define name="DDGAIN_Q" value="300"/>
<define name="DDGAIN_R" value="300"/>
</section>


<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>

<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>

<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>

<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>

<!-- feedback -->
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="250"/>
<define name="PHI_IGAIN" value="250"/>

<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="250"/>
<define name="THETA_IGAIN" value="250"/>

<define name="PSI_PGAIN" value="4000"/>
<define name="PSI_DGAIN" value="1000"/>
<define name="PSI_IGAIN" value="300"/>

<!-- feedforward -->
<define name="PHI_DDGAIN" value="300"/>
<define name="THETA_DDGAIN" value="300"/>
<define name="PSI_DDGAIN" value="2000"/>
</section>

<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="TILT_COEFF" value="BFP_OF_REAL(0.25, 8)"/>
<define name="REF_MIN_ZDD" value="-0.2*9.81"/> <!-- max descent acceleration -->
<define name="REF_MAX_ZDD" value="0.2*9.81"/> <!-- max climb acceleration -->
<define name="REF_MIN_ZD" value="-3."/> <!-- max descent speed -->
<define name="REF_MAX_ZD" value="3."/> <!-- max climb speed -->
<define name="HOVER_KP" value="120"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<define name="RC_CLIMB_COEF" value ="500"/>
<define name="RC_DESCENT_COEF" value ="200"/>
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="USE_REF" value="1"/>
<define name="USE_SPEED_REF" value="1"/> <!-- using RC to control horizontal setpoint -->
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="MAX_SPEED" value="SPEED_BFP_OF_REAL(3.)"/>
<define name="REF_MAX_ACCEL" value=".7"/> <!-- max reference horizontal acceleration in m/s2 -->
<define name="REF_MAX_SPEED" value="1."/> <!-- max reference horizontal speed in m/s -->
<define name="RC_SPEED_DEAD_BAND" value="200000"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="120"/>
<define name="IGAIN" value="10"/>
</section>

<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</section>

<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>

<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="12.8" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="13.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="13.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="16.8" unit="V"/>
</section>

</airframe>
1 change: 1 addition & 0 deletions conf/firmwares/subsystems/shared/actuators_mkk.makefile
100644 → 100755
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SC
else ifeq ($(ARCH), stm32)
ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1
ap.CFLAGS += -DUSE_I2C1
ap.srcs += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
endif

# Simulator
Expand Down
13 changes: 12 additions & 1 deletion sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -880,8 +880,13 @@ void i2c1_hw_init(void) {

i2c1.reg_addr = (void *)I2C1;
i2c1.init_struct = NULL;
#if defined(STM32F1)
i2c1.scl_pin = GPIO_I2C1_SCL;
i2c1.sda_pin = GPIO_I2C1_SDA;
#elif defined(STM32F4)
i2c2.scl_pin = GPIO8;
i2c2.sda_pin = GPIO9;
#endif
i2c1.errors = &i2c1_errors;
i2c1_watchdog_counter = 0;

Expand Down Expand Up @@ -911,11 +916,17 @@ void i2c1_hw_init(void) {
/* Enable I2C1 clock */
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_I2C1EN);
/* Enable GPIOB clock */
#if defined(STM32F1)
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPBEN);

gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_2_MHZ,
GPIO_CNF_OUTPUT_ALTFN_OPENDRAIN,
i2c1.scl_pin | i2c1.sda_pin);
#elif defined(STM32F4)
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPBEN);
gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, i2c1.scl_pin | i2c1.sda_pin);
gpio_set_output_options(GPIOB, GPIO_OTYPE_OD, GPIO_OSPEED_25MHZ, i2c1.scl_pin | i2c1.sda_pin);
gpio_set_af(GPIOB, GPIO_AF4, i2c1.scl_pin | i2c1.sda_pin);
#endif

i2c_reset(I2C1);

Expand Down
12 changes: 11 additions & 1 deletion sw/airborne/firmwares/rotorcraft/autopilot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#include "firmwares/rotorcraft/navigation.h"
#include "firmwares/rotorcraft/guidance.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "subsystems/ins.h"
#include "led.h"

uint8_t autopilot_mode;
Expand Down Expand Up @@ -217,6 +218,12 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {

}

#ifndef AP_IN_FLIGHT_MIN_SPEED
#define AP_IN_FLIGHT_MIN_SPEED SPEED_BFP_OF_REAL(0.2)
#endif
#ifndef AP_IN_FLIGHT_MIN_ACCEL
#define AP_IN_FLIGHT_MIN_ACCEL ACCEL_BFP_OF_REAL(1.0)
#endif

static inline void autopilot_check_in_flight( bool_t motors_on ) {
if (autopilot_in_flight) {
Expand All @@ -226,7 +233,10 @@ static inline void autopilot_check_in_flight( bool_t motors_on ) {
return;
}
if (autopilot_in_flight_counter > 0) {
if (autopilot_mode != AP_MODE_HOVER_Z_HOLD && autopilot_mode != AP_MODE_NAV && THROTTLE_STICK_DOWN()) {
if (THROTTLE_STICK_DOWN() && ((autopilot_mode == AP_MODE_HOVER_Z_HOLD
&& (abs(ins_ltp_speed.z) < AP_IN_FLIGHT_MIN_SPEED)
&& (abs(ins_ltp_accel.z) < AP_IN_FLIGHT_MIN_ACCEL))
|| (autopilot_mode != AP_MODE_NAV && autopilot_mode != AP_MODE_HOVER_Z_HOLD))) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot_in_flight = FALSE;
Expand Down
Loading

0 comments on commit 00790cd

Please sign in to comment.