mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-29 02:38:07 +08:00
Merge branch 'dev' into dev_i2c
This commit is contained in:
Felix Ruess
@@ -213,7 +213,6 @@
|
||||
<firmware name="rotorcraft">
|
||||
<!-- <define name="USE_PERSISTENT_SETTINGS"/> -->
|
||||
<!-- <define name="USE_INS_NAV_INIT"/> -->
|
||||
<define name="USE_ADAPT_HOVER"/>
|
||||
<define name="FAILSAFE_GROUND_DETECT"/>
|
||||
<define name="USE_GPS_ACC4R"/>
|
||||
<target name="ap" board="lisa_l_1.0">
|
||||
|
||||
@@ -11,7 +11,6 @@
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<define name="USE_INS_NAV_INIT"/>
|
||||
<define name="USE_ADAPT_HOVER"/>
|
||||
<!--define name="GUIDANCE_H_USE_REF"/-->
|
||||
|
||||
<target name="ap" board="booz_1.0">
|
||||
|
||||
@@ -15,7 +15,6 @@
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<define name="USE_INS_NAV_INIT"/>
|
||||
<define name="USE_ADAPT_HOVER"/>
|
||||
<!--define name="GUIDANCE_H_USE_REF"/-->
|
||||
|
||||
<target name="ap" board="booz_1.0">
|
||||
|
||||
@@ -10,7 +10,6 @@
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<define name="USE_INS_NAV_INIT"/>
|
||||
<define name="USE_ADAPT_HOVER"/>
|
||||
<define name="USE_ATTITUDE_REF" value="0"/>
|
||||
<!--define name="GUIDANCE_H_USE_REF"/-->
|
||||
|
||||
@@ -105,7 +104,6 @@
|
||||
|
||||
<!-- Magnetic field for Toulouse (declination -16°) -->
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<define name="MAG_UPDATE_YAW_ONLY" value="1"/>
|
||||
<define name="PROPAGATE_FREQUENCY" value="512"/>
|
||||
<define name="H_X" value=" 0.513081"/>
|
||||
<define name="H_Y" value="-0.00242783"/>
|
||||
|
||||
@@ -242,7 +242,6 @@
|
||||
<firmware name="rotorcraft">
|
||||
<!-- <define name="USE_PERSISTENT_SETTINGS"/> -->
|
||||
<!-- <define name="USE_INS_NAV_INIT"/> -->
|
||||
<define name="USE_ADAPT_HOVER"/>
|
||||
<define name="FAILSAFE_GROUND_DETECT"/>
|
||||
<define name="USE_GPS_ACC4R"/>
|
||||
<target name="ap" board="lisa_l_1.0">
|
||||
|
||||
@@ -214,7 +214,6 @@
|
||||
<firmware name="rotorcraft">
|
||||
<!-- <define name="USE_PERSISTENT_SETTINGS"/> -->
|
||||
<!-- <define name="USE_INS_NAV_INIT"/> -->
|
||||
<define name="USE_ADAPT_HOVER"/>
|
||||
<define name="FAILSAFE_GROUND_DETECT"/>
|
||||
<define name="USE_GPS_ACC4R"/>
|
||||
<target name="ap" board="lisa_l_1.0">
|
||||
|
||||
@@ -16,6 +16,7 @@
|
||||
<define name="USE_INS_NAV_INIT"/>
|
||||
<configure name="AHRS_ALIGNER_LED" value="3"/>
|
||||
<configure name="USE_NEW_I2C_DRIVER" value="1"/>
|
||||
<define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING"/>
|
||||
</target>
|
||||
<target name="sim" board="pc">
|
||||
<subsystem name="fdm" type="nps"/>
|
||||
@@ -108,39 +109,38 @@
|
||||
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<define name="PROPAGATE_FREQUENCY" value="512"/>
|
||||
<define name="MAG_UPDATE_YAW_ONLY"/>
|
||||
<define name="H_X" value="0.3770441"/>
|
||||
<define name="H_Y" value="0.0193986"/>
|
||||
<define name="H_Z" value="0.9259921"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="BARO_SENS" value="3.3" integer="16"/>
|
||||
<define name="BARO_SENS" value="22.3" 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="0" />
|
||||
<define name="DEADBAND_Q" value="0" />
|
||||
<define name="DEADBAND_R" value="200" />
|
||||
<define name="REF_TAU" value="4" />
|
||||
<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="0"/>
|
||||
<define name="DEADBAND_Q" value="0"/>
|
||||
<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="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" />
|
||||
<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" />
|
||||
<define name="DDGAIN_P" value="300"/>
|
||||
<define name="DDGAIN_Q" value="300"/>
|
||||
<define name="DDGAIN_R" value="300"/>
|
||||
</section>
|
||||
|
||||
|
||||
@@ -204,7 +204,7 @@
|
||||
<define name="RC_CLIMB_COEF" value ="163"/>
|
||||
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
|
||||
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
|
||||
<!--define name="INV_M" value ="0.21"/-->
|
||||
<!--define name="NOMINAL_HOVER_THROTTLE" value="0.5"/-->
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
|
||||
@@ -2,11 +2,11 @@
|
||||
#
|
||||
# lisa_m_2.0.makefile
|
||||
#
|
||||
# http://paparazzi.enac.fr/wiki/Lisa/M
|
||||
# http://paparazzi.enac.fr/wiki/Lisa/M_v20
|
||||
#
|
||||
|
||||
BOARD=lisa_m
|
||||
BOARD_VERSION=1.0
|
||||
BOARD_VERSION=2.0
|
||||
BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
|
||||
|
||||
ARCH=stm32
|
||||
|
||||
@@ -0,0 +1,21 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<!--
|
||||
Battery checker module
|
||||
@define BAT_CHECKER_DELAY Number of seconds the battery voltage has to be
|
||||
below LOW_BAT_LEVEL before the warning signal is
|
||||
activated.
|
||||
@define BAT_CHECKER_LED The LED to use for the warning signal.
|
||||
-->
|
||||
<module name="bat_checker">
|
||||
<header>
|
||||
<file name="bat_checker.h"/>
|
||||
</header>
|
||||
<init fun="init_bat_checker()"/>
|
||||
<periodic fun="bat_checker_periodic()" freq="2" autorun="TRUE"/>
|
||||
<makefile>
|
||||
<define name="bat_checker_periodic_FREQ" value="2"/>
|
||||
<file name="bat_checker.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -1,19 +0,0 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="battery_buzzer">
|
||||
<header>
|
||||
<file name="battery_buzzer.h"/>
|
||||
</header>
|
||||
<init fun="battery_buzzer_init()"/>
|
||||
<periodic fun="battery_buzzer_periodic()" freq="5."/>
|
||||
<makefile target="ap">
|
||||
<file name="battery_buzzer.c"/>
|
||||
<raw>
|
||||
ifneq ($(ARCH), stm32)
|
||||
$(error The battery_buzzer module is currently only implemented for STM32 based boards.)
|
||||
endif
|
||||
</raw>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
|
||||
@@ -43,6 +43,7 @@
|
||||
<dl_setting var="guidance_v_kd" min="0" step="1" max="600" module="guidance/guidance_v" shortname="kd"/>
|
||||
<dl_setting var="guidance_v_ki" min="0" step="1" max="300" module="guidance/guidance_v" shortname="ki" handler="SetKi" />
|
||||
<dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
|
||||
<dl_setting var="guidance_v_nominal_throttle" min="0.1" step="0.01" max="0.9" module="guidance/guidance_v" shortname="nominal_throttle" handler="SetNominalHoverThrottle" alt_unit="%" alt_unit_coef="0.000104167"/>
|
||||
<dl_setting var="ins_vf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="vf_realign" values="OFF|ON"/>
|
||||
</dl_settings>
|
||||
|
||||
|
||||
@@ -24,6 +24,7 @@
|
||||
#define LED_3_GPIO_PIN GPIO_Pin_2
|
||||
#define LED_3_AFIO_REMAP ((void)0)
|
||||
|
||||
// GPIO pins
|
||||
#define LED_4_BANK
|
||||
#define LED_4_GPIO GPIOC
|
||||
#define LED_4_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
@@ -42,29 +43,49 @@
|
||||
#define IMU_ACC_DRDY_GPIO GPIOB
|
||||
#define IMU_ACC_DRDY_GPIO_PORTSOURCE GPIO_PortSourceGPIOB
|
||||
|
||||
/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
|
||||
#ifndef ADC_CHANNEL_VSUPPLY
|
||||
#define ADC_CHANNEL_VSUPPLY 2
|
||||
#endif
|
||||
|
||||
|
||||
#define DefaultVoltageOfAdc(adc) (0.00485*adc)
|
||||
|
||||
/* Onboard ADCs */
|
||||
/*
|
||||
ADC1 PC3/ADC13
|
||||
ADC2 PA0/ADC0
|
||||
ADC3 PC0/ADC10
|
||||
ADC4 PC1/ADC11
|
||||
ADC5 PC5/ADC15
|
||||
ADC6 PA1/ADC1
|
||||
ADC7 PC2/ADC12
|
||||
BATT PC4/ADC14
|
||||
ADC_1 PC3/ADC13
|
||||
ADC_2 PC0/ADC10
|
||||
ADC_3 PC1/ADC11
|
||||
ADC_4 PC5/ADC15
|
||||
ADC_6 PC2/ADC12
|
||||
BATT PC4/ADC14
|
||||
*/
|
||||
#define BOARD_ADC_CHANNEL_1 ADC_Channel_13
|
||||
#define BOARD_ADC_CHANNEL_2 ADC_Channel_0
|
||||
// FIXME - removed for now and used for battery monitoring
|
||||
//#define BOARD_ADC_CHANNEL_3 ADC_Channel_10
|
||||
#define BOARD_ADC_CHANNEL_3 ADC_Channel_14
|
||||
#define BOARD_ADC_CHANNEL_4 ADC_Channel_11
|
||||
#define BOARD_ADC_CHANNEL_2 ADC_Channel_10
|
||||
#define BOARD_ADC_CHANNEL_3 ADC_Channel_11
|
||||
// we can only use ADC1,2,3; the last channel is for bat monitoring
|
||||
#define BOARD_ADC_CHANNEL_4 ADC_Channel_14
|
||||
|
||||
/* provide defines that can be used to access the ADC_x in the code or airframe file
|
||||
* these directly map to the index number of the 4 adc channels defined above
|
||||
* 4th (index 3) is used for bat monitoring by default
|
||||
*/
|
||||
#define ADC_1 0
|
||||
#define ADC_2 1
|
||||
#define ADC_3 2
|
||||
|
||||
/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
|
||||
#ifndef ADC_CHANNEL_VSUPPLY
|
||||
#define ADC_CHANNEL_VSUPPLY 3
|
||||
#endif
|
||||
|
||||
/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */
|
||||
// FIXME, this is not very nice, is also stm lib specific
|
||||
#ifdef USE_AD1
|
||||
#define ADC1_GPIO_INIT(gpio) { \
|
||||
(gpio).GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_4; \
|
||||
(gpio).GPIO_Mode = GPIO_Mode_AIN; \
|
||||
GPIO_Init(GPIOC, (&gpio)); \
|
||||
}
|
||||
#endif // USE_AD1
|
||||
|
||||
|
||||
|
||||
#define BOARD_HAS_BARO 1
|
||||
|
||||
|
||||
@@ -0,0 +1,107 @@
|
||||
#ifndef CONFIG_LISA_M_2_0_H
|
||||
#define CONFIG_LISA_M_2_0_H
|
||||
|
||||
#define BOARD_LISA_M
|
||||
|
||||
#define AHB_CLK 72000000
|
||||
|
||||
/* Onboard LEDs */
|
||||
#define LED_1_BANK
|
||||
#define LED_1_GPIO GPIOB
|
||||
#define LED_1_GPIO_CLK RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO
|
||||
#define LED_1_GPIO_PIN GPIO_Pin_4
|
||||
#define LED_1_AFIO_REMAP GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE)
|
||||
|
||||
/* shared with ADC15 */
|
||||
#define LED_2_BANK
|
||||
#define LED_2_GPIO GPIOC
|
||||
#define LED_2_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
#define LED_2_GPIO_PIN GPIO_Pin_5
|
||||
#define LED_2_AFIO_REMAP ((void)0)
|
||||
|
||||
#define LED_3_BANK
|
||||
#define LED_3_GPIO GPIOC
|
||||
#define LED_3_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
#define LED_3_GPIO_PIN GPIO_Pin_2
|
||||
#define LED_3_AFIO_REMAP ((void)0)
|
||||
|
||||
#define LED_4_BANK
|
||||
#define LED_4_GPIO GPIOC
|
||||
#define LED_4_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
#define LED_4_GPIO_PIN GPIO_Pin_12
|
||||
#define LED_4_AFIO_REMAP ((void)0)
|
||||
|
||||
#define LED_5_BANK
|
||||
#define LED_5_GPIO GPIOC
|
||||
#define LED_5_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
#define LED_5_GPIO_PIN GPIO_Pin_10
|
||||
#define LED_5_AFIO_REMAP ((void)0)
|
||||
|
||||
|
||||
/* configuration for aspirin - and more generaly IMUs */
|
||||
#define IMU_ACC_DRDY_RCC_GPIO RCC_APB2Periph_GPIOB
|
||||
#define IMU_ACC_DRDY_GPIO GPIOB
|
||||
#define IMU_ACC_DRDY_GPIO_PORTSOURCE GPIO_PortSourceGPIOB
|
||||
|
||||
|
||||
|
||||
#define DefaultVoltageOfAdc(adc) (0.00485*adc)
|
||||
|
||||
/* Onboard ADCs */
|
||||
/*
|
||||
ADC1 PC3/ADC13
|
||||
ADC2 PC0/ADC10
|
||||
ADC3 PC1/ADC11
|
||||
ADC4 PC5/ADC15
|
||||
ADC6 PC2/ADC12
|
||||
BATT PC4/ADC14
|
||||
*/
|
||||
#define BOARD_ADC_CHANNEL_1 ADC_Channel_13
|
||||
#define BOARD_ADC_CHANNEL_2 ADC_Channel_10
|
||||
#define BOARD_ADC_CHANNEL_3 ADC_Channel_11
|
||||
// we can only use ADC1,2,3; the last channel is for bat monitoring
|
||||
#define BOARD_ADC_CHANNEL_4 ADC_Channel_14
|
||||
|
||||
/* provide defines that can be used to access the ADC_x in the code or airframe file
|
||||
* these directly map to the index number of the 4 adc channels defined above
|
||||
* 4th (index 3) is used for bat monitoring by default
|
||||
*/
|
||||
#define ADC_1 0
|
||||
#define ADC_2 1
|
||||
#define ADC_3 2
|
||||
|
||||
/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
|
||||
#ifndef ADC_CHANNEL_VSUPPLY
|
||||
#define ADC_CHANNEL_VSUPPLY 3
|
||||
#endif
|
||||
|
||||
/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */
|
||||
// FIXME, this is not very nice, is also stm lib specific
|
||||
#ifdef USE_AD1
|
||||
#define ADC1_GPIO_INIT(gpio) { \
|
||||
(gpio).GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_4; \
|
||||
(gpio).GPIO_Mode = GPIO_Mode_AIN; \
|
||||
GPIO_Init(GPIOC, (&gpio)); \
|
||||
}
|
||||
#endif // USE_AD1
|
||||
|
||||
|
||||
|
||||
#define BOARD_HAS_BARO 1
|
||||
|
||||
#define USE_OPENCM3 1
|
||||
|
||||
// not needed with USE_OPENCM3:
|
||||
//#define HSE_TYPE_EXT_CLK
|
||||
//#define STM32_RCC_MODE RCC_HSE_ON
|
||||
//#define STM32_PLL_MULT RCC_PLLMul_6
|
||||
|
||||
#define PWM_5AND6_TIMER TIM5
|
||||
#define PWM_5AND6_RCC RCC_APB1Periph_TIM5
|
||||
#define PWM5_OC 1
|
||||
#define PWM6_OC 2
|
||||
#define PWM_5AND6_GPIO GPIOA
|
||||
#define PWM5_Pin GPIO_Pin_0
|
||||
#define PWM6_Pin GPIO_Pin_1
|
||||
|
||||
#endif /* CONFIG_LISA_M_2_0_H */
|
||||
@@ -1,7 +1,5 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
* Copyright (C) 2008-2012 The Paparazzi Team
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -19,7 +17,6 @@
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "firmwares/rotorcraft/autopilot.h"
|
||||
@@ -33,41 +30,47 @@
|
||||
|
||||
uint8_t autopilot_mode;
|
||||
uint8_t autopilot_mode_auto2;
|
||||
bool_t autopilot_motors_on;
|
||||
bool_t autopilot_in_flight;
|
||||
uint32_t autopilot_motors_on_counter;
|
||||
uint32_t autopilot_in_flight_counter;
|
||||
uint8_t autopilot_check_motor_status;
|
||||
bool_t kill_throttle;
|
||||
bool_t autopilot_rc;
|
||||
|
||||
bool_t autopilot_in_flight;
|
||||
uint32_t autopilot_in_flight_counter;
|
||||
uint16_t autopilot_flight_time;
|
||||
|
||||
bool_t autopilot_motors_on;
|
||||
bool_t kill_throttle;
|
||||
|
||||
bool_t autopilot_rc;
|
||||
bool_t autopilot_power_switch;
|
||||
|
||||
bool_t autopilot_detect_ground;
|
||||
bool_t autopilot_detect_ground_once;
|
||||
|
||||
uint16_t autopilot_flight_time;
|
||||
|
||||
#define AUTOPILOT_MOTOR_ON_TIME 40
|
||||
#define AUTOPILOT_IN_FLIGHT_TIME 40
|
||||
#define AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20)
|
||||
#define AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 19 / 20)
|
||||
// Motors ON check state machine
|
||||
#define STATUS_MOTORS_OFF 0
|
||||
#define STATUS_M_OFF_STICK_PUSHED 1
|
||||
#define STATUS_START_MOTORS 2
|
||||
#define STATUS_MOTORS_ON 3
|
||||
#define STATUS_M_ON_STICK_PUSHED 4
|
||||
#define STATUS_STOP_MOTORS 5
|
||||
|
||||
#ifndef AUTOPILOT_DISABLE_AHRS_KILL
|
||||
#include "subsystems/ahrs.h"
|
||||
static inline int ahrs_is_aligned(void) {
|
||||
return (ahrs.status == AHRS_RUNNING);
|
||||
}
|
||||
#else
|
||||
static inline int ahrs_is_aligned(void) {
|
||||
return TRUE;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if USE_KILL_SWITCH_FOR_MOTOR_ARMING
|
||||
#include "autopilot_arming_switch.h"
|
||||
#elif USE_THROTTLE_FOR_MOTOR_ARMING
|
||||
#include "autopilot_arming_throttle.h"
|
||||
#else
|
||||
#include "autopilot_arming_yaw.h"
|
||||
#endif
|
||||
|
||||
void autopilot_init(void) {
|
||||
autopilot_mode = AP_MODE_KILL;
|
||||
autopilot_motors_on = FALSE;
|
||||
autopilot_in_flight = FALSE;
|
||||
kill_throttle = ! autopilot_motors_on;
|
||||
autopilot_motors_on_counter = 0;
|
||||
autopilot_in_flight = FALSE;
|
||||
autopilot_in_flight_counter = 0;
|
||||
autopilot_check_motor_status = STATUS_MOTORS_OFF;
|
||||
autopilot_mode_auto2 = MODE_AUTO2;
|
||||
autopilot_detect_ground = FALSE;
|
||||
autopilot_detect_ground_once = FALSE;
|
||||
@@ -77,6 +80,7 @@ void autopilot_init(void) {
|
||||
#ifdef POWER_SWITCH_LED
|
||||
LED_ON(POWER_SWITCH_LED); // POWER OFF
|
||||
#endif
|
||||
autopilot_arming_init();
|
||||
}
|
||||
|
||||
|
||||
@@ -187,12 +191,6 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
|
||||
|
||||
}
|
||||
|
||||
#define THROTTLE_STICK_DOWN() \
|
||||
(radio_control.values[RADIO_THROTTLE] < AUTOPILOT_THROTTLE_TRESHOLD)
|
||||
#define YAW_STICK_PUSHED() \
|
||||
(radio_control.values[RADIO_YAW] > AUTOPILOT_YAW_TRESHOLD || \
|
||||
radio_control.values[RADIO_YAW] < -AUTOPILOT_YAW_TRESHOLD)
|
||||
|
||||
|
||||
static inline void autopilot_check_in_flight( bool_t motors_on ) {
|
||||
if (autopilot_in_flight) {
|
||||
@@ -223,77 +221,14 @@ static inline void autopilot_check_in_flight( bool_t motors_on ) {
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef AUTOPILOT_KILL_WITHOUT_AHRS
|
||||
#include "subsystems/ahrs.h"
|
||||
static inline int ahrs_is_aligned(void) {
|
||||
return (ahrs.status == AHRS_RUNNING);
|
||||
}
|
||||
#else
|
||||
static inline int ahrs_is_aligned(void) {
|
||||
return TRUE;
|
||||
}
|
||||
#endif
|
||||
|
||||
/** Set motors ON or OFF and change the status of the check_motors state machine
|
||||
*/
|
||||
void autopilot_set_motors_on(bool_t motors_on) {
|
||||
autopilot_motors_on = motors_on;
|
||||
if (ahrs_is_aligned() && motors_on)
|
||||
autopilot_motors_on = TRUE;
|
||||
else
|
||||
autopilot_motors_on = FALSE;
|
||||
kill_throttle = ! autopilot_motors_on;
|
||||
if (autopilot_motors_on) autopilot_check_motor_status = STATUS_MOTORS_ON;
|
||||
else autopilot_check_motor_status = STATUS_MOTORS_OFF;
|
||||
}
|
||||
|
||||
/**
|
||||
* State machine to check if motors should be turned ON or OFF
|
||||
* The motors start/stop when pushing the yaw stick without throttle during a given time
|
||||
* An intermediate state prevents oscillating between ON and OFF while keeping the stick pushed
|
||||
* The stick must return to a neutral position before starting/stoping again
|
||||
*/
|
||||
static inline void autopilot_check_motors_on( void ) {
|
||||
switch(autopilot_check_motor_status) {
|
||||
case STATUS_MOTORS_OFF:
|
||||
autopilot_motors_on = FALSE;
|
||||
autopilot_motors_on_counter = 0;
|
||||
if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed
|
||||
autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED;
|
||||
break;
|
||||
case STATUS_M_OFF_STICK_PUSHED:
|
||||
autopilot_motors_on = FALSE;
|
||||
autopilot_motors_on_counter++;
|
||||
if (autopilot_motors_on_counter >= AUTOPILOT_MOTOR_ON_TIME)
|
||||
autopilot_check_motor_status = STATUS_START_MOTORS;
|
||||
else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon
|
||||
autopilot_check_motor_status = STATUS_MOTORS_OFF;
|
||||
break;
|
||||
case STATUS_START_MOTORS:
|
||||
autopilot_motors_on = TRUE;
|
||||
autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME;
|
||||
if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released
|
||||
autopilot_check_motor_status = STATUS_MOTORS_ON;
|
||||
break;
|
||||
case STATUS_MOTORS_ON:
|
||||
autopilot_motors_on = TRUE;
|
||||
autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME;
|
||||
if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed
|
||||
autopilot_check_motor_status = STATUS_M_ON_STICK_PUSHED;
|
||||
break;
|
||||
case STATUS_M_ON_STICK_PUSHED:
|
||||
autopilot_motors_on = TRUE;
|
||||
autopilot_motors_on_counter--;
|
||||
if (autopilot_motors_on_counter == 0)
|
||||
autopilot_check_motor_status = STATUS_STOP_MOTORS;
|
||||
else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon
|
||||
autopilot_check_motor_status = STATUS_MOTORS_ON;
|
||||
break;
|
||||
case STATUS_STOP_MOTORS:
|
||||
autopilot_motors_on = FALSE;
|
||||
autopilot_motors_on_counter = 0;
|
||||
if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released
|
||||
autopilot_check_motor_status = STATUS_MOTORS_OFF;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
};
|
||||
autopilot_arming_set(autopilot_motors_on);
|
||||
}
|
||||
|
||||
|
||||
@@ -303,30 +238,18 @@ void autopilot_on_rc_frame(void) {
|
||||
AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode);
|
||||
autopilot_set_mode(new_autopilot_mode);
|
||||
|
||||
#ifdef RADIO_KILL_SWITCH
|
||||
if (radio_control.values[RADIO_KILL_SWITCH] < 0)
|
||||
if (kill_switch_is_on())
|
||||
autopilot_set_mode(AP_MODE_KILL);
|
||||
#endif
|
||||
|
||||
#ifdef AUTOPILOT_KILL_WITHOUT_AHRS
|
||||
if (!ahrs_is_aligned())
|
||||
autopilot_set_mode(AP_MODE_KILL);
|
||||
#endif
|
||||
|
||||
#ifdef NO_ARMING_SEQUENCE
|
||||
#ifndef RADIO_KILL_SWITCH
|
||||
// no arming sequence and no kill switch
|
||||
// motors are turned on when "in_flight" is detected
|
||||
// it can also be set by the flight plan
|
||||
autopilot_check_in_flight(TRUE);
|
||||
autopilot_motors_on = autopilot_in_flight;
|
||||
#endif
|
||||
#else
|
||||
// an arming sequence is used to start motors
|
||||
autopilot_check_motors_on();
|
||||
// an arming sequence is used to start/stop motors
|
||||
autopilot_arming_check_motors_on();
|
||||
|
||||
kill_throttle = ! autopilot_motors_on;
|
||||
|
||||
autopilot_check_in_flight(autopilot_motors_on);
|
||||
#endif
|
||||
kill_throttle = !autopilot_motors_on;
|
||||
|
||||
if (autopilot_mode > AP_MODE_FAILSAFE) {
|
||||
guidance_v_read_rc();
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
@@ -19,7 +17,6 @@
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef AUTOPILOT_H
|
||||
@@ -50,8 +47,8 @@
|
||||
|
||||
extern uint8_t autopilot_mode;
|
||||
extern uint8_t autopilot_mode_auto2;
|
||||
extern bool_t autopilot_motors_on;
|
||||
extern bool_t autopilot_in_flight;
|
||||
extern bool_t autopilot_motors_on;
|
||||
extern bool_t autopilot_in_flight;
|
||||
extern bool_t kill_throttle;
|
||||
extern bool_t autopilot_rc;
|
||||
|
||||
@@ -68,6 +65,8 @@ extern bool_t autopilot_detect_ground_once;
|
||||
|
||||
extern uint16_t autopilot_flight_time;
|
||||
|
||||
|
||||
|
||||
#ifndef MODE_MANUAL
|
||||
#define MODE_MANUAL AP_MODE_RATE_DIRECT
|
||||
#endif
|
||||
@@ -91,10 +90,8 @@ extern uint16_t autopilot_flight_time;
|
||||
_mode = MODE_MANUAL; \
|
||||
}
|
||||
|
||||
#define autopilot_KillThrottle(_v) { \
|
||||
kill_throttle = _v; \
|
||||
if (kill_throttle) autopilot_motors_on = FALSE; \
|
||||
else autopilot_motors_on = TRUE; \
|
||||
#define autopilot_KillThrottle(_v) { \
|
||||
autopilot_set_motors_on(_v); \
|
||||
}
|
||||
|
||||
#ifdef POWER_SWITCH_LED
|
||||
|
||||
@@ -0,0 +1,96 @@
|
||||
/*
|
||||
* Copyright (C) 2012 The Paparazzi Team
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef AUTOPILOT_ARMING_SWITCH_H
|
||||
#define AUTOPILOT_ARMING_SWITCH_H
|
||||
|
||||
#include "autopilot_rc_helpers.h"
|
||||
|
||||
#ifndef RADIO_KILL_SWITCH
|
||||
#error "You need to have a RADIO_KILL_SWITCH configured to arm the motors with the switch!"
|
||||
#endif
|
||||
|
||||
enum arming_state {
|
||||
STATE_UNINIT,
|
||||
STATE_WAITING,
|
||||
STATE_STARTABLE,
|
||||
STATE_MOTORS_ON
|
||||
};
|
||||
|
||||
enum arming_state autopilot_arming_state;
|
||||
|
||||
static inline void autopilot_arming_init(void) {
|
||||
autopilot_arming_state = STATE_UNINIT;
|
||||
}
|
||||
|
||||
static inline void autopilot_arming_set(bool_t motors_on) {
|
||||
if (motors_on) {
|
||||
autopilot_arming_state = STATE_MOTORS_ON;
|
||||
}
|
||||
else {
|
||||
if (autopilot_arming_state == STATE_MOTORS_ON)
|
||||
autopilot_arming_state = STATE_STARTABLE;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* State machine to check if motors should be turned ON or OFF using the kill switch.
|
||||
* If kill switch is off during startup (unkilled), you need to kill again first,
|
||||
* then unkill to start.
|
||||
* Also to start the motors, throttle needs to be down, other sticks centered,
|
||||
* AHRS aligned and you need be in manual mode.
|
||||
*/
|
||||
static inline void autopilot_arming_check_motors_on( void ) {
|
||||
switch(autopilot_arming_state) {
|
||||
case STATE_UNINIT:
|
||||
autopilot_motors_on = FALSE;
|
||||
if (kill_switch_is_on())
|
||||
autopilot_arming_state = STATE_STARTABLE;
|
||||
else
|
||||
autopilot_arming_state = STATE_WAITING;
|
||||
break;
|
||||
case STATE_WAITING:
|
||||
autopilot_motors_on = FALSE;
|
||||
if (kill_switch_is_on())
|
||||
autopilot_arming_state = STATE_STARTABLE;
|
||||
break;
|
||||
case STATE_STARTABLE:
|
||||
autopilot_motors_on = FALSE;
|
||||
if (!kill_switch_is_on() &&
|
||||
THROTTLE_STICK_DOWN() &&
|
||||
rc_attitude_sticks_centered() &&
|
||||
autopilot_mode == MODE_MANUAL &&
|
||||
ahrs_is_aligned()) {
|
||||
autopilot_arming_state = STATE_MOTORS_ON;
|
||||
}
|
||||
break;
|
||||
case STATE_MOTORS_ON:
|
||||
autopilot_motors_on = TRUE;
|
||||
if (kill_switch_is_on())
|
||||
autopilot_arming_state = STATE_STARTABLE;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
kill_throttle = ! autopilot_motors_on;
|
||||
}
|
||||
|
||||
#endif /* AUTOPILOT_ARMING_SWITCH_H */
|
||||
@@ -0,0 +1,122 @@
|
||||
/*
|
||||
* Copyright (C) 2012 The Paparazzi Team
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef AUTOPILOT_ARMING_THROTTLE_H
|
||||
#define AUTOPILOT_ARMING_THROTTLE_H
|
||||
|
||||
#include "autopilot_rc_helpers.h"
|
||||
|
||||
#define AUTOPILOT_ARMING_DELAY 10
|
||||
|
||||
enum arming_throttle_state {
|
||||
STATE_UNINIT,
|
||||
STATE_WAITING,
|
||||
STATE_MOTORS_OFF_READY,
|
||||
STATE_ARMING,
|
||||
STATE_MOTORS_ON,
|
||||
STATE_UNARMING
|
||||
};
|
||||
|
||||
enum arming_throttle_state autopilot_arming_state;
|
||||
uint8_t autopilot_arming_delay_counter;
|
||||
|
||||
static inline void autopilot_arming_init(void) {
|
||||
autopilot_arming_state = STATE_UNINIT;
|
||||
autopilot_arming_delay_counter = 0;
|
||||
}
|
||||
|
||||
static inline void autopilot_arming_set(bool_t motors_on) {
|
||||
if (motors_on) {
|
||||
autopilot_arming_state = STATE_MOTORS_ON;
|
||||
}
|
||||
else {
|
||||
if (autopilot_arming_state == STATE_MOTORS_ON)
|
||||
autopilot_arming_state = STATE_MOTORS_OFF_READY;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* State machine to check if motors should be turned ON or OFF.
|
||||
* - automatically unkill when applying throttle
|
||||
* - if throttle was not down at startup, you need to put throttle down again first
|
||||
* - other sticks need to be centered to start motors
|
||||
* - need to be in manual mode to start the motors
|
||||
* - AHRS needs to be aligned
|
||||
*/
|
||||
static inline void autopilot_arming_check_motors_on( void ) {
|
||||
switch(autopilot_arming_state) {
|
||||
case STATE_UNINIT:
|
||||
autopilot_motors_on = FALSE;
|
||||
autopilot_arming_delay_counter = 0;
|
||||
if (THROTTLE_STICK_DOWN())
|
||||
autopilot_arming_state = STATE_MOTORS_OFF_READY;
|
||||
else
|
||||
autopilot_arming_state = STATE_WAITING;
|
||||
break;
|
||||
case STATE_WAITING:
|
||||
autopilot_motors_on = FALSE;
|
||||
autopilot_arming_delay_counter = 0;
|
||||
if (THROTTLE_STICK_DOWN())
|
||||
autopilot_arming_state = STATE_MOTORS_OFF_READY;
|
||||
break;
|
||||
case STATE_MOTORS_OFF_READY:
|
||||
autopilot_motors_on = FALSE;
|
||||
autopilot_arming_delay_counter = 0;
|
||||
if (!THROTTLE_STICK_DOWN() &&
|
||||
rc_attitude_sticks_centered() &&
|
||||
autopilot_mode == MODE_MANUAL &&
|
||||
ahrs_is_aligned()) {
|
||||
autopilot_arming_state = STATE_ARMING;
|
||||
}
|
||||
break;
|
||||
case STATE_ARMING:
|
||||
autopilot_motors_on = FALSE;
|
||||
autopilot_arming_delay_counter++;
|
||||
if (THROTTLE_STICK_DOWN() ||
|
||||
!rc_attitude_sticks_centered() ||
|
||||
autopilot_mode != MODE_MANUAL ||
|
||||
!ahrs_is_aligned()) {
|
||||
autopilot_arming_state = STATE_MOTORS_OFF_READY;
|
||||
}
|
||||
else if (autopilot_arming_delay_counter >= AUTOPILOT_ARMING_DELAY)
|
||||
autopilot_arming_state = STATE_MOTORS_ON;
|
||||
break;
|
||||
case STATE_MOTORS_ON:
|
||||
autopilot_motors_on = TRUE;
|
||||
autopilot_arming_delay_counter = AUTOPILOT_ARMING_DELAY;
|
||||
if (THROTTLE_STICK_DOWN())
|
||||
autopilot_arming_state = STATE_UNARMING;
|
||||
break;
|
||||
case STATE_UNARMING:
|
||||
autopilot_motors_on = TRUE;
|
||||
autopilot_arming_delay_counter--;
|
||||
if (!THROTTLE_STICK_DOWN())
|
||||
autopilot_arming_state = STATE_MOTORS_ON;
|
||||
else if (autopilot_arming_delay_counter == 0)
|
||||
autopilot_arming_state = STATE_MOTORS_OFF_READY;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif /* AUTOPILOT_ARMING_THROTTLE_H */
|
||||
@@ -0,0 +1,112 @@
|
||||
/*
|
||||
* Copyright (C) 2012 The Paparazzi Team
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef AUTOPILOT_ARMING_YAW_H
|
||||
#define AUTOPILOT_ARMING_YAW_H
|
||||
|
||||
#include "autopilot_rc_helpers.h"
|
||||
|
||||
|
||||
#define AUTOPILOT_MOTOR_ON_TIME 40
|
||||
|
||||
// Motors ON check state machine
|
||||
enum arming_state {
|
||||
STATUS_MOTORS_OFF,
|
||||
STATUS_M_OFF_STICK_PUSHED,
|
||||
STATUS_START_MOTORS,
|
||||
STATUS_MOTORS_ON,
|
||||
STATUS_M_ON_STICK_PUSHED,
|
||||
STATUS_STOP_MOTORS
|
||||
};
|
||||
|
||||
uint32_t autopilot_motors_on_counter;
|
||||
enum arming_state autopilot_check_motor_status;
|
||||
|
||||
|
||||
static inline void autopilot_arming_init(void) {
|
||||
autopilot_motors_on_counter = 0;
|
||||
autopilot_check_motor_status = STATUS_MOTORS_OFF;
|
||||
}
|
||||
|
||||
|
||||
/** Update the status of the check_motors state machine.
|
||||
*/
|
||||
static inline void autopilot_arming_set(bool_t motors_on) {
|
||||
if (motors_on)
|
||||
autopilot_check_motor_status = STATUS_MOTORS_ON;
|
||||
else
|
||||
autopilot_check_motor_status = STATUS_MOTORS_OFF;
|
||||
}
|
||||
|
||||
/**
|
||||
* State machine to check if motors should be turned ON or OFF.
|
||||
* The motors start/stop when pushing the yaw stick without throttle during a given time
|
||||
* An intermediate state prevents oscillating between ON and OFF while keeping the stick pushed
|
||||
* The stick must return to a neutral position before starting/stoping again
|
||||
*/
|
||||
static inline void autopilot_arming_check_motors_on( void ) {
|
||||
switch(autopilot_check_motor_status) {
|
||||
case STATUS_MOTORS_OFF:
|
||||
autopilot_motors_on = FALSE;
|
||||
autopilot_motors_on_counter = 0;
|
||||
if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed
|
||||
autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED;
|
||||
break;
|
||||
case STATUS_M_OFF_STICK_PUSHED:
|
||||
autopilot_motors_on = FALSE;
|
||||
autopilot_motors_on_counter++;
|
||||
if (autopilot_motors_on_counter >= AUTOPILOT_MOTOR_ON_TIME)
|
||||
autopilot_check_motor_status = STATUS_START_MOTORS;
|
||||
else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon
|
||||
autopilot_check_motor_status = STATUS_MOTORS_OFF;
|
||||
break;
|
||||
case STATUS_START_MOTORS:
|
||||
autopilot_motors_on = TRUE;
|
||||
autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME;
|
||||
if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released
|
||||
autopilot_check_motor_status = STATUS_MOTORS_ON;
|
||||
break;
|
||||
case STATUS_MOTORS_ON:
|
||||
autopilot_motors_on = TRUE;
|
||||
autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME;
|
||||
if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed
|
||||
autopilot_check_motor_status = STATUS_M_ON_STICK_PUSHED;
|
||||
break;
|
||||
case STATUS_M_ON_STICK_PUSHED:
|
||||
autopilot_motors_on = TRUE;
|
||||
autopilot_motors_on_counter--;
|
||||
if (autopilot_motors_on_counter == 0)
|
||||
autopilot_check_motor_status = STATUS_STOP_MOTORS;
|
||||
else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon
|
||||
autopilot_check_motor_status = STATUS_MOTORS_ON;
|
||||
break;
|
||||
case STATUS_STOP_MOTORS:
|
||||
autopilot_motors_on = FALSE;
|
||||
autopilot_motors_on_counter = 0;
|
||||
if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released
|
||||
autopilot_check_motor_status = STATUS_MOTORS_OFF;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
};
|
||||
}
|
||||
|
||||
#endif /* AUTOPILOT_ARMING_YAW_H */
|
||||
@@ -0,0 +1,66 @@
|
||||
/*
|
||||
* Copyright (C) 2012 The Paparazzi Team
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef AUTOPILOT_RC_HELPERS_H
|
||||
#define AUTOPILOT_RC_HELPERS_H
|
||||
|
||||
#include "subsystems/radio_control.h"
|
||||
|
||||
#define AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20)
|
||||
#define AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 19 / 20)
|
||||
#ifndef AUTOPILOT_STICK_CENTER_TRESHOLD
|
||||
#define AUTOPILOT_STICK_CENTER_TRESHOLD (MAX_PPRZ * 1 / 20)
|
||||
#endif
|
||||
|
||||
#define THROTTLE_STICK_DOWN() \
|
||||
(radio_control.values[RADIO_THROTTLE] < AUTOPILOT_THROTTLE_TRESHOLD)
|
||||
#define YAW_STICK_PUSHED() \
|
||||
(radio_control.values[RADIO_YAW] > AUTOPILOT_YAW_TRESHOLD || \
|
||||
radio_control.values[RADIO_YAW] < -AUTOPILOT_YAW_TRESHOLD)
|
||||
#define YAW_STICK_CENTERED() \
|
||||
(radio_control.values[RADIO_YAW] < AUTOPILOT_STICK_CENTER_TRESHOLD && \
|
||||
radio_control.values[RADIO_YAW] > -AUTOPILOT_STICK_CENTER_TRESHOLD)
|
||||
#define PITCH_STICK_CENTERED() \
|
||||
(radio_control.values[RADIO_PITCH] < AUTOPILOT_STICK_CENTER_TRESHOLD && \
|
||||
radio_control.values[RADIO_PITCH] > -AUTOPILOT_STICK_CENTER_TRESHOLD)
|
||||
#define ROLL_STICK_CENTERED() \
|
||||
(radio_control.values[RADIO_ROLL] < AUTOPILOT_STICK_CENTER_TRESHOLD && \
|
||||
radio_control.values[RADIO_ROLL] > -AUTOPILOT_STICK_CENTER_TRESHOLD)
|
||||
|
||||
static inline bool_t rc_attitude_sticks_centered(void) {
|
||||
return ROLL_STICK_CENTERED() && PITCH_STICK_CENTERED() && YAW_STICK_CENTERED();
|
||||
}
|
||||
|
||||
#ifdef RADIO_KILL_SWITCH
|
||||
static inline bool_t kill_switch_is_on(void) {
|
||||
if (radio_control.values[RADIO_KILL_SWITCH] < 0)
|
||||
return TRUE;
|
||||
else
|
||||
return FALSE;
|
||||
}
|
||||
#else
|
||||
static inline bool_t kill_switch_is_on(void) {
|
||||
return FALSE;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif /* AUTOPILOT_RC_HELPERS_H */
|
||||
@@ -48,6 +48,10 @@
|
||||
#warning "ALL control gains are now positive!!!"
|
||||
#endif
|
||||
|
||||
#if defined GUIDANCE_V_INV_M
|
||||
#warning "GUIDANCE_V_INV_M has been removed. If you don't want to use adaptive hover, please define GUIDANCE_V_NOMINAL_HOVER_THROTTLE"
|
||||
#endif
|
||||
|
||||
#define GUIDANCE_V_GAIN_SCALER 48
|
||||
|
||||
uint8_t guidance_v_mode;
|
||||
@@ -55,6 +59,8 @@ int32_t guidance_v_ff_cmd;
|
||||
int32_t guidance_v_fb_cmd;
|
||||
int32_t guidance_v_delta_t;
|
||||
|
||||
int16_t guidance_v_nominal_throttle;
|
||||
|
||||
|
||||
/** Direct throttle from radio control.
|
||||
* range 0:#MAX_PPRZ
|
||||
@@ -101,6 +107,10 @@ void guidance_v_init(void) {
|
||||
|
||||
guidance_v_z_sum_err = 0;
|
||||
|
||||
#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
|
||||
guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE * MAX_PPRZ;
|
||||
#endif
|
||||
|
||||
gv_adapt_init();
|
||||
}
|
||||
|
||||
@@ -122,10 +132,15 @@ void guidance_v_mode_changed(uint8_t new_mode) {
|
||||
return;
|
||||
|
||||
switch (new_mode) {
|
||||
case GUIDANCE_V_MODE_HOVER:
|
||||
guidance_v_z_sp = ins_ltp_pos.z; // set current altitude as setpoint
|
||||
guidance_v_z_sum_err = 0;
|
||||
GuidanceVSetRef(ins_ltp_pos.z, 0, 0);
|
||||
break;
|
||||
|
||||
case GUIDANCE_V_MODE_RC_CLIMB:
|
||||
case GUIDANCE_V_MODE_CLIMB:
|
||||
case GUIDANCE_V_MODE_HOVER:
|
||||
guidance_v_zd_sp = 0;
|
||||
case GUIDANCE_V_MODE_NAV:
|
||||
guidance_v_z_sum_err = 0;
|
||||
GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0);
|
||||
@@ -158,8 +173,7 @@ void guidance_v_run(bool_t in_flight) {
|
||||
switch (guidance_v_mode) {
|
||||
|
||||
case GUIDANCE_V_MODE_RC_DIRECT:
|
||||
guidance_v_z_sp = ins_ltp_pos.z; // not sure why we do that
|
||||
GuidanceVSetRef(ins_ltp_pos.z, 0, 0); // or that - mode enter should take care of it ?
|
||||
guidance_v_z_sp = ins_ltp_pos.z; // for display only
|
||||
stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
|
||||
break;
|
||||
|
||||
@@ -260,8 +274,8 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
|
||||
guidance_v_z_sum_err = 0;
|
||||
|
||||
/* our nominal command : (g + zdd)*m */
|
||||
#ifdef GUIDANCE_V_INV_M
|
||||
const int32_t inv_m = BFP_OF_REAL(GUIDANCE_V_INV_M, GV_ADAPT_X_FRAC);
|
||||
#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
|
||||
const int32_t inv_m = BFP_OF_REAL(9.81/guidance_v_nominal_throttle, FF_CMD_FRAC);
|
||||
#else
|
||||
const int32_t inv_m = gv_adapt_X>>(GV_ADAPT_X_FRAC - FF_CMD_FRAC);
|
||||
#endif
|
||||
@@ -280,11 +294,11 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
|
||||
|
||||
/* our error feed back command */
|
||||
/* z-axis pointing down -> positive error means we need less thrust */
|
||||
guidance_v_fb_cmd = ((-guidance_v_kp * GUIDANCE_V_GAIN_SCALER * err_z) >> 12) +
|
||||
((-guidance_v_kd * GUIDANCE_V_GAIN_SCALER * err_zd) >> 21) +
|
||||
((-guidance_v_ki * GUIDANCE_V_GAIN_SCALER * guidance_v_z_sum_err) >> 21);
|
||||
guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 12) +
|
||||
((-guidance_v_kd * err_zd) >> 21) +
|
||||
((-guidance_v_ki * guidance_v_z_sum_err) >> 21);
|
||||
|
||||
guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd;
|
||||
guidance_v_delta_t = guidance_v_ff_cmd + (guidance_v_fb_cmd * GUIDANCE_V_GAIN_SCALER);
|
||||
|
||||
/* bound the result */
|
||||
Bound(guidance_v_delta_t, 0, MAX_PPRZ);
|
||||
|
||||
@@ -94,6 +94,11 @@ extern int32_t guidance_v_fb_cmd; ///< feed-back command
|
||||
*/
|
||||
extern int32_t guidance_v_delta_t;
|
||||
|
||||
/** nominal throttle for hover.
|
||||
* range: 0 : #MAX_PPRZ
|
||||
*/
|
||||
extern int16_t guidance_v_nominal_throttle;
|
||||
|
||||
extern int32_t guidance_v_kp; ///< vertical control P-gain
|
||||
extern int32_t guidance_v_kd; ///< vertical control D-gain
|
||||
extern int32_t guidance_v_ki; ///< vertical control I-gain
|
||||
@@ -109,5 +114,10 @@ extern void guidance_v_run(bool_t in_flight);
|
||||
guidance_v_z_sum_err = 0; \
|
||||
}
|
||||
|
||||
#define guidance_v_SetNominalHoverThrottle(_throttle) { \
|
||||
guidance_v_nominal_throttle = _throttle; \
|
||||
Bound(guidance_v_nominal_throttle, 0.1*MAX_PPRZ, 0.9*MAX_PPRZ); \
|
||||
}
|
||||
|
||||
|
||||
#endif /* GUIDANCE_V */
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2012 Thomas Kolb
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "bat_checker.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "subsystems/electrical.h"
|
||||
#include "led.h"
|
||||
|
||||
#ifndef CRITIC_BAT_LEVEL
|
||||
#error You must define CRITIC_BAT_LEVEL to use this module!
|
||||
#endif
|
||||
|
||||
#ifndef LOW_BAT_LEVEL
|
||||
#error You must define LOW_BAT_LEVEL to use this module!
|
||||
#endif
|
||||
|
||||
#ifndef BAT_CHECKER_LED
|
||||
#error You must define BAT_CHECKER_LED in your airframe file.
|
||||
#endif
|
||||
|
||||
#ifndef BAT_CHECKER_DELAY
|
||||
#warning BAT_CHECKER_DELAY is undefined. Falling back to 5 seconds.
|
||||
#define BAT_CHECKER_DELAY 5
|
||||
#endif
|
||||
|
||||
// at this level, the buzzer will be activated periodically
|
||||
#define WARN_BAT_LEVEL1 (LOW_BAT_LEVEL*10)
|
||||
|
||||
// at this level, the buzzer will be activated permanently
|
||||
#define WARN_BAT_LEVEL2 (CRITIC_BAT_LEVEL*10)
|
||||
|
||||
#pragma message "Battery checker included!"
|
||||
|
||||
void init_bat_checker(void) {
|
||||
LED_INIT(BAT_CHECKER_LED);
|
||||
LED_OFF(BAT_CHECKER_LED);
|
||||
}
|
||||
|
||||
void bat_checker_periodic(void) {
|
||||
static uint8_t bat_low_counter = 0;
|
||||
if(electrical.vsupply < WARN_BAT_LEVEL1) {
|
||||
if(bat_low_counter)
|
||||
bat_low_counter--;
|
||||
} else {
|
||||
bat_low_counter = BAT_CHECKER_DELAY * bat_checker_periodic_FREQ;
|
||||
}
|
||||
|
||||
if(!bat_low_counter) {
|
||||
if(electrical.vsupply < WARN_BAT_LEVEL2) {
|
||||
LED_ON(BAT_CHECKER_LED);
|
||||
} else if(electrical.vsupply < WARN_BAT_LEVEL1) {
|
||||
LED_TOGGLE(BAT_CHECKER_LED);
|
||||
}
|
||||
} else {
|
||||
LED_OFF(BAT_CHECKER_LED);
|
||||
}
|
||||
}
|
||||
|
||||
+9
-10
@@ -1,6 +1,7 @@
|
||||
/*
|
||||
* Copyright (C) 2010 Eric Parsonage
|
||||
* Copyright (C) 2011 Piotr Esden-Tempski <piotr@esden.net>
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2012 Thomas Kolb
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -18,15 +19,13 @@
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef BAT_CHECKER_H
|
||||
#define BAT_CHECKER_H
|
||||
|
||||
#ifndef BATTERY_BUZZER_H
|
||||
#define BATTERY_BUZZER_H
|
||||
void init_bat_checker(void);
|
||||
void bat_checker_periodic(void);
|
||||
|
||||
#include "std.h"
|
||||
|
||||
extern void battery_buzzer_init(void);
|
||||
extern void battery_buzzer_periodic(void);
|
||||
|
||||
#endif /* BATTERY_BUZZER_H */
|
||||
#endif // BAT_CHECKER_H
|
||||
@@ -1,57 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2010 Eric Parsonage
|
||||
* Copyright (C) 2011 Piotr Esden-Tempski <piotr@esden.net>
|
||||
*
|
||||
* 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 battery_buzzer.c
|
||||
* Simple module to toggle a gpio with connected buzzer on Lisa/M,
|
||||
* when battery voltage drops below a certain level.
|
||||
*/
|
||||
|
||||
#include <stm32/gpio.h>
|
||||
#include <stm32/rcc.h>
|
||||
#include "battery_buzzer.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "subsystems/electrical.h"
|
||||
|
||||
/* initialises GPIO pins */
|
||||
void battery_buzzer_init(void) {
|
||||
/* initialise peripheral clock for port C */
|
||||
RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOC, ENABLE);
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_Init(GPIOC, &GPIO_InitStructure);
|
||||
/* set port C pin 10 to be low */
|
||||
GPIO_WriteBit(GPIOC, GPIO_Pin_10, Bit_SET);
|
||||
|
||||
battery_buzzer_periodic();
|
||||
}
|
||||
|
||||
/* sets GPIO pins */
|
||||
void battery_buzzer_periodic(void) {
|
||||
|
||||
if (electrical.vsupply < (LOW_BAT_LEVEL * 10)) {
|
||||
GPIO_WriteBit(GPIOC, GPIO_Pin_10 , Bit_RESET);
|
||||
} else {
|
||||
GPIO_WriteBit(GPIOC, GPIO_Pin_10 , Bit_SET);
|
||||
}
|
||||
}
|
||||
@@ -141,32 +141,33 @@ void ins_realign_v(float z) {
|
||||
|
||||
void ins_propagate() {
|
||||
/* untilt accels */
|
||||
struct Int32Vect3 accel_body;
|
||||
INT32_RMAT_TRANSP_VMULT(accel_body, imu.body_to_imu_rmat, imu.accel);
|
||||
struct Int32Vect3 accel_ltp;
|
||||
INT32_RMAT_TRANSP_VMULT(accel_ltp, ahrs.ltp_to_body_rmat, accel_body);
|
||||
float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
|
||||
struct Int32Vect3 accel_meas_body;
|
||||
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
|
||||
struct Int32Vect3 accel_meas_ltp;
|
||||
INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, ahrs.ltp_to_body_rmat, accel_meas_body);
|
||||
|
||||
#if USE_VFF
|
||||
float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
|
||||
if (baro.status == BS_RUNNING && ins_baro_initialised) {
|
||||
vff_propagate(z_accel_float);
|
||||
vff_propagate(z_accel_meas_float);
|
||||
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
|
||||
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
|
||||
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
|
||||
}
|
||||
else { // feed accel from the sensors
|
||||
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
|
||||
// subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp)
|
||||
ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
|
||||
}
|
||||
#else
|
||||
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
|
||||
ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
|
||||
#endif /* USE_VFF */
|
||||
|
||||
#if USE_HFF
|
||||
/* propagate horizontal filter */
|
||||
b2_hff_propagate();
|
||||
#else
|
||||
ins_ltp_accel.x = accel_ltp.x;
|
||||
ins_ltp_accel.y = accel_ltp.y;
|
||||
ins_ltp_accel.x = accel_meas_ltp.x;
|
||||
ins_ltp_accel.y = accel_meas_ltp.y;
|
||||
#endif /* USE_HFF */
|
||||
|
||||
INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
|
||||
|
||||
Reference in New Issue
Block a user