mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-27 17:06:31 +08:00
Merge pull request #1352 from paparazzi/flip_guidance
[guidance] Add a flip guidance mode
This commit is contained in:
Felix Ruess
@@ -0,0 +1,248 @@
|
||||
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
|
||||
|
||||
<airframe name="bebop_indi">
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="bebop"/>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<subsystem name="fdm" type="jsbsim"/>
|
||||
</target>
|
||||
|
||||
<!--define name="USE_SONAR" value="TRUE"/-->
|
||||
|
||||
<!-- Subsystem section -->
|
||||
<subsystem name="telemetry" type="transparent_udp"/>
|
||||
<subsystem name="radio_control" type="datalink"/>
|
||||
<subsystem name="motor_mixing"/>
|
||||
<subsystem name="actuators" type="bebop"/>
|
||||
<subsystem name="imu" type="bebop"/>
|
||||
<subsystem name="gps" type="furuno"/>
|
||||
<subsystem name="stabilization" type="indi"/>
|
||||
<subsystem name="ahrs" type="float_mlkf"/>
|
||||
<subsystem name="ahrs" type="int_cmpl_quat">
|
||||
<configure name="USE_MAGNETOMETER" value="FALSE"/>
|
||||
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
|
||||
<configure name="SECONDARY_AHRS" value="int_cmpl_quat"/>
|
||||
</subsystem>
|
||||
<subsystem name="ins" type="extended"/>
|
||||
</firmware>
|
||||
|
||||
<modules main_freq="512">
|
||||
<load name="geo_mag.xml"/>
|
||||
<load name="air_data.xml"/>
|
||||
<load name="send_imu_mag_current.xml"/>
|
||||
<load name="logger_file.xml">
|
||||
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
|
||||
</load>
|
||||
</modules>
|
||||
|
||||
<commands>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="6000"/>
|
||||
</commands>
|
||||
|
||||
<section name="gyro_scaling">
|
||||
<define name="BEBOP_GYRO_RANGE" value="MPU60X0_GYRO_RANGE_2000"/>
|
||||
<define name="IMU_GYRO_P_SENS" value="4.359"/>
|
||||
<define name="IMU_GYRO_Q_SENS" value="4.359"/>
|
||||
<define name="IMU_GYRO_R_SENS" value="4.359"/>
|
||||
<define name="IMU_GYRO_P_SENS_NUM" value="4359"/>
|
||||
<define name="IMU_GYRO_Q_SENS_NUM" value="4359"/>
|
||||
<define name="IMU_GYRO_R_SENS_NUM" value="4359"/>
|
||||
<define name="IMU_GYRO_P_SENS_DEN" value="1000"/>
|
||||
<define name="IMU_GYRO_Q_SENS_DEN" value="1000"/>
|
||||
<define name="IMU_GYRO_R_SENS_DEN" value="1000"/>
|
||||
</section>
|
||||
|
||||
<servos driver="Default">
|
||||
<servo name="TOP_RIGHT" no="0" min="3000" neutral="3000" max="12000"/>
|
||||
<servo name="TOP_LEFT" no="1" min="3000" neutral="3000" max="12000"/>
|
||||
<servo name="BOTTOM_LEFT" no="2" min="3000" neutral="3000" max="12000"/>
|
||||
<servo name="BOTTOM_RIGHT" no="3" min="3000" neutral="3000" max="12000"/>
|
||||
</servos>
|
||||
|
||||
<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="TYPE" value="QUAD_X"/>
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
|
||||
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
|
||||
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
|
||||
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
|
||||
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="AIR_DATA" prefix="AIR_DATA_">
|
||||
<define name="CALC_AIRSPEED" value="FALSE"/>
|
||||
<define name="CALC_TAS_FACTOR" value="FALSE"/>
|
||||
<define name="CALC_AMSL_BARO" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<!-- Magneto calibration -->
|
||||
<define name="MAG_X_NEUTRAL" value="18"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="157"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="49"/>
|
||||
<define name="MAG_X_SENS" value="10.5007722373" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="11.1147400462" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="11.6479371722" integer="16"/>
|
||||
|
||||
<!-- Magneto current calibration -->
|
||||
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
|
||||
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
|
||||
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
|
||||
|
||||
<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>
|
||||
|
||||
<!-- local magnetic field -->
|
||||
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
|
||||
<!-- Toulouse -->
|
||||
<!--define name="H_X" value="0.513081"/>
|
||||
<define name="H_Y" value="-0.00242783"/>
|
||||
<define name="H_Z" value="0.858336"/-->
|
||||
<!-- Delft -->
|
||||
<define name="H_X" value="0.3892503"/>
|
||||
<define name="H_Y" value="0.0017972"/>
|
||||
<define name="H_Z" value="0.9211303"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="SONAR_MAX_RANGE" value="2.2"/>
|
||||
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
|
||||
</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="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- setpoint limits for attitude stabilization rc flight -->
|
||||
<define name="SP_MAX_PHI" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_R" value="300" unit="deg/s"/>
|
||||
<define name="DEADBAND_A" value="0"/>
|
||||
<define name="DEADBAND_E" value="0"/>
|
||||
<define name="DEADBAND_R" value="50"/>
|
||||
</section>
|
||||
|
||||
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- attitude reference generation model -->
|
||||
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.9"/>
|
||||
<define name="REF_MAX_P" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.9"/>
|
||||
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.9"/>
|
||||
<define name="REF_MAX_R" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<define name="G1_P" value="0.0639"/>
|
||||
<define name="G1_Q" value="0.0361"/>
|
||||
<define name="G1_R" value="0.0022"/>
|
||||
<define name="G2_R" value="0.1450"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="600.0"/>
|
||||
<define name="REF_ERR_Q" value="600.0"/>
|
||||
<define name="REF_ERR_R" value="600.0"/>
|
||||
<define name="REF_RATE_P" value="28.0"/>
|
||||
<define name="REF_RATE_Q" value="28.0"/>
|
||||
<define name="REF_RATE_R" value="28.0"/>
|
||||
|
||||
<!-- second order filter parameters -->
|
||||
<define name="FILT_OMEGA" value="50.0"/>
|
||||
<define name="FILT_ZETA" value="0.55"/>
|
||||
<define name="FILT_OMEGA_R" value="50.0"/>
|
||||
<define name="FILT_ZETA_R" value="0.55"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN_P" value="0.1"/>
|
||||
<define name="ACT_DYN_Q" value="0.1"/>
|
||||
<define name="ACT_DYN_R" value="0.1"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="283"/>
|
||||
<define name="HOVER_KD" value="82"/>
|
||||
<define name="HOVER_KI" value="13"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<!-- Good weather -->
|
||||
<define name="MAX_BANK" value="20" unit="deg"/>
|
||||
<!-- Bad weather -->
|
||||
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
|
||||
<define name="PGAIN" value="79"/>
|
||||
<define name="DGAIN" value="100"/>
|
||||
<define name="IGAIN" value="30"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
|
||||
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_FLIP"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_FLIP"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
</section>
|
||||
</airframe>
|
||||
@@ -103,6 +103,7 @@ $(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_h_ref.c
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_v_ref.c
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_v_adapt.c
|
||||
$(TARGET).srcs += $(SRC_FIRMWARE)/guidance/guidance_flip.c
|
||||
|
||||
include $(CFG_ROTORCRAFT)/navigation.makefile
|
||||
|
||||
|
||||
+1
-1
@@ -1988,7 +1988,7 @@
|
||||
<field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/>
|
||||
<field name="frame_rate" type="uint8" unit="Hz"/>
|
||||
<field name="gps_status" type="uint8" values="NO_FIX|NA|2D|3D|DGPS|RTK"/>
|
||||
<field name="ap_mode" type="uint8" values="KILL|FAILSAFE|HOME|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV|RC_DIRECT|CARE_FREE|FORWARD|MODULE"/>
|
||||
<field name="ap_mode" type="uint8" values="KILL|FAILSAFE|HOME|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV|RC_DIRECT|CARE_FREE|FORWARD|MODULE|FLIP"/>
|
||||
<field name="ap_in_flight" type="uint8" values="ON_GROUND|IN_FLIGHT"/>
|
||||
<field name="ap_motors_on" type="uint8" values="MOTORS_OFF|MOTORS_ON"/>
|
||||
<field name="ap_h_mode" type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV|CF"/>
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
<dl_settings>
|
||||
|
||||
<dl_settings NAME="System">
|
||||
<dl_setting var="autopilot_mode_auto2" min="0" step="1" max="15" module="autopilot" shortname="auto2" values="KILL|Fail|HOME|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav|RC_D|CareFree"/>
|
||||
<dl_setting var="autopilot_mode_auto2" min="0" step="1" max="15" module="autopilot" shortname="auto2" values="KILL|Fail|HOME|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav|RC_D|CareFree|Forward|Module|Flip"/>
|
||||
<dl_setting var="kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
|
||||
<dl_setting var="autopilot_power_switch" min="0" step="1" max="1" module="autopilot" values="OFF|ON" handler="SetPowerSwitch">
|
||||
<strip_button name="POWER ON" icon="on.png" value="1" group="power_switch"/>
|
||||
|
||||
@@ -436,6 +436,9 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
|
||||
guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING);
|
||||
#endif
|
||||
break;
|
||||
case AP_MODE_FLIP:
|
||||
guidance_h_mode_changed(GUIDANCE_H_MODE_FLIP);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@@ -482,6 +485,9 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
|
||||
guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING);
|
||||
#endif
|
||||
break;
|
||||
case AP_MODE_FLIP:
|
||||
guidance_v_mode_changed(GUIDANCE_V_MODE_FLIP);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -51,6 +51,7 @@
|
||||
#define AP_MODE_CARE_FREE_DIRECT 15
|
||||
#define AP_MODE_FORWARD 16
|
||||
#define AP_MODE_MODULE 17
|
||||
#define AP_MODE_FLIP 18
|
||||
|
||||
extern uint8_t autopilot_mode;
|
||||
extern uint8_t autopilot_mode_auto2;
|
||||
|
||||
@@ -0,0 +1,135 @@
|
||||
/*
|
||||
* Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
|
||||
* 2015 Ewoud Smeur
|
||||
*
|
||||
* 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 firmwares/rotorcraft/guidance/guidance_flip.c
|
||||
*
|
||||
* Open Loop guidance for making a flip. You need to tune this before using.
|
||||
* When entering this mode it saves the previous guidance mode and changes AUTO2 back to
|
||||
* the previous mode after finishing the flip.
|
||||
* Use it with caution!
|
||||
*/
|
||||
|
||||
#include "guidance_flip.h"
|
||||
|
||||
#include "autopilot.h"
|
||||
#include "guidance_h.h"
|
||||
#include "stabilization/stabilization_attitude_rc_setpoint.h"
|
||||
#include "stabilization/stabilization_attitude.h"
|
||||
|
||||
#define STOP_ROLL_CMD_ANGLE 25.0
|
||||
#define FIRST_THRUST_DURATION 0.3
|
||||
#define FINAL_THRUST_LEVEL 6000
|
||||
|
||||
uint32_t flip_counter;
|
||||
uint8_t flip_state;
|
||||
bool_t flip_rollout;
|
||||
int32_t heading_save;
|
||||
uint8_t autopilot_mode_old;
|
||||
struct Int32Vect2 flip_cmd_earth;
|
||||
|
||||
void guidance_flip_enter(void)
|
||||
{
|
||||
flip_counter = 0;
|
||||
flip_state = 0;
|
||||
flip_rollout = false;
|
||||
heading_save = stabilization_attitude_get_heading_i();
|
||||
autopilot_mode_old = autopilot_mode;
|
||||
}
|
||||
|
||||
void guidance_flip_run(void)
|
||||
{
|
||||
uint32_t timer;
|
||||
int32_t phi;
|
||||
static uint32_t timer_save = 0;
|
||||
|
||||
timer = (flip_counter++ << 12) / PERIODIC_FREQUENCY;
|
||||
phi = stateGetNedToBodyEulers_i()->phi;
|
||||
|
||||
switch (flip_state) {
|
||||
case 0:
|
||||
flip_cmd_earth.x = 0;
|
||||
flip_cmd_earth.y = 0;
|
||||
stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
|
||||
heading_save);
|
||||
stabilization_attitude_run(autopilot_in_flight);
|
||||
stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
|
||||
timer_save = 0;
|
||||
|
||||
if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) {
|
||||
flip_state++;
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
stabilization_cmd[COMMAND_ROLL] = 9000; // Rolling command
|
||||
stabilization_cmd[COMMAND_PITCH] = 0;
|
||||
stabilization_cmd[COMMAND_YAW] = 0;
|
||||
stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
|
||||
|
||||
if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
|
||||
flip_state++;
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
stabilization_cmd[COMMAND_ROLL] = 0;
|
||||
stabilization_cmd[COMMAND_PITCH] = 0;
|
||||
stabilization_cmd[COMMAND_YAW] = 0;
|
||||
stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
|
||||
|
||||
if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
|
||||
timer_save = timer;
|
||||
flip_state++;
|
||||
}
|
||||
break;
|
||||
|
||||
case 3:
|
||||
flip_cmd_earth.x = 0;
|
||||
flip_cmd_earth.y = 0;
|
||||
stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
|
||||
heading_save);
|
||||
stabilization_attitude_run(autopilot_in_flight);
|
||||
|
||||
stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling
|
||||
|
||||
if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) {
|
||||
flip_state++;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
autopilot_mode_auto2 = autopilot_mode_old;
|
||||
autopilot_set_mode(autopilot_mode_old);
|
||||
stab_att_sp_euler.psi = heading_save;
|
||||
flip_rollout = false;
|
||||
flip_counter = 0;
|
||||
timer_save = 0;
|
||||
flip_state = 0;
|
||||
|
||||
stabilization_cmd[COMMAND_ROLL] = 0;
|
||||
stabilization_cmd[COMMAND_PITCH] = 0;
|
||||
stabilization_cmd[COMMAND_YAW] = 0;
|
||||
stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
/*
|
||||
* Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
|
||||
* 2015 Ewoud Smeur
|
||||
*
|
||||
* 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 firmwares/rotorcraft/guidance/guidance_flip.h
|
||||
*
|
||||
* Open Loop guidance for making a flip. You need to tune this before using.
|
||||
* When entering this mode it saves the previous guidance mode and changes AUTO2 back to
|
||||
* the previous mode after finishing the flip.
|
||||
* Use it with caution!
|
||||
*/
|
||||
|
||||
#ifndef GUIDANCE_FLIP_H
|
||||
#define GUIDANCE_FLIP_H
|
||||
|
||||
void guidance_flip_enter(void);
|
||||
void guidance_flip_run(void);
|
||||
|
||||
#endif /* GUIDANCE_FLIP_H */
|
||||
@@ -27,6 +27,7 @@
|
||||
#include "generated/airframe.h"
|
||||
|
||||
#include "firmwares/rotorcraft/guidance/guidance_h.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_flip.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_module.h"
|
||||
#include "firmwares/rotorcraft/stabilization.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
|
||||
@@ -263,6 +264,10 @@ void guidance_h_mode_changed(uint8_t new_mode)
|
||||
stabilization_attitude_enter();
|
||||
break;
|
||||
|
||||
case GUIDANCE_H_MODE_FLIP:
|
||||
guidance_flip_enter();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@@ -317,13 +322,15 @@ void guidance_h_read_rc(bool_t in_flight)
|
||||
INT_EULERS_ZERO(guidance_h.rc_sp);
|
||||
}
|
||||
break;
|
||||
case GUIDANCE_H_MODE_FLIP:
|
||||
stabilization_attitude_read_rc(in_flight, FALSE, FALSE);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void guidance_h_run(bool_t in_flight)
|
||||
{
|
||||
switch (guidance_h.mode) {
|
||||
@@ -399,6 +406,10 @@ void guidance_h_run(bool_t in_flight)
|
||||
break;
|
||||
#endif
|
||||
|
||||
case GUIDANCE_H_MODE_FLIP:
|
||||
guidance_flip_run();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -58,6 +58,7 @@
|
||||
#define GUIDANCE_H_MODE_CARE_FREE 6
|
||||
#define GUIDANCE_H_MODE_FORWARD 7
|
||||
#define GUIDANCE_H_MODE_MODULE 8
|
||||
#define GUIDANCE_H_MODE_FLIP 9
|
||||
|
||||
|
||||
struct HorizontalGuidanceSetpoint {
|
||||
|
||||
@@ -248,6 +248,9 @@ void guidance_v_mode_changed(uint8_t new_mode)
|
||||
break;
|
||||
#endif
|
||||
|
||||
case GUIDANCE_V_MODE_FLIP:
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
|
||||
@@ -351,6 +354,10 @@ void guidance_v_run(bool_t in_flight)
|
||||
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
|
||||
break;
|
||||
}
|
||||
|
||||
case GUIDANCE_V_MODE_FLIP:
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -39,6 +39,7 @@
|
||||
#define GUIDANCE_V_MODE_HOVER 4
|
||||
#define GUIDANCE_V_MODE_NAV 5
|
||||
#define GUIDANCE_V_MODE_MODULE 6
|
||||
#define GUIDANCE_V_MODE_FLIP 7
|
||||
|
||||
extern uint8_t guidance_v_mode;
|
||||
|
||||
|
||||
@@ -1292,7 +1292,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph ->
|
||||
match ap_mode with
|
||||
"AUTO2" | "NAV" -> ok_color
|
||||
| "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" | "MODULE" -> "#10F0E0"
|
||||
| "MANUAL" | "RATE" | "ATT" | "RC_D" | "CF" | "FWD" -> warning_color
|
||||
| "MANUAL" | "RATE" | "ATT" | "RC_D" | "CF" | "FWD" | "FLIP" -> warning_color
|
||||
| _ -> alert_color in
|
||||
ac.strip#set_color "AP" color;
|
||||
end;
|
||||
|
||||
Reference in New Issue
Block a user