diff --git a/conf/airframes/CDW/ChimuTinyFw.xml b/conf/airframes/CDW/ChimuTinyFw.xml index 1e7249bf0a..239d3ecace 100644 --- a/conf/airframes/CDW/ChimuTinyFw.xml +++ b/conf/airframes/CDW/ChimuTinyFw.xml @@ -182,7 +182,7 @@ - + diff --git a/conf/airframes/CDW/ChimuTinyFwSpi.xml b/conf/airframes/CDW/ChimuTinyFwSpi.xml index 71b912ba1f..f144a138e5 100644 --- a/conf/airframes/CDW/ChimuTinyFwSpi.xml +++ b/conf/airframes/CDW/ChimuTinyFwSpi.xml @@ -157,7 +157,9 @@ - + + + @@ -168,7 +170,7 @@ - + @@ -177,7 +179,7 @@ - + diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index 62824995fe..e27ed4dde5 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -7,11 +7,11 @@ - - - - - + + + + + @@ -217,7 +217,7 @@ - + diff --git a/conf/autopilot/subsystems/fixedwing/ahrs_dcm.makefile b/conf/autopilot/subsystems/fixedwing/ahrs_dcm.makefile index 91b1eb5ecd..3d7c02b4c2 100644 --- a/conf/autopilot/subsystems/fixedwing/ahrs_dcm.makefile +++ b/conf/autopilot/subsystems/fixedwing/ahrs_dcm.makefile @@ -3,7 +3,7 @@ $(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" -ifeq ($(ARCH), lpc21) +ifeq ($(TARGET), ap) ap.CFLAGS += -DUSE_AHRS @@ -43,6 +43,7 @@ sim.CFLAGS += -DIR_PITCH_NEUTRAL_DEFAULT=0 sim.CFLAGS += -DUSE_INFRARED sim.srcs += subsystems/sensors/infrared.c +sim.srcs += subsystems/sensors/infrared_adc.c sim.srcs += $(SRC_ARCH)/sim_ir.c sim.srcs += $(SRC_ARCH)/sim_imu.c diff --git a/conf/modules/ins_chimu_spi.xml b/conf/modules/ins_chimu_spi.xml index 8efbbfec24..c4da2f470a 100644 --- a/conf/modules/ins_chimu_spi.xml +++ b/conf/modules/ins_chimu_spi.xml @@ -14,6 +14,7 @@ + diff --git a/conf/modules/ins_chimu_uart.xml b/conf/modules/ins_chimu_uart.xml index e7ded2d0df..94dc63594a 100644 --- a/conf/modules/ins_chimu_uart.xml +++ b/conf/modules/ins_chimu_uart.xml @@ -1,5 +1,11 @@ + +
diff --git a/conf/modules/photogrammetry_calculator.xml b/conf/modules/photogrammetry_calculator.xml new file mode 100644 index 0000000000..27ff06be2a --- /dev/null +++ b/conf/modules/photogrammetry_calculator.xml @@ -0,0 +1,16 @@ + + + +
+ +
+ + + + + + + + diff --git a/conf/settings/photogrammetry_calculator.xml b/conf/settings/photogrammetry_calculator.xml new file mode 100644 index 0000000000..f086a5f1cb --- /dev/null +++ b/conf/settings/photogrammetry_calculator.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c b/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c index 614704b944..5ec122bcb8 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c @@ -142,7 +142,7 @@ void SPI1_ISR(void) __attribute__((naked)); #define SSP_FRF 0x00 << 4 /* frame format : SPI */ #define SSP_CPOL 0x00 << 6 /* clock polarity : SCK idles low */ #define SSP_CPHA 0x01 << 7 /* clock phase : data captured on second clock transition */ -#define SSP_SCR 0x0F << 8 /* serial clock rate */ +#define SSP_SCR 0x00 << 8 /* serial clock rate */ /* SSPCR1 settings */ #define SSP_LBM 0x00 << 0 /* loopback mode : disabled */ diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c index 494d252322..3fc74d03bf 100644 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ b/sw/airborne/arch/stm32/mcu_arch.c @@ -43,7 +43,7 @@ void mcu_arch_init(void) { return; #endif #ifdef HSE_TYPE_EXT_CLK -#warning Using external clock +#warning Info: Using external clock /* Setup the microcontroller system. * Initialize the Embedded Flash Interface, * initialize the PLL and update the SystemFrequency variable. diff --git a/sw/airborne/arch/stm32/servos_direct_hw.h b/sw/airborne/arch/stm32/servos_direct_hw.h index cb5d89c17c..96302e8ad3 100644 --- a/sw/airborne/arch/stm32/servos_direct_hw.h +++ b/sw/airborne/arch/stm32/servos_direct_hw.h @@ -11,7 +11,7 @@ extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; #define SERVOS_TICS_OF_USEC(_v) (_v) #define Actuator(_x) actuators_pwm_values[_x] -#define ChopServo(x,a,b) Chop(x, a, b) +//#define ChopServo(x,a,b) Chop(x, a, b) #define ActuatorsCommit actuators_pwm_commit diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c index ee5f21a598..6ded02cc6e 100644 --- a/sw/airborne/arch/stm32/subsystems/settings_arch.c +++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c @@ -104,7 +104,6 @@ static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) { } static int32_t flash_detect(struct FlashInfo* flash) { - uint32_t device_id; flash->total_size = FLASH_SIZE_ * 0x400; @@ -137,6 +136,7 @@ static int32_t flash_detect(struct FlashInfo* flash) { } #else /* this is the correct way of detecting page sizes */ + uint32_t device_id; /* read device id */ device_id = DBGMCU_IDCODE & DBGMCU_IDCODE_DEV_ID_MASK; diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c index fb46f92cb0..62a0466940 100644 --- a/sw/airborne/modules/cam_control/cam.c +++ b/sw/airborne/modules/cam_control/cam.c @@ -56,7 +56,7 @@ float test_cam_estimator_hspeed_dir; #endif #ifdef CAM_TILT_NEUTRAL -#if (CAM_TILT_MAX == CAM_TILT_NEUTRAL) +#if ((CAM_TILT_MAX) == (CAM_TILT_NEUTRAL)) #error CAM_TILT_MAX has to be different from CAM_TILT_NEUTRAL #endif #if (CAM_TILT_NEUTRAL == CAM_TILT_MIN) @@ -99,7 +99,11 @@ void cam_waypoint_target(void); void cam_ac_target(void); void cam_init( void ) { +#ifdef CAM_MODE0 + cam_mode = CAM_MODE0; +#else cam_mode = CAM_MODE_OFF; +#endif } void cam_periodic( void ) { @@ -255,7 +259,7 @@ void cam_nadir( void ) { cam_target_x = estimator_x; cam_target_y = estimator_y; #endif - cam_target_alt = 0; + cam_target_alt = -10; cam_target(); } diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.c b/sw/airborne/modules/cartography/photogrammetry_calculator.c new file mode 100644 index 0000000000..48fd523065 --- /dev/null +++ b/sw/airborne/modules/cartography/photogrammetry_calculator.c @@ -0,0 +1,101 @@ +/* + * $Id$ + * + * Copyright (C) 2009 Christophe De Wagter + * + * 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 "photogrammetry_calculator.h" + +#include "generated/airframe.h" +#include "generated/flight_plan.h" + +// Flightplan Variables +#ifndef PHOTOGRAMMETRY_SWEEP_ANGLE +#define PHOTOGRAMMETRY_SWEEP_ANGLE 0 +#endif + +#ifndef PHOTOGRAMMETRY_OVERLAP +#define PHOTOGRAMMETRY_OVERLAP 50 +#endif + +#ifndef PHOTOGRAMMETRY_SIDELAP +#define PHOTOGRAMMETRY_SIDELAP 50 +#endif + + +// Flightplan Paramters +int photogrammetry_sweep_angle = 0; + +int photogrammetry_sidestep = 0; +int photogrammetry_triggerstep = 0; +int photogrammetry_height = 0; + +// Photogrammetry Goals +int photogrammetry_sidelap; // Percent 0 - 100 +int photogrammetry_overlap; // Percent 0 - 100 +int photogrammetry_resolution; // Millimeter per pixel + +// Safety Aspects +int photogrammetry_height_min; +int photogrammetry_height_max; +int photogrammetry_radius_min; + + +void init_photogrammetry_calculator(void) +{ + photogrammetry_sweep_angle = PHOTOGRAMMETRY_SWEEP_ANGLE; + + photogrammetry_sidelap = PHOTOGRAMMETRY_SIDELAP; + photogrammetry_overlap = PHOTOGRAMMETRY_OVERLAP; + photogrammetry_resolution = PHOTOGRAMMETRY_RESOLUTION; + + photogrammetry_height_min = PHOTOGRAMMETRY_HEIGHT_MIN; + photogrammetry_height_max = PHOTOGRAMMETRY_HEIGHT_MAX; + photogrammetry_radius_min = PHOTOGRAMMETRY_RADIUS_MIN; + + photogrammetry_calculator_update(); +} + +void photogrammetry_calculator_update(void) +{ + + // Photogrammetry Goals + float photogrammetry_sidelap_f = ((float) photogrammetry_sidelap) / 100.0f; + float photogrammetry_overlap_f = ((float) photogrammetry_overlap) / 100.0f; + + // Linear Projection Camera Model + float viewing_ratio_height = ((float) PHOTOGRAMMETRY_SENSOR_HEIGHT) / ((float)PHOTOGRAMMETRY_FOCAL_LENGTH); + float viewing_ratio_width = ((float) PHOTOGRAMMETRY_SENSOR_WIDTH) / ((float)PHOTOGRAMMETRY_FOCAL_LENGTH); + float pixel_projection_width = viewing_ratio_width / ((float)PHOTOGRAMMETRY_PIXELS_WIDTH); + + // Flightplan Variables + photogrammetry_height = ((float) photogrammetry_resolution) / pixel_projection_width / 1000.0f; + + if (photogrammetry_height > photogrammetry_height_max) + photogrammetry_height = photogrammetry_height_max; + else if (photogrammetry_height < photogrammetry_height_min) + photogrammetry_height = photogrammetry_height_min; + + photogrammetry_sidestep = viewing_ratio_width * photogrammetry_height * (1.0f - photogrammetry_sidelap_f); + photogrammetry_triggerstep = viewing_ratio_height * photogrammetry_height * (1.0f - photogrammetry_overlap_f); +} + + diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.h b/sw/airborne/modules/cartography/photogrammetry_calculator.h new file mode 100644 index 0000000000..a906474646 --- /dev/null +++ b/sw/airborne/modules/cartography/photogrammetry_calculator.h @@ -0,0 +1,134 @@ +/* + * $Id$ + * + * Copyright (C) 2009 Christophe De Wagter + * + * 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 photogrammetry_calculator.h + +Add to airframe file: + +
+ + + + + + + + + + +
+ + + + + +Add to flightplan or airframe file: + + + +Add to flightplan + +
+#define PHOTOGRAMMETRY_SWEEP_ANGLE 53 // Degrees from the North +#define PHOTOGRAMMETRY_OVERLAP 50 // 1-99 Procent +#define PHOTOGRAMMETRY_SIDELAP 50 // 1-99 Procent +#define PHOTOGRAMMETRY_RESOLUTION 80 // mm pixel projection size +
+ + + + + + + + + + + + */ + +#ifndef PHOTOGRAMMETRY_CALCULATOR_H +#define PHOTOGRAMMETRY_CALCULATOR_H + +#include "std.h" +#include "paparazzi.h" + +#include "subsystems/navigation/OSAMNav.h" +#include "subsystems/navigation/poly_survey_adv.h" + + +// Flightplan Variables +extern int photogrammetry_sweep_angle; +extern int photogrammetry_sidestep; +extern int photogrammetry_triggerstep; +extern int photogrammetry_height; + +extern int photogrammetry_height_min; +extern int photogrammetry_height_max; +extern int photogrammetry_radius_min; + + +// Photogrammetry Goals +extern int photogrammetry_sidelap; +extern int photogrammetry_overlap; +extern int photogrammetry_resolution; + +void init_photogrammetry_calculator(void); +void photogrammetry_calculator_update(void); + +// Update Parameters on Settings Change +#define photogrammetry_calculator_UpdateSideLap(X) { \ + photogrammetry_sidelap = X; \ + photogrammetry_calculator_update(); \ +} + +#define photogrammetry_calculator_UpdateOverLap(X) { \ + photogrammetry_overlap = X; \ + photogrammetry_calculator_update(); \ +} + +#define photogrammetry_calculator_UpdateResolution(X) { \ + photogrammetry_resolution = X; \ + photogrammetry_calculator_update(); \ +} + +// Flightplan Routine Wrappers +#define PhotogrammetryCalculatorPolygonSurvey(_WP, _COUNT) { \ + WaypointAlt(WP__BASELEG) = photogrammetry_height + GROUND_ALT; \ + int _ang = 90 - photogrammetry_sweep_angle; \ + if (_ang > 90) _ang -= 180; if (_ang < -90) _ang += 180; \ + InitializePolygonSurvey((_WP), (_COUNT), 2*photogrammetry_sidestep, _ang); \ +} + +#define PhotogrammetryCalculatorPolygonSurveyADV(_WP, _COUNT) { \ + init_poly_survey_adv((_WP), (_COUNT), photogrammetry_sweep_angle, \ + photogrammetry_sidestep, photogrammetry_triggerstep, \ + photogrammetry_radius_min, photogrammetry_height + GROUND_ALT); \ +} + +#endif diff --git a/sw/airborne/modules/ins/imu_chimu.c b/sw/airborne/modules/ins/imu_chimu.c index 61694472e8..46787a26c9 100644 --- a/sw/airborne/modules/ins/imu_chimu.c +++ b/sw/airborne/modules/ins/imu_chimu.c @@ -43,26 +43,6 @@ #include "math.h" -/*************************************************************************** - * Endianness Swapping Functions - */ - -static float FloatSwap( float f ) -{ - union - { - float f; - unsigned char b[4]; - } dat1, dat2; - - dat1.f = f; - dat2.b[0] = dat1.b[3]; - dat2.b[1] = dat1.b[2]; - dat2.b[2] = dat1.b[1]; - dat2.b[3] = dat1.b[0]; - return dat2.f; -} - /*************************************************************************** * Cyclic Redundancy Checksum */ @@ -123,47 +103,37 @@ void CHIMU_Checksum(unsigned char *data, unsigned char buflen) #define CHIMU_STATE_MACHINE_PAYLOAD 5 #define CHIMU_STATE_MACHINE_XSUM 6 -// Message ID's that go TO the CHIMU -#define MSG00_PING 0x00 -#define MSG01_BIAS 0x01 -#define MSG02_DACMODE 0x02 -#define MSG03_CALACC 0x03 -#define MSG04_CALMAG 0x04 -#define MSG05_CALRATE 0x05 -#define MSG06_CONFIGCLR 0x06 -#define MSG07_CONFIGSET 0x07 -#define MSG08_SAVEGYROBIAS 0x08 -#define MSG09_ESTIMATOR 0x09 -#define MSG0A_SFCHECK 0x0A -#define MSG0B_CENTRIP 0x0B -#define MSG0C_INITGYROS 0x0C -#define MSG0D_DEVICEID 0x0D -#define MSG0E_REFVECTOR 0x0E -#define MSG0F_RESET 0x0F -#define MSG10_UARTSETTINGS 0x10 -#define MSG11_SERIALNUMBER 0x11 - -// Output message identifiers from the CHIMU unit -#define CHIMU_Msg_0_Ping 0 -#define CHIMU_Msg_1_IMU_Raw 1 -#define CHIMU_Msg_2_IMU_FP 2 -#define CHIMU_Msg_3_IMU_Attitude 3 -#define CHIMU_Msg_4_BiasSF 4 -#define CHIMU_Msg_5_BIT 5 -#define CHIMU_Msg_6_MagCal 6 -#define CHIMU_Msg_7_GyroBias 7 -#define CHIMU_Msg_8_TempCal 8 -#define CHIMU_Msg_9_DAC_Offsets 9 -#define CHIMU_Msg_10_Res 10 -#define CHIMU_Msg_11_Res 11 -#define CHIMU_Msg_12_Res 12 -#define CHIMU_Msg_13_Res 13 -#define CHIMU_Msg_14_RefVector 14 -#define CHIMU_Msg_15_SFCheck 15 - // Communication Definitions #define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above +/*************************************************************************** + * Endianness Swapping Functions + */ + +#ifdef CHIMU_BIG_ENDIAN + +static float FloatSwap( float f ) +{ + union + { + float f; + unsigned char b[4]; + } dat1, dat2; + + dat1.f = f; + dat2.b[0] = dat1.b[3]; + dat2.b[1] = dat1.b[2]; + dat2.b[2] = dat1.b[1]; + dat2.b[3] = dat1.b[0]; + return dat2.f; +} + +#else + +#define FloatSwap(X) (X) + +#endif + /*--------------------------------------------------------------------------- Name: CHIMU_Init @@ -379,6 +349,7 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa pstData->gCHIMU_SW_Minor = pPayloadData[CHIMU_index]; CHIMU_index++; pstData->gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index]<<8) & (0x0000FF00); CHIMU_index++; pstData->gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++; + return TRUE; break; case CHIMU_Msg_1_IMU_Raw: diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h index 6fade8a21e..6fea78a658 100644 --- a/sw/airborne/modules/ins/imu_chimu.h +++ b/sw/airborne/modules/ins/imu_chimu.h @@ -44,6 +44,48 @@ #ifndef CHIMU_DEFINED_H #define CHIMU_DEFINED_H +#define CHIMU_STX 0xae +#define CHIMU_BROADCAST 0xaa + +// Message ID's that go TO the CHIMU +#define MSG00_PING 0x00 +#define MSG01_BIAS 0x01 +#define MSG02_DACMODE 0x02 +#define MSG03_CALACC 0x03 +#define MSG04_CALMAG 0x04 +#define MSG05_CALRATE 0x05 +#define MSG06_CONFIGCLR 0x06 +#define MSG07_CONFIGSET 0x07 +#define MSG08_SAVEGYROBIAS 0x08 +#define MSG09_ESTIMATOR 0x09 +#define MSG0A_SFCHECK 0x0A +#define MSG0B_CENTRIP 0x0B +#define MSG0C_INITGYROS 0x0C +#define MSG0D_DEVICEID 0x0D +#define MSG0E_REFVECTOR 0x0E +#define MSG0F_RESET 0x0F +#define MSG10_UARTSETTINGS 0x10 +#define MSG11_SERIALNUMBER 0x11 + +// Output message identifiers from the CHIMU unit +#define CHIMU_Msg_0_Ping 0 +#define CHIMU_Msg_1_IMU_Raw 1 +#define CHIMU_Msg_2_IMU_FP 2 +#define CHIMU_Msg_3_IMU_Attitude 3 +#define CHIMU_Msg_4_BiasSF 4 +#define CHIMU_Msg_5_BIT 5 +#define CHIMU_Msg_6_MagCal 6 +#define CHIMU_Msg_7_GyroBias 7 +#define CHIMU_Msg_8_TempCal 8 +#define CHIMU_Msg_9_DAC_Offsets 9 +#define CHIMU_Msg_10_Res 10 +#define CHIMU_Msg_11_Res 11 +#define CHIMU_Msg_12_Res 12 +#define CHIMU_Msg_13_Res 13 +#define CHIMU_Msg_14_RefVector 14 +#define CHIMU_Msg_15_SFCheck 15 + + typedef struct { float phi; float theta; diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c index 1820a43ca2..6ea0deaca2 100644 --- a/sw/airborne/modules/ins/ins_chimu_spi.c +++ b/sw/airborne/modules/ins/ins_chimu_spi.c @@ -36,25 +36,23 @@ volatile uint8_t new_ins_attitude; void ins_init( void ) { - uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI -// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI -// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI - uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI - - CHIMU_Checksum(rate,12); + // uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 }; + uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI + uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI + // uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI + // uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI new_ins_attitude = 0; ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; + // Init CHIMU_Init(&CHIMU_DATA); // Quat Filter - for (int i=0;i<7;i++) - { - InsSend1(quaternions[i]); - } + CHIMU_Checksum(quaternions,7); + InsSend(quaternions,7); // Wait a bit (SPI send zero) InsSend1(0); @@ -64,11 +62,8 @@ void ins_init( void ) InsSend1(0); // 50Hz data: attitude only - for (int i=0;i<12;i++) - { - InsSend1(rate[i]); - } - + CHIMU_Checksum(rate,12); + InsSend(rate,12); } void parse_ins_msg( void ) @@ -79,10 +74,10 @@ void parse_ins_msg( void ) if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { - if(CHIMU_DATA.m_MsgID==0x03) + RunOnceEvery(25, LED_TOGGLE(3) ); + if(CHIMU_DATA.m_MsgID==CHIMU_Msg_3_IMU_Attitude) { new_ins_attitude = 1; - RunOnceEvery(25, LED_TOGGLE(3) ); if (CHIMU_DATA.m_attitude.euler.phi > M_PI) { CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; @@ -111,11 +106,7 @@ void ins_periodic_task( void ) // Fill X-speed CHIMU_Checksum(centripedal,19); - - for (int i=0;i<19;i++) - { - InsSend1(centripedal[i]); - } + InsSend(centripedal,19); // Downlink Send } diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c index 479b8663f5..5e2e6e6968 100644 --- a/sw/airborne/modules/ins/ins_chimu_uart.c +++ b/sw/airborne/modules/ins/ins_chimu_uart.c @@ -36,12 +36,12 @@ volatile uint8_t new_ins_attitude; void ins_init( void ) { - uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI -// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI -// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI - uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI + uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 }; + uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI + uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI +// uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI +// uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI - CHIMU_Checksum(rate,12); new_ins_attitude = 0; @@ -50,6 +50,13 @@ void ins_init( void ) CHIMU_Init(&CHIMU_DATA); + // Request Software version + for (int i=0;i<7;i++) + { + InsUartSend1(ping[i]); + } + + // Quat Filter for (int i=0;i<7;i++) { @@ -57,17 +64,9 @@ void ins_init( void ) } - - - - - - // 50Hz - for (int i=0;i<12;i++) - { - InsUartSend1(rate[i]); - } + CHIMU_Checksum(rate,12); + InsSend(rate,12); } diff --git a/sw/airborne/modules/ins/ins_module.h b/sw/airborne/modules/ins/ins_module.h index f83cf815a1..194dcb5f89 100644 --- a/sw/airborne/modules/ins/ins_module.h +++ b/sw/airborne/modules/ins/ins_module.h @@ -86,6 +86,7 @@ void parse_ins_buffer( uint8_t ); #define ReadInsBuffer() { while (InsLink(ChAvailable())&&!ins_msg_received) parse_ins_buffer(InsLink(Getch())); } #define InsSend1(c) InsLink(Transmit(c)) #define InsUartSend1(c) InsSend1(c) +#define InsSend(_dat,_len) { for (uint8_t i = 0; i< (_len); i++) InsSend1(_dat[i]); }; #define InsUartSetBaudrate(_b) InsLink(SetBaudrate(_b)) #define InsUartRunning InsLink(TxRunning) diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c index c092107616..be8bf19f3b 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.c +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -68,10 +68,10 @@ void imu_impl_init(void) #if PERIODIC_FREQUENCY == 60 /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */ ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0); -# warning ITG3200 read at 50Hz +# warning Info: ITG3200 read at 50Hz #else # if PERIODIC_FREQUENCY == 120 -# warning ITG3200 read at 100Hz +# warning Info: ITG3200 read at 100Hz /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */ ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0); # else diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index e1be916360..5bc9ea9aa9 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -312,7 +312,7 @@ void Normalize(void) float error=0; float temporary[3][3]; float renorm=0; - boolean problem=FALSE; + uint8_t problem=FALSE; // Find the non-orthogonality of X wrt Y error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index e1f85d4d41..46b8b9567b 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -77,6 +77,10 @@ extern bool_t gps_configuring; #define GpsParseOrConfigure() gps_ubx_read_message() #endif +/* Gps callback is called when receiving a VELNED or a SOL message + * All position/speed messages are sent in one shot and VELNED is the last one on fixedwing + * For rotorcraft, only SOL message is needed for pos/speed data + */ #define GpsEvent(_sol_available_callback) { \ if (GpsBuffer()) { \ ReadGpsBuffer(); \ @@ -84,7 +88,8 @@ extern bool_t gps_configuring; if (gps_ubx.msg_available) { \ GpsParseOrConfigure(); \ if (gps_ubx.msg_class == UBX_NAV_ID && \ - gps_ubx.msg_id == UBX_NAV_VELNED_ID) { \ + (gps_ubx.msg_id == UBX_NAV_VELNED_ID || \ + gps_ubx.msg_id == UBX_NAV_SOL_ID)) { \ if (gps.fix == GPS_FIX_3D) { \ gps.last_fix_time = cpu_time_sec; \ } \ diff --git a/sw/ground_segment/cockpit/horizon.ml b/sw/ground_segment/cockpit/horizon.ml index 4cf6d13505..7254bee323 100644 --- a/sw/ground_segment/cockpit/horizon.ml +++ b/sw/ground_segment/cockpit/horizon.ml @@ -2,8 +2,8 @@ * $Id$ * * Multi aircrafts map display and flight plan editor -* -* Copyright (C) 2004-2006 ENAC, Pascal Brisset, Antoine Drouin +* +* Copyright (C) 2004-2006 ENAC, Pascal Brisset, Antoine Drouin / 2011 Tobias Muench, Rolf Noellenburg * * This file is part of paparazzi. * @@ -20,7 +20,7 @@ * 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. +* Boston, MA 02111-1307, USA. * *) @@ -30,7 +30,8 @@ open Latlong let affine_pos_and_angle xw yw angle = let cos_a = cos angle in let sin_a = sin angle in - [| cos_a ; sin_a ; ~-. sin_a; cos_a; xw ; yw |] + [| cos_a ; sin_a ; ~-. sin_a; cos_a; xw; yw |] + let affine_pos xw yw = affine_pos_and_angle xw yw 0. @@ -72,13 +73,13 @@ let ruler = fun ?(index_on_right=false) ~text_props ~max ~scale ~w ~index_width let v = truncate v / step in for i = Pervasives.max 0 (v - 5) to min (v + 5) (Array.length tab - 1) do (* FIXME *) if not tab.(i) then begin - tab.(i) <- true; - draw i + tab.(i) <- true; + draw i end done in (** Yellow index *) - let _ = GnoCanvas.line ~points:[|0.;0.;w;0.|] ~fill_color:"yellow" root in + let _ = GnoCanvas.line ~points:[|0.;0.;w;0.|] ~props:[`WIDTH_PIXELS 2] ~fill_color:"yellow" root in let s = index_width in let idx = GnoCanvas.polygon ~points:[|0.;0.;-.s;s/.2.;-.s;-.s/.2.|] ~fill_color:"yellow" root in if index_on_right then @@ -88,11 +89,11 @@ let ruler = fun ?(index_on_right=false) ~text_props ~max ~scale ~w ~index_width let _ = GnoCanvas.rect ~x1:0. ~y1:(-.height) ~x2:w ~y2:(-.h) ~fill_color:"black" root in let _ = GnoCanvas.rect ~x1:0. ~y1:height ~x2:w ~y2:h ~fill_color:"black" root in r, lazy_drawer - - + + class h = fun ?packing size -> let canvas = GnoCanvas.canvas ~aa:true ~width:size ~height:size ?packing () in - let _ = + let _ = canvas#set_center_scroll_region false; in @@ -103,12 +104,12 @@ class h = fun ?packing size -> let pitch_scale = fun pitch -> pitch *. size2 *. 2. in let speed_scale = size2 /. 10. in let alt_scale = size2 /. 50. in - let speed_width = size2/.5. in - let alt_width = size2/.2.5 in - let index_width = size2 /. 15. in + let speed_width = size2/.3. in + let alt_width = size2/.2.25 in + let index_width = size2 /. 10. in let xc = left_margin +. speed_width +. size2 - and yc = size2*.1.1 in + and yc = size2*.1.25 in let text_props = [`FONT "Sans 8"; `ANCHOR `CENTER; `FILL_COLOR "white"] in @@ -116,7 +117,7 @@ class h = fun ?packing size -> let _top = GnoCanvas.rect ~x1:(-.size) ~y1:(-.size2*.5.) ~x2:size ~y2:0. ~fill_color:"#0099cb" disc and _bottom = GnoCanvas.rect ~x1:(-.size) ~y1:0. ~x2:size ~y2:(size2*.5.) ~fill_color:"#986701" disc and _line = GnoCanvas.line ~props:[`WIDTH_PIXELS 4] ~points:[|-.size;0.;size;0.|] ~fill_color:"white" disc - and _ = GnoCanvas.line ~points:[|0.;-.size2;0.;size2|] ~fill_color:"white" disc + and _ = GnoCanvas.line ~points:[|0.;-.size;0.;size|] ~fill_color:"white" disc in let grads = fun ?(text=false) n s a b -> for i = 0 to n do @@ -125,15 +126,16 @@ class h = fun ?packing size -> ignore (GnoCanvas.line ~points:[|-.s; y; s; y|] ~fill_color:"white" disc); ignore (GnoCanvas.line ~points:[|-.s; -.y; s; -.y|] ~fill_color:"white" disc); if text then - let text = Printf.sprintf "%d" (truncate deg) - and x = 2.*.s in - ignore (GnoCanvas.text ~props:text_props ~text ~y:(-.y) ~x disc); - ignore (GnoCanvas.text ~props:text_props ~text ~y:(-.y) ~x:(-.x) disc); - let text = "-"^text in - ignore (GnoCanvas.text ~props:text_props ~text ~y ~x disc); - ignore (GnoCanvas.text ~props:text_props ~text ~y ~x:(-.x) disc); + let text = Printf.sprintf "%d" (truncate deg) + and x = 2.*.s in + ignore (GnoCanvas.text ~props:text_props ~text ~y:(-.y) ~x disc); + ignore (GnoCanvas.text ~props:text_props ~text ~y:(-.y) ~x:(-.x) disc); + let text = "-"^text in + ignore (GnoCanvas.text ~props:text_props ~text ~y ~x disc); + ignore (GnoCanvas.text ~props:text_props ~text ~y ~x:(-.x) disc); done in - let _ = + + let _ = grads 10 (size2/.10.) 5. 2.5; grads 5 (size2/.7.) 10. 5.; grads ~text:true 5 (size2/.5.) 10. 10. in @@ -146,10 +148,10 @@ class h = fun ?packing size -> let (x, _y) = arc_above.(n-1) in let rest = [|(x, 0.);(10.*.size, 0.); (10.*.size, 10.*.size); (-.size, 10.*.size);(-.size,0.);(-.x,0.)|] in let points = floats_of_points (Array.append arc_above rest) in - let _ = + let _ = ignore (GnoCanvas.polygon ~fill_color:"black" ~points mask); for i = 0 to Array.length points / 2 - 1 do - points.(2*i+1) <- -. points.(2*i+1) + points.(2*i+1) <- -. points.(2*i+1) done; ignore (GnoCanvas.polygon ~fill_color:"black" ~points mask); let s = size2/. 5. in @@ -157,42 +159,49 @@ class h = fun ?packing size -> ignore (GnoCanvas.line ~props:[`WIDTH_PIXELS 4] ~points:[|x;0.;x+.s;0.;x+.s;s|] ~fill_color:"black" mask); (* Top and bottom graduations *) - let g = fun a -> - let l = GnoCanvas.line~props:[`WIDTH_PIXELS 2] ~fill_color:"white" ~points:[|0.;-.size2;0.;-.1.2*.size2|] mask in + let g = fun a -> + let l = GnoCanvas.line~props:[`WIDTH_PIXELS 1] ~fill_color:"white" ~points:[|0.;-.size2;0.;-.1.07*.size2|] mask in l#affine_relative (affine_pos_and_angle 0. 0. ((Deg>>Rad)a)) in - for i = 0 to 4 do + for i = 1 to 5 do let a = float (i*10) in g a; g (-.a) done; - let _30 = fun a -> - let t = GnoCanvas.text ~text:"30" ~props:text_props ~x:0. ~y:(-1.1*.size2) mask in + + let gg = fun a -> + let l = GnoCanvas.line~props:[`WIDTH_PIXELS 2] ~fill_color:"white" ~points:[|0.;-.size2;0.;-.1.15*.size2|] mask in + l#affine_relative (affine_pos_and_angle 0. 0. ((Deg>>Rad)a)) in + gg 30.; gg (-30.); + gg 0.; gg 0.; + + let _30 = fun a -> + let t = GnoCanvas.text ~text:"30" ~props:text_props ~x:0. ~y:(-1.28*.size2) mask in t#affine_relative (affine_pos_and_angle 0. 0. ((Deg>>Rad)a)) in - _30 30.; _30 (-30.) + _30 30.; _30 (-30.) in + (* Speedometer on the left side *) - let speed, mi, mx, lazy_speed = + let speed, mi, mx, lazy_speed = let g = GnoCanvas.group ~x:left_margin ~y:yc canvas#root in let r, lazy_ruler = ruler ~text_props ~index_on_right:true ~max:50 ~scale:speed_scale ~w:speed_width ~step:2 ~index_width ~h:(0.75*.size2) g in - let mx = - GnoCanvas.text ~x:(speed_width/.2.) ~y:(-0.85*.size2) ~props:text_props g + let mx = + GnoCanvas.text ~x:(speed_width/.2.) ~y:(-0.88*.size2) ~props:text_props g and mi = - GnoCanvas.text ~x:(speed_width/.2.) ~y:(0.80*.size2) ~props:text_props g in + GnoCanvas.text ~x:(speed_width/.2.) ~y:(0.875*.size2) ~props:text_props g in mx#set [`FILL_COLOR "yellow"]; mi#set [`FILL_COLOR "yellow"]; lazy_ruler 0.; r, mi, mx, lazy_ruler (* Altimeter on the right side *) - and alt, lazy_alt = + and alt, lazy_alt = let g = GnoCanvas.group ~x:(xc+.size2) ~y:yc canvas#root in ruler ~text_props ~max:3000 ~scale:alt_scale ~w:alt_width ~step:10 ~index_width ~h:(0.75*.size2) g in - + object method set_attitude = fun roll pitch -> - disc#affine_absolute (affine_pos_and_angle xc (yc+.pitch_scale pitch) (-.roll)) - + disc#affine_absolute (affine_pos_and_angle (xc+.((sin roll)*.(pitch_scale pitch))) (yc+.pitch_scale pitch*.(cos roll)) (-.roll)) val mutable max_speed = 0. val mutable min_speed = max_float method set_speed = fun (s:float) -> @@ -200,20 +209,20 @@ class h = fun ?packing size -> lazy_speed s; speed#affine_absolute (affine_pos 0. (speed_scale*.s)); min_speed <- min min_speed s; - max_speed <- max max_speed s; + max_speed <- max max_speed s; mi#set [`TEXT (sprintf "%.1f" min_speed)]; mx#set [`TEXT (sprintf "%.1f" max_speed)] initializer ignore (speed#connect#event (function - `BUTTON_PRESS _ev -> - max_speed <- 0.; min_speed <- max_float; true - | _ -> false)) - + `BUTTON_PRESS _ev -> + max_speed <- 0.; min_speed <- max_float; true + | _ -> false)) + method set_alt = fun (s:float) -> alt#affine_absolute (affine_pos 0. 0.); lazy_alt s; alt#affine_absolute (affine_pos 0. (alt_scale*.s)) - + end (*****************************************************************************) @@ -222,13 +231,11 @@ class h = fun ?packing size -> class pfd ?(visible = fun _ -> true) (widget: GBin.frame) = let horizon = new h ~packing: widget#add 150 in let _lazy = fun f x -> if visible widget then f x in - + object method set_attitude roll pitch = - _lazy (horizon#set_attitude ((Deg>>Rad)roll)) ((Deg>>Rad)pitch) + _lazy (horizon#set_attitude ((Deg>>Rad)roll)) ((Deg>>Rad)pitch) method set_alt (a:float) = _lazy horizon#set_alt a method set_climb (_c:float) = () method set_speed (c:float) = _lazy horizon#set_speed c end - - diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 830bd030c2..03ac26bc1b 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -2,7 +2,7 @@ * $Id$ * * Real time handling of flying A/Cs -* +* * Copyright (C) 2004-2006 ENAC, Pascal Brisset, Antoine Drouin * * This file is part of paparazzi. @@ -20,7 +20,7 @@ * 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. +* Boston, MA 02111-1307, USA. * *) @@ -111,12 +111,12 @@ let find_ac = fun ac_id -> Hashtbl.find aircrafts ac_id with Not_found -> raise AC_not_found - + let active_ac = ref "" let get_ac = fun vs -> let ac_id = Pprz.string_assoc "ac_id" vs in find_ac ac_id - + let select_ac = fun acs_notebook ac_id -> if !active_ac <> ac_id then @@ -127,7 +127,7 @@ let select_ac = fun acs_notebook ac_id -> if !active_ac <> "" then begin let ac' = find_ac !active_ac in ac'.strip#hide_buttons (); - ac'.notebook_label#set_width_chars (String.length ac'.notebook_label#text) + ac'.notebook_label#set_width_chars (String.length ac'.notebook_label#text) end; (* Set the new active *) @@ -139,7 +139,7 @@ let select_ac = fun acs_notebook ac_id -> ac.notebook_label#set_width_chars 20; module M = Map.Make (struct type t = string let compare = compare end) -let log = +let log = let last = ref M.empty in fun ?(say = false) (a:Pages.alert) ac_id s -> if not (M.mem ac_id !last) || M.find ac_id !last <> s then begin @@ -168,20 +168,20 @@ let resize_track = fun ac track -> let send_move_waypoint_msg = fun ac i w -> let wgs84 = w#pos in let vs = ["ac_id", Pprz.String ac; - "wp_id", Pprz.Int i; - "lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat); - "long", Pprz.Float ((Rad>>Deg)wgs84.posn_long); - "alt", Pprz.Float w#alt - ] in + "wp_id", Pprz.Int i; + "lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat); + "long", Pprz.Float ((Rad>>Deg)wgs84.posn_long); + "alt", Pprz.Float w#alt + ] in Ground_Pprz.message_send "gcs" "MOVE_WAYPOINT" vs let commit_changes = fun ac -> let a = find_ac ac in - List.iter + List.iter (fun w -> let (i, w) = a.fp_group#index w in - if w#moved then - send_move_waypoint_msg ac i w) + if w#moved then + send_move_waypoint_msg ac i w) a.fp_group#waypoints let center = fun geomap track () -> @@ -203,7 +203,7 @@ let blocks_of_stages = fun stages -> List.sort compare !blocks let jump_to_block = fun ac_id id -> - Ground_Pprz.message_send "gcs" "JUMP_TO_BLOCK" + Ground_Pprz.message_send "gcs" "JUMP_TO_BLOCK" ["ac_id", Pprz.String ac_id; "block_id", Pprz.Int id] let dl_setting = fun ac_id idx value -> @@ -231,17 +231,17 @@ let show_snapshot = fun (geomap:G.widget) geo_FL geo_BR point pixbuf name ev -> let image = GMisc.image ~pixbuf () in let icon = image#coerce in begin - match GToolbox.question_box ~title:name ~buttons:["Delete"; "Close"] ~icon "" with - 1 -> - point#destroy () - | _ -> () + match GToolbox.question_box ~title:name ~buttons:["Delete"; "Close"] ~icon "" with + 1 -> + point#destroy () + | _ -> () end; true | `LEAVE_NOTIFY _ev -> begin - match !icon with - None -> () - | Some i -> i#destroy () + match !icon with + None -> () + | Some i -> i#destroy () end; false | `ENTER_NOTIFY _ev -> @@ -252,47 +252,47 @@ let show_snapshot = fun (geomap:G.widget) geo_FL geo_BR point pixbuf name ev -> false | _ -> false - + let mark = fun (geomap:G.widget) ac_id track plugin_frame -> let i = ref 1 in fun () -> match track#last with Some geo -> - begin - let group = geomap#background in - let point = geomap#circle ~group ~fill_color:"blue" geo 5. in - point#raise_to_top (); - let lat = (Rad>>Deg)geo.posn_lat - and long = (Rad>>Deg)geo.posn_long in - Tele_Pprz.message_send ac_id "MARK" - ["ac_id", Pprz.String ac_id; - "lat", Pprz.Float lat; - "long", Pprz.Float long]; - let frame = - match plugin_frame with - None -> geomap#canvas#coerce - | Some pf -> pf#coerce in - let width, height = Gdk.Drawable.get_size frame#misc#window in - let dest = GdkPixbuf.create width height() in - GdkPixbuf.get_from_drawable ~dest ~width ~height frame#misc#window; - let name = sprintf "Snapshot-%s-%d_%f_%f_%f.png" ac_id !i lat long (track#last_heading) in - let png = sprintf "%s/var/logs/%s" Env.paparazzi_home name in - GdkPixbuf.save png "png" dest; - incr i; - - (* Computing the footprint: front_left and back_right *) - let cam_aperture = 2.4/.1.9 in (* width over distance FIXME *) - let alt = track#last_altitude -. float (Srtm.of_wgs84 geo) in - let width = cam_aperture *. alt in - let height = width *. 3. /. 4. in - let utm = utm_of WGS84 geo in - let a = (Deg>>Rad)track#last_heading in - let (xfl,yfl) = rotate a (-.width/.2., height/.2.) - and (xbr,ybr) = rotate a (width/.2., -.height/.2.) in - let geo_FL = of_utm WGS84 (utm_add utm (xfl,yfl)) - and geo_BR = of_utm WGS84 (utm_add utm (xbr,ybr)) in - ignore (point#connect#event (show_snapshot geomap geo_FL geo_BR point dest name)) - end + begin + let group = geomap#background in + let point = geomap#circle ~group ~fill_color:"blue" geo 5. in + point#raise_to_top (); + let lat = (Rad>>Deg)geo.posn_lat + and long = (Rad>>Deg)geo.posn_long in + Tele_Pprz.message_send ac_id "MARK" + ["ac_id", Pprz.String ac_id; + "lat", Pprz.Float lat; + "long", Pprz.Float long]; + let frame = + match plugin_frame with + None -> geomap#canvas#coerce + | Some pf -> pf#coerce in + let width, height = Gdk.Drawable.get_size frame#misc#window in + let dest = GdkPixbuf.create width height() in + GdkPixbuf.get_from_drawable ~dest ~width ~height frame#misc#window; + let name = sprintf "Snapshot-%s-%d_%f_%f_%f.png" ac_id !i lat long (track#last_heading) in + let png = sprintf "%s/var/logs/%s" Env.paparazzi_home name in + GdkPixbuf.save png "png" dest; + incr i; + + (* Computing the footprint: front_left and back_right *) + let cam_aperture = 2.4/.1.9 in (* width over distance FIXME *) + let alt = track#last_altitude -. float (Srtm.of_wgs84 geo) in + let width = cam_aperture *. alt in + let height = width *. 3. /. 4. in + let utm = utm_of WGS84 geo in + let a = (Deg>>Rad)track#last_heading in + let (xfl,yfl) = rotate a (-.width/.2., height/.2.) + and (xbr,ybr) = rotate a (width/.2., -.height/.2.) in + let geo_FL = of_utm WGS84 (utm_add utm (xfl,yfl)) + and geo_BR = of_utm WGS84 (utm_add utm (xbr,ybr)) in + ignore (point#connect#event (show_snapshot geomap geo_FL geo_BR point dest name)) + end | None -> () @@ -394,19 +394,19 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id let ac_menu_fact = new GMenu.factory ac_menu in let fp = ac_menu_fact#add_check_item "Fligh Plan" ~active:true in ignore (fp#connect#toggled (fun () -> show_mission ac_id fp#active)); - + let track = new MapTrack.track ~size: !track_size ~name ~color:color geomap in geomap#register_to_fit (track:>MapCanvas.geographic); let center_ac = center geomap track in ignore (ac_menu_fact#add_item "Center A/C" ~callback:center_ac); - + ignore (ac_menu_fact#add_item "Clear Track" ~callback:(fun () -> track#clear_map2D)); ignore (ac_menu_fact#add_item "Resize Track" ~callback:(fun () -> resize_track ac_id track)); let reset_wp_menu = ac_menu_fact#add_item "Reset Waypoints" in let jump_block_entries = List.map (menu_entry_of_block ac_id) blocks in - + let commit_moves = fun () -> commit_changes ac_id in let sm = ac_menu_fact#add_submenu "Datalink" in @@ -431,7 +431,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id ignore (acs_notebook#append_page ~tab_label:eb#coerce ac_frame#coerce); let ac_notebook = GPack.notebook ~packing: ac_frame#add () in let visible = fun w -> - ac_notebook#page_num w#coerce = ac_notebook#current_page in + ac_notebook#page_num w#coerce = ac_notebook#current_page in (** Add a strip *) let min_bat, max_bat = get_bat_levels af_xml in @@ -456,19 +456,19 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id ignore (reset_wp_menu#connect#activate (reset_waypoints fp)); (** Monitor waypoints changes *) - List.iter + List.iter (fun w -> let (i, w) = fp#index w in w#set_commit_callback (fun () -> send_move_waypoint_msg ac_id i w)) fp#waypoints; (** Add waypoints as geo references *) - List.iter + List.iter (fun w -> let (_i, w) = fp#index w in geomap#add_info_georef (sprintf "%s.%s" name w#name) (w :> < pos : Latlong.geographic >)) fp#waypoints; - + (** Add the short cut buttons in the strip *) let tooltips = GData.tooltips () in let keys = ref [] in (* Associations between keys and block ids *) @@ -476,37 +476,37 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id let id = ExtXml.int_attrib block "no" in begin (* Is it a key short cut ? *) try - let key, modifiers = GtkData.AccelGroup.parse (Xml.attrib block "key") in - keys := (key, (modifiers, id)) :: !keys + let key, modifiers = GtkData.AccelGroup.parse (Xml.attrib block "key") in + keys := (key, (modifiers, id)) :: !keys with - _ -> () + _ -> () end; try (* Is it a strip button ? *) let label = ExtXml.attrib block "strip_button" and block_name = ExtXml.attrib block "name" and group = ExtXml.attrib_or_default block "group" "" in let b = - try (* Is it an icon ? *) - let icon = Xml.attrib block "strip_icon" in - let b = GButton.button () in - let pixbuf = GdkPixbuf.from_file (Env.gcs_icons_path // icon) in - ignore (GMisc.image ~pixbuf ~packing:b#add ()); - - (* Drag for Drop *) - let papget = Papget_common.xml "goto_block" "button" - [ "block_name", block_name; - "icon", icon] in - Papget_common.dnd_source b#coerce papget; - - (* Associates the label as a tooltip *) - tooltips#set_tip b#coerce ~text:label; - b - with - Xml.No_attribute _ -> (* It's not an icon *) + try (* Is it an icon ? *) + let icon = Xml.attrib block "strip_icon" in + let b = GButton.button () in + let pixbuf = GdkPixbuf.from_file (Env.gcs_icons_path // icon) in + ignore (GMisc.image ~pixbuf ~packing:b#add ()); + + (* Drag for Drop *) + let papget = Papget_common.xml "goto_block" "button" + [ "block_name", block_name; + "icon", icon] in + Papget_common.dnd_source b#coerce papget; + + (* Associates the label as a tooltip *) + tooltips#set_tip b#coerce ~text:label; + b + with + Xml.No_attribute _ -> (* It's not an icon *) GButton.button ~label () - | exc -> - fprintf stderr "Error: '%s' Using a standard button" (Printexc.to_string exc); - GButton.button ~label () + | exc -> + fprintf stderr "Error: '%s' Using a standard button" (Printexc.to_string exc); + GButton.button ~label () in strip#add_widget b#coerce ~group; ignore (b#connect#clicked (fun _ -> jump_to_block ac_id id)) @@ -523,12 +523,12 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id (** Insert the flight plan tab *) let fp_label = GMisc.label ~text: "Flight Plan" () in ignore ((ac_notebook:GPack.notebook)#append_page ~tab_label:fp_label#coerce fp#window#coerce); - + let infrared_label = GMisc.label ~text: "Infrared" () in let infrared_frame = GBin.frame ~show:false ~shadow_type:`NONE () in ignore (ac_notebook#append_page ~tab_label: infrared_label#coerce infrared_frame#coerce); let ir_page = new Pages.infrared infrared_frame in - + let gps_label = GMisc.label ~text: "GPS" () in let gps_frame = GBin.frame ~shadow_type: `NONE () in ignore (ac_notebook#append_page ~tab_label: gps_label#coerce gps_frame#coerce); @@ -547,7 +547,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id let settings_url = Pprz.string_assoc "settings" config in let settings_file = Http.file_of_url settings_url in - let settings_xml = + let settings_xml = try ExtXml.parse_file ~noprovedtd:true settings_file with exc -> @@ -567,7 +567,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id (** Connect key shortcuts *) let key_press = fun ev -> - key_press_event settings_tab#keys (fun commit -> commit ()) ev in + key_press_event settings_tab#keys (fun commit -> commit ()) ev in ignore (geomap#canvas#event#connect#after#key_press key_press); let tab_label = GPack.hbox () in @@ -581,49 +581,49 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id with exc -> log alert ac_id (Printexc.to_string exc); None in - + let rc_settings_page = try let xml_settings = Xml.children (ExtXml.child settings_xml "rc_settings") in if xml_settings = [] then - raise Exit + raise Exit else - let settings_tab = new Pages.rc_settings ~visible xml_settings in - let tab_label = (GMisc.label ~text:"RC Settings" ())#coerce in - ignore (ac_notebook#append_page ~tab_label settings_tab#widget); - Some settings_tab + let settings_tab = new Pages.rc_settings ~visible xml_settings in + let tab_label = (GMisc.label ~text:"RC Settings" ())#coerce in + ignore (ac_notebook#append_page ~tab_label settings_tab#widget); + Some settings_tab with _ -> None in let wp_HOME = let rec loop = function - [] -> None + [] -> None | w::ws -> - let (_i, w) = fp#index w in - if w#name = "HOME" then Some w else loop ws in + let (_i, w) = fp#index w in + if w#name = "HOME" then Some w else loop ws in loop fp#waypoints in let ac = { track = track; color = color; last_dist_to_wp = 0.; - fp_group = fp ; config = config ; wp_HOME = wp_HOME; - fp = fp_xml; ac_name = name; - blocks = blocks; last_ap_mode= ""; - last_stage = (-1,-1); - ir_page = ir_page; flight_time = 0; - gps_page = gps_page; - pfd_page = pfd_page; - misc_page = misc_page; - dl_settings_page = dl_settings_page; - rc_settings_page = rc_settings_page; - strip = strip; first_pos = true; - last_block_name = ""; alt = 0.; target_alt = 0.; - in_kill_mode = false; speed = 0.; - wind_dir = 42.; ground_prox = true; - wind_speed = 0.; - pages = ac_frame#coerce; - notebook_label = _label; - got_track_status_timer = 1000; - dl_values = [||]; last_unix_time = 0.; - airspeed = 0. - } in + fp_group = fp ; config = config ; wp_HOME = wp_HOME; + fp = fp_xml; ac_name = name; + blocks = blocks; last_ap_mode= ""; + last_stage = (-1,-1); + ir_page = ir_page; flight_time = 0; + gps_page = gps_page; + pfd_page = pfd_page; + misc_page = misc_page; + dl_settings_page = dl_settings_page; + rc_settings_page = rc_settings_page; + strip = strip; first_pos = true; + last_block_name = ""; alt = 0.; target_alt = 0.; + in_kill_mode = false; speed = 0.; + wind_dir = 42.; ground_prox = true; + wind_speed = 0.; + pages = ac_frame#coerce; + notebook_label = _label; + got_track_status_timer = 1000; + dl_values = [||]; last_unix_time = 0.; + airspeed = 0. + } in Hashtbl.add aircrafts ac_id ac; select_ac acs_notebook ac_id; @@ -633,70 +633,70 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id if misc_page#periodic_send then begin (* FIXME: Disabling the timer would be preferable *) try - let a = (pi/.2. -. ac.wind_dir) - and w = ac.wind_speed in + let a = (pi/.2. -. ac.wind_dir) + and w = ac.wind_speed in - let wind_east = sprintf "%.1f" (-. cos a *. w) - and wind_north = sprintf "%.1f" (-. sin a *. w) - and airspeed = sprintf "%.1f" ac.airspeed in - - let msg_items = ["WIND_INFO"; ac_id; "42"; wind_east; wind_north;airspeed] in - let value = String.concat ";" msg_items in - let vs = ["ac_id", Pprz.String ac_id; "message", Pprz.String value] in - Ground_Pprz.message_send "dl" "RAW_DATALINK" vs; + let wind_east = sprintf "%.1f" (-. cos a *. w) + and wind_north = sprintf "%.1f" (-. sin a *. w) + and airspeed = sprintf "%.1f" ac.airspeed in + + let msg_items = ["WIND_INFO"; ac_id; "42"; wind_east; wind_north;airspeed] in + let value = String.concat ";" msg_items in + let vs = ["ac_id", Pprz.String ac_id; "message", Pprz.String value] in + Ground_Pprz.message_send "dl" "RAW_DATALINK" vs; with - exc -> log alert ac_id (sprintf "send_wind (%s): %s" ac_id (Printexc.to_string exc)) + exc -> log alert ac_id (sprintf "send_wind (%s): %s" ac_id (Printexc.to_string exc)) end; true in - + if is_int ac_id then ignore (Glib.Timeout.add 10000 send_wind); - + begin match dl_settings_page with Some settings_tab -> (** Connect the strip buttons *) - let connect = fun ?(warning=true) setting_name strip_connect -> - try - let id = settings_tab#assoc setting_name in - strip_connect (fun x -> dl_setting_callback id x) - with Not_found -> - if warning then - fprintf stderr "Warning: %s not setable from GCS strip (i.e. not listed in the xml settings file)" setting_name in + let connect = fun ?(warning=true) setting_name strip_connect -> + try + let id = settings_tab#assoc setting_name in + strip_connect (fun x -> dl_setting_callback id x) + with Not_found -> + if warning then + fprintf stderr "Warning: %s not setable from GCS strip (i.e. not listed in the xml settings file)" setting_name in - connect "flight_altitude" (fun f -> ac.strip#connect_shift_alt (fun x -> f (ac.target_alt+.x))); - connect "launch" ac.strip#connect_launch; - connect "kill_throttle" ac.strip#connect_kill; - connect "nav_shift" ac.strip#connect_shift_lateral; - connect "pprz_mode" ac.strip#connect_mode; - connect "estimator_flight_time" ac.strip#connect_flight_time; - let get_ac_unix_time = fun () -> ac.last_unix_time in - connect ~warning:false "snav_desired_tow" (ac.strip#connect_apt get_ac_unix_time); - begin (* Periodically update the appointment *) - try - let id = settings_tab#assoc "snav_desired_tow" in - let set_appointment = fun _ -> - begin try - let v = ac.dl_values.(id) in - let t = Unix.gmtime (Latlong.unix_time_of_tow (truncate v)) in - ac.strip#set_label "apt" (sprintf "%d:%02d:%02d" t.Unix.tm_hour t.Unix.tm_min t.Unix.tm_sec) - with _ -> () end; - true - in - ignore (Glib.Timeout.add 1000 set_appointment) - with Not_found -> () - end; + connect "flight_altitude" (fun f -> ac.strip#connect_shift_alt (fun x -> f (ac.target_alt+.x))); + connect "launch" ac.strip#connect_launch; + connect "kill_throttle" ac.strip#connect_kill; + connect "nav_shift" ac.strip#connect_shift_lateral; + connect "pprz_mode" ac.strip#connect_mode; + connect "estimator_flight_time" ac.strip#connect_flight_time; + let get_ac_unix_time = fun () -> ac.last_unix_time in + connect ~warning:false "snav_desired_tow" (ac.strip#connect_apt get_ac_unix_time); + begin (* Periodically update the appointment *) + try + let id = settings_tab#assoc "snav_desired_tow" in + let set_appointment = fun _ -> + begin try + let v = ac.dl_values.(id) in + let t = Unix.gmtime (Latlong.unix_time_of_tow (truncate v)) in + ac.strip#set_label "apt" (sprintf "%d:%02d:%02d" t.Unix.tm_hour t.Unix.tm_min t.Unix.tm_sec) + with _ -> () end; + true + in + ignore (Glib.Timeout.add 1000 set_appointment) + with Not_found -> () + end; - (** Connect the GPS reset button *) - begin - try - let gps_reset_id = settings_tab#assoc "gps.reset" in - gps_page#connect_reset - (fun x -> dl_setting_callback gps_reset_id (float x)) - with Not_found -> - prerr_endline "Warning: GPS reset not setable from GCS (i.e. 'gps.reset' not listed in the xml settings file)" - end + (** Connect the GPS reset button *) + begin + try + let gps_reset_id = settings_tab#assoc "gps.reset" in + gps_page#connect_reset + (fun x -> dl_setting_callback gps_reset_id (float x)) + with Not_found -> + prerr_endline "Warning: GPS reset not setable from GCS (i.e. 'gps.reset' not listed in the xml settings file)" + end | None -> () end; @@ -717,7 +717,7 @@ let alert_color = "red" (** Bind to message while catching all the esceptions of the callback *) let safe_bind = fun msg cb -> let safe_cb = fun sender vs -> - try cb sender vs with + try cb sender vs with AC_not_found -> () (* A/C not yet registed; silently ignore *) | x -> fprintf stderr "%s: safe_bind (%s:%a): %s\n%!" Sys.argv.(0) msg (fun c vs -> List.iter (fun (_,v) -> fprintf c "%s " (Pprz.string_of_value v)) vs) vs (Printexc.to_string x) in ignore (Ground_Pprz.message_bind msg safe_cb) @@ -729,7 +729,7 @@ let alert_bind = fun msg cb -> let tele_bind = fun msg cb -> let safe_cb = fun sender vs -> - try cb sender vs with + try cb sender vs with AC_not_found -> () (* A/C not yet registed; silently ignore *) | x -> fprintf stderr "tele_bind (%s): %s\n%!" msg (Printexc.to_string x) in ignore (Tele_Pprz.message_bind msg safe_cb) @@ -741,12 +741,12 @@ let ask_config = fun alert geomap fp_notebook ac -> in Ground_Pprz.message_req "gcs" "CONFIG" ["ac_id", Pprz.String ac] get_config - + let one_new_ac = fun alert (geomap:G.widget) fp_notebook ac -> if not (Hashtbl.mem aircrafts ac) then ask_config alert geomap fp_notebook ac - + let get_wind_msg = fun (geomap:G.widget) _sender vs -> let ac = get_ac vs in @@ -780,14 +780,14 @@ let get_fbw_msg = fun alarm _sender vs -> if mode = "FAILSAFE" then begin log_and_say alarm ac.ac_name (sprintf "%s, mayday, AP Failure. Switch to manual." ac.ac_name) end - + let get_engine_status_msg = fun _sender vs -> let ac = get_ac vs in ac.strip#set_throttle ~kill:ac.in_kill_mode (Pprz.float_assoc "throttle" vs); ac.strip#set_bat (Pprz.float_assoc "bat" vs) - + let get_if_calib_msg = fun _sender vs -> let ac = get_ac vs in match ac.rc_settings_page with @@ -809,7 +809,7 @@ let listen_if_calib_msg = fun () -> safe_bind "INFLIGH_CALIB" get_if_calib_msg let list_separator = Str.regexp "," - + let aircrafts_msg = fun alert (geomap:G.widget) fp_notebook acs -> let acs = Pprz.string_assoc "ac_list" acs in let acs = Str.split list_separator acs in @@ -821,12 +821,12 @@ let listen_dl_value = fun () -> let ac = get_ac vs in match ac.dl_settings_page with Some settings -> - let csv = Pprz.string_assoc "values" vs in - let values = Array.map float_of_string (Array.of_list (Str.split list_separator csv)) in - ac.dl_values <- values; - for i = 0 to min (Array.length values) settings#length - 1 do - settings#set i (try values.(i) with _ -> failwith (sprintf "values.(%d)" i)) - done + let csv = Pprz.string_assoc "values" vs in + let values = Array.map float_of_string (Array.of_list (Str.split list_separator csv)) in + ac.dl_values <- values; + for i = 0 to min (Array.length values) settings#length - 1 do + settings#set i (try values.(i) with _ -> failwith (sprintf "values.(%d)" i)) + done | None -> () in safe_bind "DL_VALUES" get_dl_value @@ -844,7 +844,7 @@ let check_approaching = fun ac geo alert -> | Some ac_pos -> let d = LL.wgs84_distance ac_pos geo in if d < ac.speed *. approaching_alert_time then - log_and_say alert ac.ac_name (sprintf "%s, approaching" ac.ac_name) + log_and_say alert ac.ac_name (sprintf "%s, approaching" ac.ac_name) let ac_alt_graph = [14,0;-5,0;-7,-6] @@ -883,9 +883,9 @@ let draw_altgraph = fun (da_object:Gtk_tools.pixmap_in_drawin_area) (geomap:MapC dr#put_layout ~x ~y:(y-h) ~fore:(`NAME color) layout in (* find min and max alt *) - let max_alt = ref 0 + let max_alt = ref 0 and min_alt = ref 35786000 in - Hashtbl.iter (fun _ac_id ac -> + Hashtbl.iter (fun _ac_id ac -> let track = ac.track in let alt = (truncate track#last_altitude) in let ground_alt = alt - (truncate (track#height ())) in @@ -910,7 +910,7 @@ let draw_altgraph = fun (da_object:Gtk_tools.pixmap_in_drawin_area) (geomap:MapC done; (* aircrafts *) - Hashtbl.iter (fun _ac_id ac -> + Hashtbl.iter (fun _ac_id ac -> dr#set_foreground (`NAME ac.color); let track = ac.track in match track#last with @@ -970,27 +970,27 @@ module GCS_icon = struct let timeout = 10000 (* ms : time before changing to outdated color *) let display = fun (geomap:G.widget) vs -> - let item = + let item = match !status with - None -> (* First call, create the graphical object *) - GnoCanvas.ellipse ~fill_color ~props:[`WIDTH_PIXELS 2] - ~x1: ~-.radius ~y1: ~-.radius ~x2:radius ~y2:radius - geomap#canvas#root + None -> (* First call, create the graphical object *) + GnoCanvas.ellipse ~fill_color ~props:[`WIDTH_PIXELS 2] + ~x1: ~-.radius ~y1: ~-.radius ~x2:radius ~y2:radius + geomap#canvas#root | Some (item, timeout_handle) -> (* Remove the timeouted color modification *) - Glib.Timeout.remove timeout_handle; - item in - + Glib.Timeout.remove timeout_handle; + item in + item#set [`OUTLINE_COLOR color]; - let change_color_if_not_updated = + let change_color_if_not_updated = Glib.Timeout.add 10000 (fun () -> item#set [`OUTLINE_COLOR outdated_color]; false) in (* Store the object and the timeout to change its color *) status := Some (item, change_color_if_not_updated); - + let lat = Pprz.float_assoc "lat" vs and lon = Pprz.float_assoc "long" vs in let wgs84 = LL.make_geo_deg lat lon in - + geomap#move_item item wgs84 end (* module GCS_icon *) @@ -1012,25 +1012,25 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> pfd_page#set_alt alt; pfd_page#set_climb climb; pfd_page#set_speed speed; - + let wgs84 = { posn_lat=(Deg>>Rad)(a "lat"); posn_long = (Deg>>Rad)(a "long") } in ac.track#move_icon wgs84 (a "heading") alt speed climb; ac.speed <- speed; - + let unix_time = a "unix_time" in if unix_time > ac.last_unix_time then begin - let utc = Unix.gmtime unix_time in - geomap#set_utc_time utc.Unix.tm_hour utc.Unix.tm_min utc.Unix.tm_sec; - ac.last_unix_time <- unix_time + let utc = Unix.gmtime unix_time in + geomap#set_utc_time utc.Unix.tm_hour utc.Unix.tm_min utc.Unix.tm_sec; + ac.last_unix_time <- unix_time end; - + if auto_center_new_ac && ac.first_pos then begin - center geomap ac.track (); - ac.first_pos <- false + center geomap ac.track (); + ac.first_pos <- false end; - + let set_label = fun lbl_name value -> - ac.strip#set_label lbl_name (sprintf "%.0fm" value) + ac.strip#set_label lbl_name (sprintf "%.0fm" value) in set_label "altitude" alt; ac.strip#set_speed speed; @@ -1039,10 +1039,10 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> ac.alt <- alt; ac.strip#set_agl agl; if not ac.ground_prox && ac.flight_time > 10 && agl < 20. then begin - log_and_say alert ac.ac_name (sprintf "%s, %s" ac.ac_name "Ground Proximity Warning"); + log_and_say alert ac.ac_name (sprintf "%s, %s" ac.ac_name "Ground Proximity Warning"); ac.ground_prox <- true end else if agl > 25. then - ac.ground_prox <- false; + ac.ground_prox <- false; try if not (alt_graph#drawing_area#misc#parent = None) then draw_altgraph alt_graph geomap aircrafts @@ -1080,28 +1080,28 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> (* Estimated Time Arrival to next waypoint *) let d = Pprz.float_assoc "dist_to_wp" vs in - let label = + let label = if d = 0. || ac.speed = 0. then - "N/A" + "N/A" else - sprintf "%.0fs" (d /. ac.speed) in + sprintf "%.0fs" (d /. ac.speed) in ac.strip#set_label "eta_time" label; ac.last_dist_to_wp <- d; (* Estimated Time to HOME *) try match ac.wp_HOME with - Some wp_HOME -> - let (bearing_to_HOME_deg, d) = Latlong.bearing ac.track#pos wp_HOME#pos in - let bearing_to_HOME = (Deg>>Rad)bearing_to_HOME_deg in - let wind_north = -. ac.wind_speed *. cos ac.wind_dir - and wind_east = -. ac.wind_speed *. sin ac.wind_dir in - let c = ac.wind_speed *. ac.wind_speed -. ac.airspeed *. ac.airspeed - and scal = wind_east *. sin bearing_to_HOME +. wind_north *. cos bearing_to_HOME in - let delta = 4. *. (scal*.scal -. c) in - let ground_speed_to_HOME = scal +. sqrt delta /. 2. in - let time_to_HOME = d /. ground_speed_to_HOME in - ac.misc_page#set_value "Time to HOME" (sprintf "%.0fs" time_to_HOME) + Some wp_HOME -> + let (bearing_to_HOME_deg, d) = Latlong.bearing ac.track#pos wp_HOME#pos in + let bearing_to_HOME = (Deg>>Rad)bearing_to_HOME_deg in + let wind_north = -. ac.wind_speed *. cos ac.wind_dir + and wind_east = -. ac.wind_speed *. sin ac.wind_dir in + let c = ac.wind_speed *. ac.wind_speed -. ac.airspeed *. ac.airspeed + and scal = wind_east *. sin bearing_to_HOME +. wind_north *. cos bearing_to_HOME in + let delta = 4. *. (scal*.scal -. c) in + let ground_speed_to_HOME = scal +. sqrt delta /. 2. in + let time_to_HOME = d /. ground_speed_to_HOME in + ac.misc_page#set_value "Time to HOME" (sprintf "%.0fs" time_to_HOME) | _ -> () with _NotSoImportant -> () @@ -1113,7 +1113,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> let a = fun s -> Pprz.float_assoc s vs in let cam_wgs84 = { posn_lat = (Deg>>Rad)(a "cam_lat"); posn_long = (Deg>>Rad)(a "cam_long") } and target_wgs84 = { posn_lat = (Deg>>Rad)(a "cam_target_lat"); posn_long = (Deg>>Rad)(a "cam_target_long") } in - + ac.track#move_cam cam_wgs84 target_wgs84 in safe_bind "CAM_STATUS" get_cam_status; @@ -1123,7 +1123,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> ac.got_track_status_timer <- 0; let a = fun s -> Pprz.float_assoc s vs in let wgs84 = { posn_lat = (Deg>>Rad)(a "circle_lat"); posn_long = (Deg>>Rad)(a "circle_long") } in - ac.track#draw_circle wgs84 (float_of_string (Pprz.string_assoc "radius" vs)) + ac.track#draw_circle wgs84 (float_of_string (Pprz.string_assoc "radius" vs)) in safe_bind "CIRCLE_STATUS" get_circle_status; @@ -1134,7 +1134,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> let geo1 = { posn_lat = (Deg>>Rad)(a "segment1_lat"); posn_long = (Deg>>Rad)(a "segment1_long") } and geo2 = { posn_lat = (Deg>>Rad)(a "segment2_lat"); posn_long = (Deg>>Rad)(a "segment2_long") } in ac.track#draw_segment geo1 geo2; - + (* Check if approaching the end of the segment *) check_approaching ac geo2 alert in @@ -1162,7 +1162,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> ac.last_ap_mode <- ap_mode; let label = Pprz.string_assoc "ap_mode" vs in ac.strip#set_label "AP" (if label="MANUAL" then "MANU" else label); - let color = + let color = match ap_mode with "AUTO2" | "NAV" -> ok_color | "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" -> "#10F0E0" @@ -1171,26 +1171,26 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> ac.strip#set_color "AP" color; end; let status_filter_mode = Pprz.string_assoc "state_filter_mode" vs in - let gps_mode = + let gps_mode = if (status_filter_mode <> "UNKNOWN") && (status_filter_mode <> "OK") && (status_filter_mode <> "GPS_LOST") then status_filter_mode else Pprz.string_assoc "gps_mode" vs in ac.strip#set_label "GPS" gps_mode; ac.strip#set_color "GPS" (if gps_mode<>"3D" then alert_color else ok_color); - let ft = + let ft = sprintf "%02d:%02d:%02d" (flight_time / 3600) ((flight_time / 60) mod 60) (flight_time mod 60) in ac.strip#set_label "flight_time" ft; let kill_mode = Pprz.string_assoc "kill_mode" vs in if kill_mode <> "OFF" then begin if not ac.in_kill_mode then - log_and_say alert ac.ac_name (sprintf "%s, mayday, kill mode" ac.ac_name); + log_and_say alert ac.ac_name (sprintf "%s, mayday, kill mode" ac.ac_name); ac.in_kill_mode <- true end else ac.in_kill_mode <- false; match ac.rc_settings_page with None -> () - | Some p -> - p#set_rc_mode ap_mode + | Some p -> + p#set_rc_mode ap_mode in safe_bind "AP_STATUS" get_ap_status; @@ -1211,13 +1211,13 @@ let listen_waypoint_moved = fun () -> Not_found -> () (* Silently ignore unknown waypoints *) in safe_bind "WAYPOINT_MOVED" get_values - -let get_alert_bat_low = fun a _sender vs -> + +let get_alert_bat_low = fun a _sender vs -> let ac = get_ac vs in let level = Pprz.string_assoc "level" vs in - log_and_say a ac.ac_name (sprintf "%s %s %s" ac.ac_name "BAT LOW" level) + log_and_say a ac.ac_name (sprintf "%s, %s %s" ac.ac_name "BAT LOW" level) -let listen_alert = fun a -> +let listen_alert = fun a -> alert_bind "BAT_LOW" (get_alert_bat_low a) let get_svsinfo = fun alarm _sender vs -> @@ -1226,25 +1226,25 @@ let get_svsinfo = fun alarm _sender vs -> let svids = Str.split list_separator (Pprz.string_assoc "svid" vs) and cn0s = Str.split list_separator (Pprz.string_assoc "cno" vs) and flagss = Str.split list_separator (Pprz.string_assoc "flags" vs) - and ages = Str.split list_separator (Pprz.string_assoc "msg_age" vs) in + and ages = Str.split list_separator (Pprz.string_assoc "msg_age" vs) in let a = Array.create (List.length svids) (0,0,0,0) in let rec loop = fun i s c f ages -> match (s, c, f, ages) with [], [], [], [] -> () | s::ss, c::cs, f::fs, age::ages -> - a.(i) <- (int_of_string s, int_of_string c, int_of_string f, int_of_string age); - loop (i+1) ss cs fs ages + a.(i) <- (int_of_string s, int_of_string c, int_of_string f, int_of_string age); + loop (i+1) ss cs fs ages | _ -> assert false in loop 0 svids cn0s flagss ages; let pacc = Pprz.int_assoc "pacc" vs in - + gps_page#svsinfo pacc a; if pacc > 1500 && pacc < 9999 then log_and_say alarm "gcs" (sprintf "GPS acc: %d m" (pacc / 100)) - + let listen_svsinfo = fun a -> safe_bind "SVSINFO" (get_svsinfo a) let message_request = Ground_Pprz.message_req @@ -1254,7 +1254,7 @@ let get_ts = fun _sender vs -> let t = Pprz.float_assoc "time_since_last_bat_msg" vs in ac.strip#set_label "telemetry_status" (if t > 2. then sprintf "%.0f" t else " "); ac.strip#set_color "telemetry_status" (if t > 5. then alert_color else ok_color) - + let listen_telemetry_status = fun () -> safe_bind "TELEMETRY_STATUS" get_ts @@ -1265,15 +1265,15 @@ let mark_dcshot = fun (geomap:G.widget) _sender vs -> (* let ac = get_ac vs in *) match ac.track#last with Some geo -> - begin - let group = geomap#background in - let point = geomap#circle ~group ~fill_color:"yellow" geo 3. in - point#raise_to_top () - end + begin + let group = geomap#background in + let point = geomap#circle ~group ~fill_color:"yellow" geo 3. in + point#raise_to_top () + end | None -> () (* mark geomap ac.ac_name track !Plugin.frame *) - + let listen_dcshot = fun _geom -> tele_bind "DC_SHOT" (mark_dcshot _geom) @@ -1327,9 +1327,9 @@ let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac alt let callback = fun i -> let ac_page = ac_notebook#get_nth_page i in Hashtbl.iter - (fun ac_id ac -> - if ac.pages#get_oid = ac_page#get_oid - then select_ac ac_notebook ac_id) + (fun ac_id ac -> + if ac.pages#get_oid = ac_page#get_oid + then select_ac ac_notebook ac_id) aircrafts in ignore (ac_notebook#connect#switch_page ~callback); @@ -1343,4 +1343,3 @@ let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac alt | k when k = GdkKeysyms._c -> center_active () ; true | _ -> false in ignore (geomap#canvas#event#connect#after#key_press key_press) - diff --git a/sw/ground_segment/cockpit/speech.ml b/sw/ground_segment/cockpit/speech.ml index 31afa67a66..124d90986b 100644 --- a/sw/ground_segment/cockpit/speech.ml +++ b/sw/ground_segment/cockpit/speech.ml @@ -1,5 +1,38 @@ let active = ref false +let current_os = ref "not_set" + +(* These two functions are from sw/lib/defivybus.ml *) +let read_process_output command = + let buffer_size = 2048 in + let buffer = Buffer.create buffer_size in + let string = String.create buffer_size in + let in_channel = Unix.open_process_in command in + let chars_read = ref 1 in + while !chars_read <> 0 do + chars_read := input in_channel string 0 buffer_size; + Buffer.add_substring buffer string 0 !chars_read + done; + ignore (Unix.close_process_in in_channel); + Buffer.contents buffer + +let contains s substring = + try ignore (Str.search_forward (Str.regexp_string substring) s 0); true + with Not_found -> false + let say = fun s -> - if !active then - ignore (Sys.command (Printf.sprintf "spd-say '%s'&" s)) + ( + if !active then ( + (* Checks if the os is known and gets the uname if not *) + if contains !current_os "not_set" then ( + current_os := read_process_output "uname"; + ); + (* If the os is Darwin, then use "say" *) + if contains !current_os "Darwin" then ( + ignore (Sys.command (Printf.sprintf "say '%s'&" s)); + ) + (* If the os is anything else, use "spd-say" (add additional cases here if necessary) *) + else ( + ignore (Sys.command (Printf.sprintf "spd-say '%s'&" s)); + ) + ));; diff --git a/sw/tools/gen_modules.ml b/sw/tools/gen_modules.ml index ecbb843641..921952fa80 100644 --- a/sw/tools/gen_modules.ml +++ b/sw/tools/gen_modules.ml @@ -91,7 +91,11 @@ let print_init_functions = fun modules -> match Xml.tag i with "init" -> lprintf out_h "%s;\n" (Xml.attrib i "fun") | "periodic" -> if not (is_status_lock i) then - lprintf out_h "%s = %s;\n" (get_status_name i module_name) (try Xml.attrib i "autorun" with _ -> "TRUE") + lprintf out_h "%s = %s;\n" (get_status_name i module_name) (try match Xml.attrib i "autorun" with + "TRUE" | "true" -> "MODULES_START" + | "FALSE" | "false" | "LOCK" | "lock" -> "MODULES_IDLE" + | _ -> failwith "Error: Unknown autorun value (possible values are: TRUE, FALSE, LOCK(default))" + with _ -> "MODULES_IDLE" (* this should not be possible anyway *)) | _ -> ()) (Xml.children m)) modules;