Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

Merge branch 'dev' into 4.0_beta

Browse Source 
* utm for skytraq gps
* rotorcraft vertical guidance adaption improvements (should be usable with pwm_supervision now)
This commit is contained in:
Felix Ruess
2012-03-15 13:45:00 +01:00
parent 2044e9c081 c1f911023c
commit 5fc02bb591
54 changed files with 1283 additions and 1104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Doxyfile
+764 -763
View File
File diff suppressed because it is too large Load Diff
conf/airframes/CDW/LisaAspirin2.xml
+4 -4
View File
@@ -198,7 +198,7 @@
<firmware name="fixedwing">
<target name="ap" board="lisa_m_1.0">
<target name="ap" board="lisa_l_1.0">
<define name="STRONG_WIND"/>
<define name="WIND_INFO"/>
<define name="WIND_INFO_RET"/>
@@ -206,7 +206,7 @@
<configure name="PERIODIC_FREQUENCY" value="120"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
<configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
<configure name="AHRS_ALIGNER_LED" value="1"/>
<configure name="CPU_LED" value="1"/>
@@ -232,7 +232,7 @@
<subsystem name="radio_control" type="ppm"/>
<!-- Communication -->
<subsystem name="telemetry" type="transparent">
<subsystem name="telemetry" type="xbee_api">
<configure name="MODEM_BAUD" value="B57600"/>
</subsystem>
@@ -240,7 +240,7 @@
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="navigation"/>
<subsystem name="gps" type="ublox_utm"/>
<subsystem name="gps" type="ublox"/>
</firmware>
conf/airframes/booz2_flixr.xml
+5 -2
View File
@@ -37,7 +37,9 @@
<load name="booz_drop.xml"/>
<load name="sys_mon.xml"/>
<!--load name="booz_cam.xml"/-->
<!--load name="sonar_maxbotix_booz.xml"/-->
<load name="sonar_maxbotix_booz.xml">
<configure name="ADC_SONAR" value="ADC_0"/>
</load>
</modules>
<commands>
@@ -185,7 +187,8 @@
</section>
<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="15." integer="16" />
<define name="BARO_SENS" value="15." integer="16"/>
<define name="SONAR_SENS" value="2.146" integer="16"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
conf/autopilot/subsystems/fixedwing/gps_skytraq.makefile
+22
View File
@@ -0,0 +1,22 @@
# Hey Emacs, this is a -*- makefile -*-
ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
ap.CFLAGS += -DGPS_LINK=$(GPS_PORT)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.
conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile
+10 -6
View File
@@ -1,14 +1,18 @@
# Hey Emacs, this is a -*- makefile -*-
ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c
ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
ap.CFLAGS += -DUSE_GPS -DGPS_LINK=$(GPS_PORT)
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_LINK=$(GPS_PORT)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c
sim.CFLAGS += -DUSE_GPS
sim.srcs += $(SRC_SUBSYSTEMS)/gps.c
conf/boards/lisa_m_1.0.makefile
+7 -2
View File
@@ -1,7 +1,8 @@
# Hey Emacs, this is a -*- makefile -*-
#
# lisa_m_1.0.makefile
#
# http://paparazzi.enac.fr/wiki/User/LisaM
# http://paparazzi.enac.fr/wiki/Lisa/M
#
BOARD=lisa_m
@@ -51,9 +52,13 @@ ifndef SYS_TIME_LED
SYS_TIME_LED = 1
endif
ifndef RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT = UART3
endif
ifndef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT
RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT = UART5
endif
ifndef MODEM_PORT
MODEM_PORT=UART2
conf/boards/lisa_m_2.0.makefile
+5 -7
View File
@@ -1,14 +1,12 @@
# Hey Emacs, this is a -*- makefile -*-
include $(PAPARAZZI_SRC)/conf/boards/lisa_m_1.0.makefile
#
# Swap GPS UART with spektrum UART
#
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT = UART1
GPS_PORT=UART3
#
# Disable aligner led for now.
#
AHRS_ALIGNER_LED = none
ifndef GPS_PORT
GPS_PORT=UART3
endif
include $(PAPARAZZI_SRC)/conf/boards/lisa_m_1.0.makefile
conf/messages.xml
+1 -1
View File
@@ -1792,7 +1792,7 @@
<field name="ap_h_mode" type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV"/>
<field name="ap_v_mode" type="uint8" values="KILL|RC_DIRECT|RC_CLIMB|CLIMB|HOVER|NAV"/>
<field name="vsupply" type="uint8" unit="decivolt"/>
<field name="cpu_time" type="uint16" unit="s"></field>
<field name="cpu_time" type="uint16" unit="s"/>
</message>
<message name="STATE_FILTER_STATUS" id="232">
conf/modules/demo_module.xml
+1 -1
View File
@@ -9,7 +9,7 @@
<periodic fun="periodic_10Hz_demo()" period="0.1" start="start_demo()" stop="stop_demo()" autorun="FALSE"/>
<makefile>
<raw>
#Exemple of RAW makefile part
#Example of RAW makefile part
</raw>
<define name="DEMO_MODULE_LED" value="2"/>
<file name="demo_module.c"/>
conf/modules/sonar_maxbotix_booz.xml
+6
View File
@@ -19,6 +19,12 @@
<file name="sonar_maxbotix.c"/>
</makefile>
<makefile target="ap">
<raw>
# set ADC_SONAR to ADC_0 as default
ifeq ($(ADC_SONAR),)
ADC_SONAR = ADC_0
endif
</raw>
<define name="ADC_CHANNEL_SONAR" value="$(ADC_SONAR)"/>
<define name="USE_$(ADC_SONAR)"/>
<define name="USE_SONAR"/>
conf/settings/tuning_ctl_new.xml
+2
View File
@@ -42,6 +42,8 @@
<strip_button name="AS" value="1" group="speed_mode"/>
<strip_button name="GS" value="2" group="speed_mode"/>
</dl_setting>
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_min_cruise_throttle" shortname="min cruise thr" module="guidance/guidance_v" param="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE"/>
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_max_cruise_throttle" shortname="max cruise thr" module="guidance/guidance_v" param="V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE"/>
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_cruise_throttle" shortname="cruise throttle" module="guidance/guidance_v" handler="SetCruiseThrottle" param="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE"/>
<dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_pitch_pgain" shortname="pitch_p" param="V_CTL_AUTO_PITCH_PGAIN"/>
conf/settings/tuning_ins.xml
+1 -1
View File
@@ -55,7 +55,7 @@
</dl_settings>
<dl_settings name="alt">
<dl_setting MAX="0.2" MIN="2" STEP="0.01" VAR="v_ctl_altitude_pgain" shortname="alt_pgain" param="V_CTL_ALTITUDE_PGAIN"/>
<dl_setting MAX="0.2" MIN="0" STEP="0.01" VAR="v_ctl_altitude_pgain" shortname="alt_pgain" param="V_CTL_ALTITUDE_PGAIN" module="guidance/guidance_v"/>
<dl_setting MAX="10" MIN="0.1" STEP="0.1" VAR="v_ctl_altitude_max_climb" shortname="max climb" param="V_CTL_ALTITUDE_MAX_CLIMB"/>
<dl_setting MAX="2" MIN="0.0" STEP="0.05" VAR="v_ctl_altitude_pre_climb_correction" shortname="pre climb cor" param="V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION"/>
</dl_settings>
sw/airborne/arch/lpc21/peripherals/ms2100_arch.c
+2 -2
View File
@@ -17,11 +17,11 @@ void ms2100_arch_init( void ) {
ms2100_cur_axe = 0;
/* configure SS pin */
Ms2001Unselect(); /* pin idles high */
Ms2100Unselect(); /* pin idles high */
SetBit(MS2100_SS_IODIR, MS2100_SS_PIN); /* pin is output */
/* configure RESET pin */
Ms2001Reset(); /* pin idles low */
Ms2100Reset(); /* pin idles low */
SetBit(MS2100_RESET_IODIR, MS2100_RESET_PIN); /* pin is output */
/* configure DRDY pin */
sw/airborne/arch/lpc21/peripherals/ms2100_arch.h
+13 -13
View File
@@ -16,20 +16,20 @@
extern volatile uint8_t ms2100_cur_axe;
#define Ms2001Select() SetBit(MS2100_SS_IOCLR,MS2100_SS_PIN)
#define Ms2001Unselect() SetBit(MS2100_SS_IOSET,MS2100_SS_PIN)
#define Ms2100Select() SetBit(MS2100_SS_IOCLR,MS2100_SS_PIN)
#define Ms2100Unselect() SetBit(MS2100_SS_IOSET,MS2100_SS_PIN)
#define Ms2001Reset() SetBit(MS2100_RESET_IOCLR,MS2100_RESET_PIN)
#define Ms2001Set() SetBit(MS2100_RESET_IOSET,MS2100_RESET_PIN)
#define Ms2100Reset() SetBit(MS2100_RESET_IOCLR,MS2100_RESET_PIN)
#define Ms2100Set() SetBit(MS2100_RESET_IOSET,MS2100_RESET_PIN)
#define Ms2001OnSpiInt() { \
#define Ms2100OnSpiInt() { \
switch (ms2100_status) { \
case MS2100_SENDING_REQ: \
{ \
/* read dummy control byte reply */ \
uint8_t foo __attribute__ ((unused)) = SSPDR; \
ms2100_status = MS2100_WAITING_EOC; \
Ms2001Unselect(); \
Ms2100Unselect(); \
SSP_ClearRti(); \
SSP_DisableRti(); \
SSP_Disable(); \
@@ -42,7 +42,7 @@ extern volatile uint8_t ms2100_cur_axe;
new_val += SSPDR; \
if (abs(new_val) < 2000) \
ms2100_values[ms2100_cur_axe] = new_val; \
Ms2001Unselect(); \
Ms2100Unselect(); \
SSP_ClearRti(); \
SSP_DisableRti(); \
SSP_Disable(); \
@@ -59,22 +59,22 @@ extern volatile uint8_t ms2100_cur_axe;
}
#define Ms2001SendReq() { \
Ms2001Select(); \
#define Ms2100SendReq() { \
Ms2100Select(); \
ms2100_status = MS2100_SENDING_REQ; \
Ms2001Set(); \
Ms2100Set(); \
SSP_ClearRti(); \
SSP_EnableRti(); \
Ms2001Reset(); \
Ms2100Reset(); \
uint8_t control_byte = (ms2100_cur_axe+1) << 0 | \
MS2100_DIVISOR << 4; \
SSP_Send(control_byte); \
SSP_Enable(); \
}
#define Ms2001ReadRes() { \
#define Ms2100ReadRes() { \
ms2100_status = MS2100_READING_RES; \
Ms2001Select(); \
Ms2100Select(); \
/* trigger 2 bytes read */ \
SSP_Send(0); \
SSP_Send(0); \
sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
+4 -4
View File
@@ -115,10 +115,10 @@ static void SSP_ISR(void) {
if (ms2100_status == MS2100_IDLE || ms2100_status == MS2100_GOT_EOC) {
ImuSetSSP8bits();
if (ms2100_status == MS2100_IDLE) {
Ms2001SendReq();
Ms2100SendReq();
}
else { /* MS2100_GOT_EOC */
Ms2001ReadRes();
Ms2100ReadRes();
}
imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
}
@@ -127,9 +127,9 @@ static void SSP_ISR(void) {
}
break;
case IMU_SSP_STA_BUSY_MS2100:
Ms2001OnSpiInt();
Ms2100OnSpiInt();
if (ms2100_status == MS2100_IDLE) {
Ms2001SendReq();
Ms2100SendReq();
imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
}
else
sw/airborne/arch/stm32/peripherals/ms2100_arch.c
+4 -4
View File
@@ -41,8 +41,8 @@ void ms2100_arch_init( void ) {
ms2100_cur_axe = 0;
/* set mag SS and reset as output and assert them (SS on PC12 reset on PC13) ----*/
Ms2001Unselect();
Ms2001Set();
Ms2100Unselect();
Ms2100Set();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
@@ -83,13 +83,13 @@ void ms2100_arch_init( void ) {
#ifdef MS2100_HANDLES_SPI_IRQ
void spi2_irq_handler(void) {
Ms2001OnSpiIrq();
Ms2100OnSpiIrq();
}
#endif
#ifdef MS2100_HANDLES_DMA_IRQ
void dma1_c4_irq_handler(void) {
Ms2001OnDmaIrq();
Ms2100OnDmaIrq();
}
#endif /* MS2100_HANDLES_DMA_IRQ */
sw/airborne/arch/stm32/peripherals/ms2100_arch.h
+15 -15
View File
@@ -30,21 +30,21 @@
extern uint8_t ms2100_cur_axe;
extern int16_t ms2100_last_reading;
#define Ms2001Select() GPIOC->BRR = GPIO_Pin_12
#define Ms2001Unselect() GPIOC->BSRR = GPIO_Pin_12
#define Ms2100Select() GPIOC->BRR = GPIO_Pin_12
#define Ms2100Unselect() GPIOC->BSRR = GPIO_Pin_12
#define Ms2001Reset() GPIOC->BSRR = GPIO_Pin_13;
#define Ms2001Set() GPIOC->BRR = GPIO_Pin_13
#define Ms2100Reset() GPIOC->BSRR = GPIO_Pin_13;
#define Ms2100Set() GPIOC->BRR = GPIO_Pin_13
#define Ms2001HasEOC() GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5)
#define Ms2100HasEOC() GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5)
#define Ms2001SendReq() { \
Ms2001Select(); \
#define Ms2100SendReq() { \
Ms2100Select(); \
__IO uint32_t nCount = 4;for(; nCount != 0; nCount--); \
Ms2001Reset(); \
Ms2100Reset(); \
ms2100_status = MS2100_SENDING_REQ; \
nCount = 4;for(; nCount != 0; nCount--); \
Ms2001Set(); \
Ms2100Set(); \
uint16_t ctl_byte = ((ms2100_cur_axe+1) | (MS2100_DIVISOR << 4)); \
nCount = 20;for(; nCount != 0; nCount--); \
SPI_Cmd(SPI2, DISABLE); \
@@ -65,9 +65,9 @@ extern int16_t ms2100_last_reading;
SPI_I2S_SendData(SPI2, ctl_byte); \
}
#define Ms2001ReadRes() { \
#define Ms2100ReadRes() { \
ms2100_status = MS2100_READING_RES; \
Ms2001Select(); \
Ms2100Select(); \
SPI_Cmd(SPI2, DISABLE); \
SPI_InitTypeDef SPI_InitStructure = { \
.SPI_Direction = SPI_Direction_2Lines_FullDuplex, \
@@ -122,13 +122,13 @@ extern int16_t ms2100_last_reading;
\
}
#define Ms2001OnDmaIrq() { \
#define Ms2100OnDmaIrq() { \
/* ASSERT((ms2100_status == MS2100_READING_RES), \
* DEBUG_MS2100, MS2100_ERR_SPURIOUS_DMA_IRQ); \
*/ \
if (abs(ms2100_last_reading) < 1000) \
ms2100_values[ms2100_cur_axe] = ms2100_last_reading; \
Ms2001Unselect(); \
Ms2100Unselect(); \
ms2100_cur_axe++; \
if (ms2100_cur_axe > 2) { \
ms2100_cur_axe = 0; \
@@ -140,13 +140,13 @@ extern int16_t ms2100_last_reading;
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE); \
}
#define Ms2001OnSpiIrq() { \
#define Ms2100OnSpiIrq() { \
/* ASSERT((ms2100_status == MS2100_SENDING_REQ), \
* DEBUG_MS2100, MS2100_ERR_SPURIOUS_SPI_IRQ); \
*/ \
/* read unused control byte reply */ \
uint8_t foo __attribute__ ((unused)) = SPI_I2S_ReceiveData(SPI2); \
Ms2001Unselect(); \
Ms2100Unselect(); \
ms2100_status = MS2100_WAITING_EOC; \
SPI_Cmd(SPI2, DISABLE); \
SPI_I2S_ITConfig(SPI2, SPI_I2S_IT_RXNE, DISABLE); \
sw/airborne/arch/stm32/subsystems/imu/imu_b2_arch.c
+5 -5
View File
@@ -93,11 +93,11 @@ void dma1_c4_irq_handler(void) {
SPI_Cmd(SPI2, DISABLE);
#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
if (ms2100_status == MS2100_IDLE) {
Ms2001SendReq();
Ms2100SendReq();
imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
}
else if (ms2100_status == MS2100_WAITING_EOC && Ms2001HasEOC()) {
Ms2001ReadRes();
else if (ms2100_status == MS2100_WAITING_EOC && Ms2100HasEOC()) {
Ms2100ReadRes();
imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
}
else
@@ -106,7 +106,7 @@ void dma1_c4_irq_handler(void) {
break;
case IMU_SSP_STA_BUSY_MS2100:
#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
Ms2001OnDmaIrq();
Ms2100OnDmaIrq();
#endif
break;
default:
@@ -118,6 +118,6 @@ void dma1_c4_irq_handler(void) {
void spi2_irq_handler(void) {
#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
Ms2001OnSpiIrq();
Ms2100OnSpiIrq();
#endif
}
sw/airborne/firmwares/beth/rcv_telemetry.c
Executable → Regular
View File
sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+4
View File
@@ -56,6 +56,8 @@ uint8_t v_ctl_auto_throttle_submode;
/* "auto throttle" inner loop parameters */
float v_ctl_auto_throttle_cruise_throttle;
float v_ctl_auto_throttle_nominal_cruise_throttle;
float v_ctl_auto_throttle_min_cruise_throttle;
float v_ctl_auto_throttle_max_cruise_throttle;
float v_ctl_auto_throttle_climb_throttle_increment;
float v_ctl_auto_throttle_pgain;
float v_ctl_auto_throttle_igain;
@@ -133,6 +135,8 @@ void v_ctl_init( void ) {
/* "auto throttle" inner loop parameters */
v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
v_ctl_auto_throttle_cruise_throttle = v_ctl_auto_throttle_nominal_cruise_throttle;
v_ctl_auto_throttle_min_cruise_throttle = V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE;
v_ctl_auto_throttle_max_cruise_throttle = V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE;
v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
v_ctl_auto_throttle_pgain = V_CTL_AUTO_THROTTLE_PGAIN;
v_ctl_auto_throttle_igain = V_CTL_AUTO_THROTTLE_IGAIN;
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+25 -59
View File
@@ -37,6 +37,7 @@
#include "generated/airframe.h"
/* warn if some gains are still negative */
#if (GUIDANCE_V_HOVER_KP < 0) || \
(GUIDANCE_V_HOVER_KD < 0) || \
@@ -44,42 +45,28 @@
#warning "ALL control gains are now positive!!!"
#endif
/* In case Asctec controllers are used without supervision */
#ifndef SUPERVISION_MIN_MOTOR
#define SUPERVISION_MIN_MOTOR 1
#endif
#ifndef SUPERVISION_MAX_MOTOR
#define SUPERVISION_MAX_MOTOR 200
#endif
uint8_t guidance_v_mode;
int32_t guidance_v_ff_cmd;
int32_t guidance_v_fb_cmd;
/* command output */
int32_t guidance_v_delta_t;
/* direct throttle from radio control */
/* range 0:200 */
int32_t guidance_v_rc_delta_t;
/* vertical speed setpoint from radio control */
/* Q12.19 : accuracy 0.0000019, range +/-4096 */
int32_t guidance_v_rc_zd_sp;
/* altitude setpoint in meter (input) */
/* Q23.8 : accuracy 0.0039, range 8388km */
int32_t guidance_v_z_sp;
/* vertical speed setpoint in meter/s (input) */
/* Q12.19 : accuracy 0.0000019, range +/-4096 */
int32_t guidance_v_zd_sp;
#define GUIDANCE_V_ZD_SP_FRAC INT32_SPEED_FRAC
/* altitude reference in meter */
/* Q23.8 : accuracy 0.0039, range 8388km */
/** Direct throttle from radio control.
* range 0:200
*/
int32_t guidance_v_rc_delta_t;
/** Vertical speed setpoint from radio control.
* fixed point representation: Q12.19
* accuracy 0.0000019, range +/-4096
*/
int32_t guidance_v_rc_zd_sp;
int32_t guidance_v_z_sp;
int32_t guidance_v_zd_sp;
int32_t guidance_v_z_ref;
/* vertical speed reference in meter/s */
/* Q12.19 : accuracy 0.0000038, range 4096 */
int32_t guidance_v_zd_ref;
/* vertical acceleration reference in meter/s^2 */
/* Q21.10 : accuracy 0.0009766, range 2097152 */
int32_t guidance_v_zdd_ref;
int32_t guidance_v_kp;
@@ -117,14 +104,9 @@ void guidance_v_init(void) {
void guidance_v_read_rc(void) {
// used in RC_DIRECT directly and as saturation in CLIMB and HOVER
#ifndef USE_HELI
guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * 200 / MAX_PPRZ;
#else
guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * 4 / 5;
#endif
guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * (SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / MAX_PPRZ;
// used in RC_CLIMB
guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]) *
GUIDANCE_V_RC_CLIMB_COEF;
guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]) * GUIDANCE_V_RC_CLIMB_COEF;
DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_RC_CLIMB_DEAD_BAND);
}
@@ -134,10 +116,6 @@ void guidance_v_mode_changed(uint8_t new_mode) {
if (new_mode == guidance_v_mode)
return;
// switch ( guidance_v_mode ) {
//
// }
switch (new_mode) {
case GUIDANCE_V_MODE_RC_CLIMB:
@@ -147,10 +125,11 @@ void guidance_v_mode_changed(uint8_t new_mode) {
guidance_v_z_sum_err = 0;
GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0);
break;
default:
break;
}
}
guidance_v_mode = new_mode;
@@ -167,14 +146,7 @@ void guidance_v_run(bool_t in_flight) {
// FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
// AKA SUPERVISION and co
if (in_flight) {
// we should use something after the supervision!!! fuck!!!
int32_t cmd_hack = Chop(stabilization_cmd[COMMAND_THRUST], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR);
gv_adapt_run(ins_ltp_accel.z, cmd_hack);
//gv_adapt_run(ins_ltp_accel.z, cmd_hack, guidance_v_zd_ref);
}
else {
// reset vertical filter until takeoff
//ins_vf_realign = TRUE;
gv_adapt_run(ins_ltp_accel.z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref);
}
switch (guidance_v_mode) {
@@ -288,29 +260,23 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
#endif
const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
(guidance_v_zdd_ref<<(FF_CMD_FRAC - INT32_ACCEL_FRAC));
#if 0
if (g_m_zdd > 0)
guidance_v_ff_cmd = ( g_m_zdd + (inv_m>>1)) / inv_m;
else
guidance_v_ff_cmd = ( g_m_zdd - (inv_m>>1)) / inv_m;
#else
guidance_v_ff_cmd = g_m_zdd / inv_m;
int32_t cphi,ctheta,cphitheta;
PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_body_euler.phi);
PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_body_euler.theta);
cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
if (cphitheta < MAX_BANK_COEF) cphitheta = MAX_BANK_COEF;
/* feed forward command */
guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta;
#endif
/* our error command */
/* our error feed back command */
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);
// z-axis pointing down -> positive error means we need less thrust
guidance_v_delta_t = - (guidance_v_ff_cmd + guidance_v_fb_cmd);
// guidance_v_delta_t = -guidance_v_fb_cmd;
// z-axis pointing down -> positive error means we need less thrust
guidance_v_delta_t = -(guidance_v_ff_cmd + guidance_v_fb_cmd);
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+47 -13
View File
@@ -1,6 +1,4 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -26,6 +24,17 @@
#include "std.h"
/** Supervision default bounds
* In case Asctec controllers are used without supervision
* Used in control and adaptation filter
* */
#ifndef SUPERVISION_MIN_MOTOR
#define SUPERVISION_MIN_MOTOR 1
#endif
#ifndef SUPERVISION_MAX_MOTOR
#define SUPERVISION_MAX_MOTOR 200
#endif
#include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
#include "firmwares/rotorcraft/guidance/guidance_v_adpt.h"
@@ -38,19 +47,44 @@
extern uint8_t guidance_v_mode;
/** altitude setpoint in meters (input).
* fixed point representation: Q23.8
* accuracy 0.0039, range 8388km
*/
extern int32_t guidance_v_z_sp;
extern int32_t guidance_v_zd_sp;
extern int32_t guidance_v_z_ref;
extern int32_t guidance_v_zd_ref;
extern int32_t guidance_v_zdd_ref;
extern int32_t guidance_v_z_sum_err;
extern int32_t guidance_v_ff_cmd;
extern int32_t guidance_v_fb_cmd;
extern int32_t guidance_v_delta_t;
extern int32_t guidance_v_kp;
extern int32_t guidance_v_kd;
extern int32_t guidance_v_ki;
/** vertical speed setpoint in meter/s (input).
* fixed point representation: Q12.19
* accuracy 0.0000019, range +/-4096
*/
extern int32_t guidance_v_zd_sp;
/** altitude reference in meters.
* fixed point representation: Q23.8
* accuracy 0.0039, range 8388km
*/
extern int32_t guidance_v_z_ref;
/** vertical speed reference in meter/s.
* fixed point representation: Q12.19
* accuracy 0.0000038, range 4096
*/
extern int32_t guidance_v_zd_ref;
/** vertical acceleration reference in meter/s^2.
* fixed point representation: Q21.10
* accuracy 0.0009766, range 2097152
*/
extern int32_t guidance_v_zdd_ref;
extern int32_t guidance_v_z_sum_err; ///< accumulator for I-gain
extern int32_t guidance_v_ff_cmd; ///< feed-forward command
extern int32_t guidance_v_fb_cmd; ///< feed-back command
extern int32_t guidance_v_delta_t; ///< command output (ff+fb)
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
extern void guidance_v_init(void);
extern void guidance_v_read_rc(void);
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
+103 -65
View File
@@ -1,6 +1,4 @@
/*
* $Id$
*
* Copyright (C) 2009-2010 The Paparazzi Team
*
* This file is part of paparazzi.
@@ -19,37 +17,67 @@
* 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 guidance_v_adapt.h
* Adaptation bloc of the vertical guidance.
*
*
* Adaptation bloc of the vertical guidance
* This is a dimension one kalman filter estimating
* the ratio of vertical acceleration over thrust command ( ~ invert of the mass )
* needed by the invert dynamic model to produce a nominal command
* This is a dimension one kalman filter estimating
* the ratio of vertical acceleration over thrust command ( ~ invert of the mass )
* needed by the invert dynamic model to produce a nominal command
*/
#ifndef GUIDANCE_V_ADPT
#define GUIDANCE_V_ADPT
/** Adapt noise factor.
* Smaller values will make the filter to adapter faster
* Bigger values (slower adaptation) make the filter more robust to external perturbations
* Factor should always be >0
*/
#ifndef GUIDANCE_V_ADAPT_NOISE_FACTOR
#define GUIDANCE_V_ADAPT_NOISE_FACTOR 1.0
#endif
/** Filter is not fed if accel values are more than +/- MAX_ACCEL
* MAX_ACCEL is a positive value in m/s^2
*/
#ifndef GUIDANCE_V_ADAPT_MAX_ACCEL
#define GUIDANCE_V_ADAPT_MAX_ACCEL 4.0
#endif
/** Filter is not fed if command values are out of a % of MIN/MAX_SUPERVISION
* MAX_CMD and MIN_CMD must be between 0 and 1 with MIN_CMD < MAX_CMD
*/
#ifndef GUIDANCE_V_ADAPT_MAX_CMD
#define GUIDANCE_V_ADAPT_MAX_CMD 0.9
#endif
#ifndef GUIDANCE_V_ADAPT_MIN_CMD
#define GUIDANCE_V_ADAPT_MIN_CMD 0.1
#endif
/** State of the estimator.
* fixed point representation with #GV_ADAPT_X_FRAC
* Q13.18
*/
extern int32_t gv_adapt_X;
#define GV_ADAPT_X_FRAC 18
/** Covariance.
* fixed point representation with #GV_ADAPT_P_FRAC
* Q13.18
*/
extern int32_t gv_adapt_P;
#define GV_ADAPT_P_FRAC 18
/** Measurement */
extern int32_t gv_adapt_Xmeas;
#ifdef GUIDANCE_V_C
/* Our State
Q13.18
*/
int32_t gv_adapt_X;
#define GV_ADAPT_X_FRAC 18
/* Our covariance
Q13.18
*/
int32_t gv_adapt_P;
#define GV_ADAPT_P_FRAC 18
/* Our measurement */
int32_t gv_adapt_Xmeas;
@@ -59,87 +87,97 @@ int32_t gv_adapt_Xmeas;
#define GV_ADAPT_P0_F 0.5
#define GV_ADAPT_P0 BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC)
/* System and Measuremement noises */
/* System noises */
#define GV_ADAPT_SYS_NOISE_F 0.00005
#define GV_ADAPT_SYS_NOISE BFP_OF_REAL(GV_ADAPT_SYS_NOISE_F, GV_ADAPT_P_FRAC)
#if !USE_ADAPT_HOVER
#define GV_ADAPT_MEAS_NOISE_F 2.0
#define GV_ADAPT_MEAS_NOISE BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_F, GV_ADAPT_P_FRAC)
#else /* USE_ADAPT_HOVER */
#define GV_ADAPT_MEAS_NOISE_HOVER_F 10.0
/* Measuremement noises */
#define GV_ADAPT_MEAS_NOISE_HOVER_F (8.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
#define GV_ADAPT_MEAS_NOISE_HOVER BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)
#define GV_ADAPT_MEAS_NOISE_F 50.0
#define GV_ADAPT_MEAS_NOISE BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_F, GV_ADAPT_P_FRAC)
#define GV_ADAPT_MEAS_NOISE_OF_ZD (20.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
#define GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(4.0)
#define GV_ADAPT_HOVER_ACCEL ACCEL_BFP_OF_REAL(1.0)
#define GV_ADAPT_MAX_CMD 180
#define GV_ADAPT_MIN_CMD 20
#define GV_ADAPT_HOVER_MAX_CMD 120
#define GV_ADAPT_HOVER_MIN_CMD 60
/* Max accel */
#define GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL)
/* Command bounds */
#define GV_ADAPT_MAX_CMD ((int32_t)Blend(SUPERVISION_MAX_MOTOR, SUPERVISION_MIN_MOTOR, GUIDANCE_V_ADAPT_MAX_CMD))
#define GV_ADAPT_MIN_CMD ((int32_t)Blend(SUPERVISION_MAX_MOTOR, SUPERVISION_MIN_MOTOR, GUIDANCE_V_ADAPT_MIN_CMD))
/* Output bounds.
* Don't let it climb over a value that would
* give less than zero throttle and don't let it down to zero.
* Worst cases:
* MIN_ACCEL / MAX_THROTTLE
* MAX_ACCEL / MIN_THROTTLE
* ex:
* 9.81*2^18/255 = 10084
* 9.81*2^18/1 = 2571632
*/
// TODO Check this properly
#define GV_ADAPT_MAX_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / SUPERVISION_MIN_MOTOR)
#define GV_ADAPT_MIN_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / SUPERVISION_MAX_MOTOR)
#endif /* USE_ADAPT_HOVER */
static inline void gv_adapt_init(void) {
gv_adapt_X = GV_ADAPT_X0;
gv_adapt_P = GV_ADAPT_P0;
}
/*
zdd_meas : INT32_ACCEL_FRAC
thrust_applied : controller input [2-200]
*/
#define K_FRAC 12
static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
/* Do you really think we want to divide by zero ? */
if (thrust_applied == 0) return;
/** Adaptation function.
* @param zdd_meas vert accel measurement in m/s^2 with #INT32_ACCEL_FRAC
* @param thrust_applied controller input [SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR]
* @param zd_ref vertical speed reference in m/s with #INT32_SPEED_FRAC
*/
static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) {
/* Do you really think we want to divide by zero ?
* Negative commands are also prohibited here
*/
if (thrust_applied <= 0) return;
/* We don't propagate state, it's constant ! */
/* We propagate our covariance */
gv_adapt_P = gv_adapt_P + GV_ADAPT_SYS_NOISE;
/* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 24 bits */
const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) - zdd_meas)<<(GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
#if USE_ADAPT_HOVER
/* Update only if accel and commands are in a valid range */
if (thrust_applied < GV_ADAPT_MIN_CMD || thrust_applied > GV_ADAPT_MAX_CMD
|| zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL)
|| zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL) {
return;
#endif
if ( g_m_zdd > 0)
}
/* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 24 bits */
const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) - zdd_meas)<<(GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
if ( g_m_zdd > 0) {
gv_adapt_Xmeas = (g_m_zdd + (thrust_applied>>1)) / thrust_applied;
else
} else {
gv_adapt_Xmeas = (g_m_zdd - (thrust_applied>>1)) / thrust_applied;
}
/* Compute a residual */
int32_t residual = gv_adapt_Xmeas - gv_adapt_X;
/* Covariance Error */
int32_t E = 0;
#if USE_ADAPT_HOVER
if ((thrust_applied > GV_ADAPT_HOVER_MIN_CMD && thrust_applied < GV_ADAPT_HOVER_MAX_CMD) ||
(zdd_meas > -GV_ADAPT_HOVER_ACCEL && zdd_meas < GV_ADAPT_HOVER_ACCEL))
E = gv_adapt_P + GV_ADAPT_MEAS_NOISE_HOVER;
else
#endif
E = gv_adapt_P + GV_ADAPT_MEAS_NOISE;
int32_t ref = zd_ref >> (INT32_SPEED_FRAC - GV_ADAPT_P_FRAC);
if (zd_ref < 0) ref = -ref;
int32_t E = gv_adapt_P + GV_ADAPT_MEAS_NOISE_HOVER + ref * GV_ADAPT_MEAS_NOISE_OF_ZD;
/* Kalman gain */
int32_t K = (gv_adapt_P<<K_FRAC) / E;
/* Update Covariance */
gv_adapt_P = gv_adapt_P - ((K * gv_adapt_P)>>K_FRAC);
/* Don't let covariance climb over initial value */
if (gv_adapt_P > GV_ADAPT_P0) gv_adapt_P = GV_ADAPT_P0;
/* Update State */
gv_adapt_X = gv_adapt_X + ((K * residual)>>K_FRAC);
/* Again don't let it climb over a value that would
give less than zero throttle and don't let it down to zero.
30254 = MAX_ACCEL*GV_ADAPT_X_FRAC/MAX_THROTTLE
aka
30254 = 3*9.81*2^8/255
2571632 = 9.81*2^18
*/
Bound(gv_adapt_X, 10000, 2571632);
* give less than zero throttle and don't let it down to zero.
*/
Bound(gv_adapt_X, GV_ADAPT_MIN_OUT, GV_ADAPT_MAX_OUT);
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
+19 -10
View File
@@ -1,6 +1,4 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -29,24 +27,35 @@
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_int.h"
/* update frequency */
/* update frequency */
#define GV_FREQ_FRAC 9
#define GV_FREQ (1<<GV_FREQ_FRAC)
/* reference model vaccel in meters/sec2 (output) */
/* Q23.8 : accuracy 0.0039 , range 8388km/s2 */
/* int32_4_8_t */
/** reference model vertical accel in meters/s^2 (output)
* fixed point representation with #GV_ZDD_REF_FRAC
* Q23.8 : accuracy 0.0039 , range 8388km/s^2
*/
extern int32_t gv_zdd_ref;
/** number of bits for the fractional part of #gv_zdd_ref */
#define GV_ZDD_REF_FRAC 8
/* reference model vspeed in meters/sec (output) */
/* Q14.17 : accuracy 0.0000076 , range 16384m/s2 */
/** reference model vertical speed in meters/sec (output)
* fixed point representation with #GV_ZD_REF_FRAC
* Q14.17 : accuracy 0.0000076 , range 16384m/s2
*/
extern int32_t gv_zd_ref;
/** number of bits for the fractional part of #gv_zd_ref */
#define GV_ZD_REF_FRAC (GV_ZDD_REF_FRAC + GV_FREQ_FRAC)
/* reference model altitude in meters (output) */
/* Q37.26 : */
/** reference model altitude in meters (output)
* fixed point representation with #GV_Z_REF_FRAC
* Q37.26 :
*/
extern int64_t gv_z_ref;
/** number of bits for the fractional part of #gv_z_ref */
#define GV_Z_REF_FRAC (GV_ZD_REF_FRAC + GV_FREQ_FRAC)
/* Saturations definition */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c
+3
View File
@@ -32,6 +32,9 @@
#define REF_DOT_FRAC 11
#define REF_FRAC 16
#ifndef SUPERVISION_SCALE
#define SUPERVISION_SCALE MAX_PPRZ
#endif
#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
sw/airborne/firmwares/rotorcraft/telemetry.h
+10 -12
View File
@@ -57,6 +57,7 @@
#define PERIODIC_SEND_ROTORCRAFT_STATUS(_trans, _dev) { \
uint32_t imu_nb_err = 0; \
uint8_t _twi_blmc_nb_err = 0; \
uint16_t time_sec = sys_time.nb_sec; \
DOWNLINK_SEND_ROTORCRAFT_STATUS(_trans, _dev, \
&imu_nb_err, \
&_twi_blmc_nb_err, \
@@ -69,7 +70,7 @@
&guidance_h_mode, \
&guidance_v_mode, \
&electrical.vsupply, \
&sys_time.nb_sec \
&time_sec \
); \
}
#else /* !USE_GPS */
@@ -77,6 +78,7 @@
uint32_t imu_nb_err = 0; \
uint8_t twi_blmc_nb_err = 0; \
uint8_t fix = GPS_FIX_NONE; \
uint16_t time_sec = sys_time.nb_sec; \
DOWNLINK_SEND_ROTORCRAFT_STATUS(_trans, _dev, \
&imu_nb_err, \
&twi_blmc_nb_err, \
@@ -89,7 +91,7 @@
&guidance_h_mode, \
&guidance_v_mode, \
&electrical.vsupply, \
&sys_time.nb_sec \
&time_sec \
); \
}
#endif /* USE_GPS */
@@ -512,11 +514,11 @@
&b2_hff_state.yP[1][1]); \
}
#ifdef GPS_LAG
#define PERIODIC_SEND_HFF_GPS(_trans, _dev) { \
DOWNLINK_SEND_HFF_GPS(_trans, _dev, \
&b2_hff_rb_last->lag_counter, \
&lag_counter_err, \
&save_counter); \
#define PERIODIC_SEND_HFF_GPS(_trans, _dev) { \
DOWNLINK_SEND_HFF_GPS(_trans, _dev, \
&(b2_hff_rb_last->lag_counter), \
&lag_counter_err, \
&save_counter); \
}
#else
#define PERIODIC_SEND_HFF_GPS(_trans, _dev) {}
@@ -747,12 +749,8 @@
); \
}
//TODO replace by BOOZ_EXTRA_ADC
#ifdef BOOZ2_SONAR
#define PERIODIC_SEND_BOOZ2_SONAR(_trans, _dev) DOWNLINK_SEND_BOOZ2_SONAR(_trans, _dev,&booz2_adc_1,&booz2_adc_2,&booz2_adc_3,&booz2_adc_4);
#else
// FIXME: still used?? or replace by EXTRA_ADC
#define PERIODIC_SEND_BOOZ2_SONAR(_trans, _dev) {}
#endif
#ifdef BOOZ2_TRACK_CAM
#include "cam_track.h"
sw/airborne/firmwares/vor/lo.h
Executable → Regular
View File
sw/airborne/firmwares/vor/vor_int_demod_decim.c
Executable → Regular
View File
sw/airborne/firmwares/vor/vor_int_demod_decim.h
Executable → Regular
View File
sw/airborne/firmwares/vor/vor_int_filters_decim.h
Executable → Regular
View File
sw/airborne/lisa/test/lisa_test_ms2100.c
+3 -3
View File
@@ -70,11 +70,11 @@ static inline void main_periodic_task( void ) {
switch(ms2100_status) {
case MS2100_IDLE:
Ms2001SendReq();
Ms2100SendReq();
break;
case MS2100_WAITING_EOC:
if (Ms2001HasEOC()) {
Ms2001ReadRes();
if (Ms2100HasEOC()) {
Ms2100ReadRes();
}
break;
}
sw/airborne/modules/nav/nav_catapult.c
+17 -2
View File
@@ -64,7 +64,7 @@ static uint16_t nav_catapult_launch = 0;
#endif
#ifndef NAV_CATAPULT_MOTOR_DELAY
#define NAV_CATAPULT_MOTOR_DELAY 20 // Main Control Loops
#define NAV_CATAPULT_MOTOR_DELAY 45 // Main Control Loops
#endif
#define NAV_CATAPULT_HEADING_DELAY (60 * 3)
@@ -124,7 +124,7 @@ bool_t nav_catapult_init(void)
bool_t nav_catapult(uint8_t _climb)
bool_t nav_catapult(uint8_t _to, uint8_t _climb)
{
float alt = WaypointAlt(_climb);
@@ -154,6 +154,10 @@ bool_t nav_catapult(uint8_t _climb)
NavVerticalThrottleMode(9600*(0));
// Store take-off waypoint
WaypointX(_to) = GetPosX();
WaypointY(_to) = GetPosY();
WaypointAlt(_to) = GetPosAlt();
nav_catapult_x = estimator_x;
nav_catapult_y = estimator_y;
@@ -192,3 +196,14 @@ bool_t nav_catapult(uint8_t _climb)
return TRUE;
} // end of gls()
bool_t nav_select_touch_down(uint8_t _td)
{
WaypointX(_td) = GetPosX();
WaypointY(_td) = GetPosY();
WaypointAlt(_td) = GetPosAlt();
return FALSE;
}
sw/airborne/modules/nav/nav_catapult.h
+3 -1
View File
@@ -39,7 +39,9 @@ void nav_catapult_highrate_module(void);
extern bool_t nav_catapult_init(void);
extern bool_t nav_catapult_arm(void);
extern bool_t nav_catapult(uint8_t _climb);
extern bool_t nav_catapult(uint8_t _to, uint8_t _climb);
extern bool_t nav_catapult_disarm(void);
extern bool_t nav_select_touch_down(uint8_t _td);
#endif
sw/airborne/modules/openlog/openlog.c
Executable → Regular
View File
sw/airborne/modules/openlog/openlog.h
Executable → Regular
View File
sw/airborne/modules/sensors/airspeed_amsys.c
Executable → Regular
View File
sw/airborne/modules/sensors/airspeed_amsys.h
Executable → Regular
View File
sw/airborne/modules/sensors/baro_amsys.c
Executable → Regular
View File
sw/airborne/modules/sensors/baro_amsys.h
Executable → Regular
View File
sw/airborne/subsystems/gps.h
-4
View File
@@ -37,10 +37,6 @@
#include GPS_TYPE_H
#endif
#define GPS_FIX_NONE 0x00
#define GPS_FIX_2D 0x02
#define GPS_FIX_3D 0x03
#define GpsFixValid() (gps.fix == GPS_FIX_3D)
sw/airborne/subsystems/gps/gps_mtk.c
+1 -10
View File
@@ -156,16 +156,7 @@ void gps_mtk_read_message(void) {
gps.speed_3d = gps.gspeed;
gps.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf)))*10;
gps.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf);
switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) {
case MTK_DIY_FIX_3D:
gps.fix = GPS_FIX_3D;
break;
case MTK_DIY_FIX_2D:
gps.fix = GPS_FIX_2D;
break;
default:
gps.fix = GPS_FIX_NONE;
}
gps.fix = MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf);
gps.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);;
// FIXME: with MTK DIY 1.4 you do not receive GPS week
gps.week = 0;
sw/airborne/subsystems/gps/gps_mtk.h
+4 -4
View File
@@ -28,6 +28,10 @@
/** Includes macros generated from mtk.xml */
#include "mtk_protocol.h"
#define GPS_FIX_NONE 0x01
#define GPS_FIX_2D 0x02
#define GPS_FIX_3D 0x03
#define GPS_MTK_MAX_PAYLOAD 255
struct GpsMtk {
@@ -105,10 +109,6 @@ extern bool_t gps_configuring;
extern void gps_mtk_read_message(void);
extern void gps_mtk_parse(uint8_t c);
#define MTK_DIY_FIX_3D 3
#define MTK_DIY_FIX_2D 2
#define MTK_DIY_FIX_NONE 1
/*
* dynamic GPS configuration
*/
sw/airborne/subsystems/gps/gps_nmea.c
+1 -1
View File
@@ -37,7 +37,7 @@
#include "led.h"
#ifdef GPS_USE_LATLONG
#if GPS_USE_LATLONG
#include "subsystems/nav.h"
#include "math/pprz_geodetic_float.h"
#endif
sw/airborne/subsystems/gps/gps_nmea.h
+6 -2
View File
@@ -20,8 +20,8 @@
*
*/
/** \file gps_nmea.h
* \brief NMEA protocol specific code
/** @file gps_nmea.h
* NMEA protocol specific code
*
*/
@@ -33,6 +33,10 @@
#define GPS_NB_CHANNELS 16
#define GPS_FIX_NONE 0x00
#define GPS_FIX_2D 0x02
#define GPS_FIX_3D 0x03
#ifdef DEBUG_NMEA
#define NMEA_PRINT(...) { UsbSPrintString( __VA_ARGS__);};
#else
sw/airborne/subsystems/gps/gps_sim.h
+4
View File
@@ -3,6 +3,10 @@
#include "std.h"
#define GPS_FIX_NONE 0x00
#define GPS_FIX_2D 0x02
#define GPS_FIX_3D 0x03
#define GPS_NB_CHANNELS 16
extern bool_t gps_available;
sw/airborne/subsystems/gps/gps_sim_nps.h
+4
View File
@@ -3,6 +3,10 @@
#include "nps_sensors.h"
#define GPS_FIX_NONE 0x00
#define GPS_FIX_2D 0x02
#define GPS_FIX_3D 0x03
#define GPS_NB_CHANNELS 16
extern bool_t gps_available;
sw/airborne/subsystems/gps/gps_skytraq.c
+22
View File
@@ -21,6 +21,11 @@
#include "subsystems/gps.h"
#if GPS_USE_LATLONG
#include "subsystems/nav.h"
#include "math/pprz_geodetic_float.h"
#endif
struct GpsSkytraq gps_skytraq;
/* parser status */
@@ -68,6 +73,23 @@ void gps_skytraq_read_message(void) {
gps.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf);
gps.fix = SKYTRAQ_NAVIGATION_DATA_FixMode(gps_skytraq.msg_buf);
gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf)/10;
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
gps.utm_pos.alt = utm_f.alt*1000;
gps.utm_pos.zone = nav_utm_zone0;
#else
//DEBUG_S2_TOGGLE();
#ifdef GPS_LED
sw/airborne/subsystems/gps/gps_skytraq.h
+7
View File
@@ -24,6 +24,13 @@
#include "mcu_periph/uart.h"
#define GPS_FIX_NONE 0x00
#define GPS_FIX_2D 0x01
#define GPS_FIX_3D 0x02
#define GPS_FIX_3D_DGPS 0x03
#define SKYTRAQ_SYNC1 0xA0
#define SKYTRAQ_SYNC2 0xA1
sw/airborne/subsystems/gps/gps_ubx.c
+2 -2
View File
@@ -24,7 +24,7 @@
#include "led.h"
#ifdef GPS_USE_LATLONG
#if GPS_USE_LATLONG
#include "subsystems/nav.h"
#include "math/pprz_geodetic_float.h"
#endif
@@ -124,7 +124,7 @@ void gps_ubx_read_message(void) {
gps.lla_pos.lon = RadOfDeg(UBX_NAV_POSLLH_LON(gps_ubx.msg_buf));
gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf);
gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf);
#ifdef GPS_USE_LATLONG
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
sw/airborne/subsystems/gps/gps_ubx.h
+4
View File
@@ -38,6 +38,10 @@
#define GPS_NB_CHANNELS 16
#define GPS_FIX_NONE 0x00
#define GPS_FIX_2D 0x02
#define GPS_FIX_3D 0x03
#define GPS_UBX_MAX_PAYLOAD 255
struct GpsUbx {
bool_t msg_available;

Some files were not shown because too many files have changed in this diff Show More