Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-02 13:27:32 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Merge branch 'master' into dev

Browse Source
This commit is contained in:
Felix Ruess
2011-07-07 23:37:37 +02:00
parent c4ede742b4 7cadaea7da
commit 7b1621b24d
27 changed files with 765 additions and 424 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/CDW/ChimuTinyFw.xml
+1 -1
View File
@@ -182,7 +182,7 @@
<!-- Communication --> <!-- Communication -->
<subsystem name="telemetry" type="xbee_api"> <subsystem name="telemetry" type="xbee_api">
<configure name="MODEM_BAUD" value="B57600"/> <configure name="MODEM_BAUD" value="B9600"/>
</subsystem> </subsystem>
<!-- Actuators --> <!-- Actuators -->
conf/airframes/CDW/ChimuTinyFwSpi.xml
+5 -3
View File
@@ -157,7 +157,9 @@
<modules> <modules>
<load name="ins_chimu_spi.xml" /> <load name="ins_chimu_spi.xml" >
<define name="CHIMU_BIG_ENDIAN" />
</load>
</modules> </modules>
@@ -168,7 +170,7 @@
<define name="WIND_INFO_RET"/> <define name="WIND_INFO_RET"/>
<define name="LOITER_TRIM"/> <define name="LOITER_TRIM"/>
<define name="ALT_KALMAN"/> <define name="ALT_KALMAN"/>
<define name="NB_CHANNELS" value="5" /> <!-- <define name="NB_CHANNELS" value="5" /> -->
</target> </target>
<target name="sim" board="pc"/> <target name="sim" board="pc"/>
@@ -177,7 +179,7 @@
<!-- Communication --> <!-- Communication -->
<subsystem name="telemetry" type="xbee_api"> <subsystem name="telemetry" type="xbee_api">
<configure name="MODEM_BAUD" value="B57600"/> <configure name="MODEM_BAUD" value="B9600"/>
</subsystem> </subsystem>
<!-- Actuators --> <!-- Actuators -->
conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+6 -6
View File
@@ -7,11 +7,11 @@
<airframe name="Yapa v1"> <airframe name="Yapa v1">
<servos> <servos>
<servo name="THROTTLE" no="4" min="1000" neutral="1000" max="2000"/> <servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="AILERON_LEFT" no="5" min="1000" neutral="1500" max="2000"/> <servo name="AILERON_LEFT" no="1" min="1000" neutral="1500" max="2000"/>
<servo name="AILERON_RIGHT" no="1" min="2000" neutral="1500" max="1000"/> <servo name="AILERON_RIGHT" no="2" min="2000" neutral="1500" max="1000"/>
<servo name="ELEVATOR" no="8" min="2000" neutral="1500" max="1000"/> <servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
<servo name="RUDDER" no="9" min="1100" neutral="1500" max="1900"/> <servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
</servos> </servos>
<commands> <commands>
@@ -217,7 +217,7 @@
<firmware name="fixedwing"> <firmware name="fixedwing">
<target name="ap" board="tiny_2.11"> <target name="ap" board="lisa_l_1.1"> <!-- board="tiny_2.11"> -->
<define name="STRONG_WIND"/> <define name="STRONG_WIND"/>
<define name="WIND_INFO"/> <define name="WIND_INFO"/>
<define name="WIND_INFO_RET"/> <define name="WIND_INFO_RET"/>
conf/autopilot/subsystems/fixedwing/ahrs_dcm.makefile
+2 -1
View File
@@ -3,7 +3,7 @@
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" $(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
ifeq ($(ARCH), lpc21) ifeq ($(TARGET), ap)
ap.CFLAGS += -DUSE_AHRS ap.CFLAGS += -DUSE_AHRS
@@ -43,6 +43,7 @@ sim.CFLAGS += -DIR_PITCH_NEUTRAL_DEFAULT=0
sim.CFLAGS += -DUSE_INFRARED sim.CFLAGS += -DUSE_INFRARED
sim.srcs += subsystems/sensors/infrared.c 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_ir.c
sim.srcs += $(SRC_ARCH)/sim_imu.c sim.srcs += $(SRC_ARCH)/sim_imu.c
conf/modules/ins_chimu_spi.xml
+1
View File
@@ -14,6 +14,7 @@
<define name="USE_SPI" /> <define name="USE_SPI" />
<define name="INS_LINK" value="SpiSlave"/> <define name="INS_LINK" value="SpiSlave"/>
<define name="USE_USB_HIGH_PCLK" />
<file name="ins_chimu_spi.c"/> <file name="ins_chimu_spi.c"/>
<file name="imu_chimu.c"/> <file name="imu_chimu.c"/>
</makefile> </makefile>
conf/modules/ins_chimu_uart.xml
+6
View File
@@ -1,5 +1,11 @@
<!DOCTYPE module SYSTEM "module.dtd"> <!DOCTYPE module SYSTEM "module.dtd">
<!--
For older CHIMU v1.0 you should define CHIMU_BIG_ENDIAN
-->
<module name="ins"> <module name="ins">
<!-- <depend conflict="ins" --> <!-- <depend conflict="ins" -->
<header> <header>
conf/modules/photogrammetry_calculator.xml
+16
View File
@@ -0,0 +1,16 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="cartography">
<header>
<file name="photogrammetry_calculator.h"/>
</header>
<init fun="init_photogrammetry_calculator()"/>
<!--
<periodic fun="periodic_1Hz_demo()" freq="1." start="start_demo()" stop="stop_demo()" autorun="TRUE"/>
-->
<makefile>
<define name="photogrammetry_calculator_LED" value="2"/>
<file name="photogrammetry_calculator.c"/>
</makefile>
</module>
conf/settings/photogrammetry_calculator.xml
+24
View File
@@ -0,0 +1,24 @@
<!DOCTYPE settings SYSTEM "settings.dtd">
<settings>
<dl_settings name="control">
<dl_settings name="photocalc">
<dl_setting max="90" min="-90" step="1" module="cartography/photogrammetry_calculator" var="photogrammetry_sweep_angle" shortname="Angle" param="PHOTOGRAMMETRY_SWEEP_ANGLE" />
<dl_setting max="95" min="0" step="1" VAR="photogrammetry_sidelap" module="cartography/photogrammetry_calculator" shortname="SideLap %" param="PHOTOGRAMMETRY_SIDELAP" handler="UpdateSideLap" />
<dl_setting max="95" min="0" step="1" var="photogrammetry_overlap" module="cartography/photogrammetry_calculator" shortname="OverLap %" param="PHOTOGRAMMETRY_OVERLAP" handler="UpdateOverLap" />
<dl_setting max="200" min="1" step="1" var="photogrammetry_resolution" module="cartography/photogrammetry_calculator" shortname="Pixel mm" param="PHOTOGRAMMETRY_RESOLUTION" handler="UpdateResolution" />
<dl_setting max="500" min="1" step="1" var="photogrammetry_height_min" shortname="h_min" />
<dl_setting max="500" min="1" step="1" var="photogrammetry_height_max" shortname="h_max" />
<dl_setting max="500" min="1" step="1" var="photogrammetry_radius_min" shortname="r_min" />
<dl_setting max="500" min="1" step="1" var="photogrammetry_height" shortname="computed height" />
<dl_setting max="1000" min="1" step="1" var="photogrammetry_sidestep" shortname="computed sidestep" />
<dl_setting max="1000" min="1" step="1" var="photogrammetry_triggerstep" shortname="computed trigger" />
</dl_settings>
</dl_settings>
</settings>
sw/airborne/arch/lpc21/mcu_periph/spi_arch.c
+1 -1
View File
@@ -142,7 +142,7 @@ void SPI1_ISR(void) __attribute__((naked));
#define SSP_FRF 0x00 << 4 /* frame format : SPI */ #define SSP_FRF 0x00 << 4 /* frame format : SPI */
#define SSP_CPOL 0x00 << 6 /* clock polarity : SCK idles low */ #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_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 */ /* SSPCR1 settings */
#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */ #define SSP_LBM 0x00 << 0 /* loopback mode : disabled */
sw/airborne/arch/stm32/mcu_arch.c
+1 -1
View File
@@ -43,7 +43,7 @@ void mcu_arch_init(void) {
return; return;
#endif #endif
#ifdef HSE_TYPE_EXT_CLK #ifdef HSE_TYPE_EXT_CLK
#warning Using external clock #warning Info: Using external clock
/* Setup the microcontroller system. /* Setup the microcontroller system.
* Initialize the Embedded Flash Interface, * Initialize the Embedded Flash Interface,
* initialize the PLL and update the SystemFrequency variable. * initialize the PLL and update the SystemFrequency variable.
sw/airborne/arch/stm32/servos_direct_hw.h
+1 -1
View File
@@ -11,7 +11,7 @@ extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
#define SERVOS_TICS_OF_USEC(_v) (_v) #define SERVOS_TICS_OF_USEC(_v) (_v)
#define Actuator(_x) actuators_pwm_values[_x] #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 #define ActuatorsCommit actuators_pwm_commit
sw/airborne/arch/stm32/subsystems/settings_arch.c
+1 -1
View File
@@ -104,7 +104,6 @@ static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) {
} }
static int32_t flash_detect(struct FlashInfo* flash) { static int32_t flash_detect(struct FlashInfo* flash) {
uint32_t device_id;
flash->total_size = FLASH_SIZE_ * 0x400; 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 */ #else /* this is the correct way of detecting page sizes */
uint32_t device_id;
/* read device id */ /* read device id */
device_id = DBGMCU_IDCODE & DBGMCU_IDCODE_DEV_ID_MASK; device_id = DBGMCU_IDCODE & DBGMCU_IDCODE_DEV_ID_MASK;
sw/airborne/modules/cam_control/cam.c
+6 -2
View File
@@ -56,7 +56,7 @@ float test_cam_estimator_hspeed_dir;
#endif #endif
#ifdef CAM_TILT_NEUTRAL #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 #error CAM_TILT_MAX has to be different from CAM_TILT_NEUTRAL
#endif #endif
#if (CAM_TILT_NEUTRAL == CAM_TILT_MIN) #if (CAM_TILT_NEUTRAL == CAM_TILT_MIN)
@@ -99,7 +99,11 @@ void cam_waypoint_target(void);
void cam_ac_target(void); void cam_ac_target(void);
void cam_init( void ) { void cam_init( void ) {
#ifdef CAM_MODE0
cam_mode = CAM_MODE0;
#else
cam_mode = CAM_MODE_OFF; cam_mode = CAM_MODE_OFF;
#endif
} }
void cam_periodic( void ) { void cam_periodic( void ) {
@@ -255,7 +259,7 @@ void cam_nadir( void ) {
cam_target_x = estimator_x; cam_target_x = estimator_x;
cam_target_y = estimator_y; cam_target_y = estimator_y;
#endif #endif
cam_target_alt = 0; cam_target_alt = -10;
cam_target(); cam_target();
} }
sw/airborne/modules/cartography/photogrammetry_calculator.c
+101
View File
@@ -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);
}
sw/airborne/modules/cartography/photogrammetry_calculator.h
+134
View File
@@ -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:
<section name="Photogrammetry" prefix="PHOTOGRAMMETRY_">
<!-- Camera Parameters -->
<define name="FOCAL_LENGTH" value="35" unit="mm"/>
<define name="SENSOR_WIDTH" value="24" unit="mm"/> <!-- In direction of the plane's wings -->
<define name="SENSOR_HEIGHT" value="13.5" unit="mm"/> <!-- In direction of the plane's nose -->
<define name="PIXELS_WIDTH" value="1024" unit=""/>
<!-- Flight Safety Parameters -->
<define name="HEIGHT_MIN" value="35" unit="m"/>
<define name="HEIGHT_MAX" value="500" unit="m"/>
<define name="RADIUS_MIN" value="70" unit="m"/>
</section>
<modules>
<load name="photogrammetry_calculator.xml" />
</modules>
Add to flightplan or airframe file:
<!-- Photogrammetry Parameters: define these in the flightplan
<define name="OVERLAP" value="0.5" unit="PROCENT"/>
<define name="SIDELAP" value="0.5" unit="PROCENT"/>
<define name="RESOLUTION" value="50" unit="mm pixel projection"/>
-->
Add to flightplan
<header>
#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
</header>
<block group="survey" name="Initialize Poly Survey 56789" strip_button="Survey5678" strip_icon="survey.png">
<call fun="PhotogrammetryCalculatorPolygonSurvey(WP_5, 5)"/>
<call fun="PolygonSurvey()"/>
</block>
<block group="survey" name="Initialize ADV Poly 1234 Survey" strip_button="SurveyADV" strip_icon="survey.png">
<call fun="PhotogrammetryCalculatorPolygonSurveyADV(WP_1, 4)"/>
<call fun="poly_survey_adv()"/>
</block>
*/
#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
sw/airborne/modules/ins/imu_chimu.c
+29 -58
View File
@@ -43,26 +43,6 @@
#include "math.h" #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 * 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_PAYLOAD 5
#define CHIMU_STATE_MACHINE_XSUM 6 #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 // Communication Definitions
#define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above #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 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_Minor = pPayloadData[CHIMU_index]; CHIMU_index++;
pstData->gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index]<<8) & (0x0000FF00); CHIMU_index++; pstData->gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index]<<8) & (0x0000FF00); CHIMU_index++;
pstData->gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++; pstData->gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++;
return TRUE; return TRUE;
break; break;
case CHIMU_Msg_1_IMU_Raw: case CHIMU_Msg_1_IMU_Raw:
sw/airborne/modules/ins/imu_chimu.h
+42
View File
@@ -44,6 +44,48 @@
#ifndef CHIMU_DEFINED_H #ifndef CHIMU_DEFINED_H
#define 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 { typedef struct {
float phi; float phi;
float theta; float theta;
sw/airborne/modules/ins/ins_chimu_spi.c
+13 -22
View File
@@ -36,25 +36,23 @@ volatile uint8_t new_ins_attitude;
void ins_init( void ) 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 ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI 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 euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 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; new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
// Init
CHIMU_Init(&CHIMU_DATA); CHIMU_Init(&CHIMU_DATA);
// Quat Filter // Quat Filter
for (int i=0;i<7;i++) CHIMU_Checksum(quaternions,7);
{ InsSend(quaternions,7);
InsSend1(quaternions[i]);
}
// Wait a bit (SPI send zero) // Wait a bit (SPI send zero)
InsSend1(0); InsSend1(0);
@@ -64,11 +62,8 @@ void ins_init( void )
InsSend1(0); InsSend1(0);
// 50Hz data: attitude only // 50Hz data: attitude only
for (int i=0;i<12;i++) CHIMU_Checksum(rate,12);
{ InsSend(rate,12);
InsSend1(rate[i]);
}
} }
void parse_ins_msg( void ) void parse_ins_msg( void )
@@ -79,10 +74,10 @@ void parse_ins_msg( void )
if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) 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; new_ins_attitude = 1;
RunOnceEvery(25, LED_TOGGLE(3) );
if (CHIMU_DATA.m_attitude.euler.phi > M_PI) if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{ {
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
@@ -111,11 +106,7 @@ void ins_periodic_task( void )
// Fill X-speed // Fill X-speed
CHIMU_Checksum(centripedal,19); CHIMU_Checksum(centripedal,19);
InsSend(centripedal,19);
for (int i=0;i<19;i++)
{
InsSend1(centripedal[i]);
}
// Downlink Send // Downlink Send
} }
sw/airborne/modules/ins/ins_chimu_uart.c
+14 -15
View File
@@ -36,12 +36,12 @@ volatile uint8_t new_ins_attitude;
void ins_init( void ) 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 ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI 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 euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 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; new_ins_attitude = 0;
@@ -50,6 +50,13 @@ void ins_init( void )
CHIMU_Init(&CHIMU_DATA); CHIMU_Init(&CHIMU_DATA);
// Request Software version
for (int i=0;i<7;i++)
{
InsUartSend1(ping[i]);
}
// Quat Filter // Quat Filter
for (int i=0;i<7;i++) for (int i=0;i<7;i++)
{ {
@@ -57,17 +64,9 @@ void ins_init( void )
} }
// 50Hz // 50Hz
for (int i=0;i<12;i++) CHIMU_Checksum(rate,12);
{ InsSend(rate,12);
InsUartSend1(rate[i]);
}
} }
sw/airborne/modules/ins/ins_module.h
+1
View File
@@ -86,6 +86,7 @@ void parse_ins_buffer( uint8_t );
#define ReadInsBuffer() { while (InsLink(ChAvailable())&&!ins_msg_received) parse_ins_buffer(InsLink(Getch())); } #define ReadInsBuffer() { while (InsLink(ChAvailable())&&!ins_msg_received) parse_ins_buffer(InsLink(Getch())); }
#define InsSend1(c) InsLink(Transmit(c)) #define InsSend1(c) InsLink(Transmit(c))
#define InsUartSend1(c) InsSend1(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 InsUartSetBaudrate(_b) InsLink(SetBaudrate(_b))
#define InsUartRunning InsLink(TxRunning) #define InsUartRunning InsLink(TxRunning)
sw/airborne/modules/ins/ins_ppzuavimu.c
+2 -2
View File
@@ -68,10 +68,10 @@ void imu_impl_init(void)
#if PERIODIC_FREQUENCY == 60 #if PERIODIC_FREQUENCY == 60
/* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */ /* 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); ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
# warning ITG3200 read at 50Hz # warning Info: ITG3200 read at 50Hz
#else #else
# if PERIODIC_FREQUENCY == 120 # 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 */ /* 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); ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0);
# else # else
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+1 -1
View File
@@ -312,7 +312,7 @@ void Normalize(void)
float error=0; float error=0;
float temporary[3][3]; float temporary[3][3];
float renorm=0; float renorm=0;
boolean problem=FALSE; uint8_t problem=FALSE;
// Find the non-orthogonality of X wrt Y // Find the non-orthogonality of X wrt Y
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
sw/airborne/subsystems/gps/gps_ubx.h
+6 -1
View File
@@ -77,6 +77,10 @@ extern bool_t gps_configuring;
#define GpsParseOrConfigure() gps_ubx_read_message() #define GpsParseOrConfigure() gps_ubx_read_message()
#endif #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) { \ #define GpsEvent(_sol_available_callback) { \
if (GpsBuffer()) { \ if (GpsBuffer()) { \
ReadGpsBuffer(); \ ReadGpsBuffer(); \
@@ -84,7 +88,8 @@ extern bool_t gps_configuring;
if (gps_ubx.msg_available) { \ if (gps_ubx.msg_available) { \
GpsParseOrConfigure(); \ GpsParseOrConfigure(); \
if (gps_ubx.msg_class == UBX_NAV_ID && \ 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) { \ if (gps.fix == GPS_FIX_3D) { \
gps.last_fix_time = cpu_time_sec; \ gps.last_fix_time = cpu_time_sec; \
} \ } \
sw/ground_segment/cockpit/horizon.ml
+56 -49
View File
@@ -2,8 +2,8 @@
* $Id$ * $Id$
* *
* Multi aircrafts map display and flight plan editor * 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. * This file is part of paparazzi.
* *
@@ -20,7 +20,7 @@
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * 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 affine_pos_and_angle xw yw angle =
let cos_a = cos angle in let cos_a = cos angle in
let sin_a = sin 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. 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 let v = truncate v / step in
for i = Pervasives.max 0 (v - 5) to min (v + 5) (Array.length tab - 1) do (* FIXME *) for i = Pervasives.max 0 (v - 5) to min (v + 5) (Array.length tab - 1) do (* FIXME *)
if not tab.(i) then begin if not tab.(i) then begin
tab.(i) <- true; tab.(i) <- true;
draw i draw i
end end
done in done in
(** Yellow index *) (** 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 s = index_width in
let idx = GnoCanvas.polygon ~points:[|0.;0.;-.s;s/.2.;-.s;-.s/.2.|] ~fill_color:"yellow" root in let idx = GnoCanvas.polygon ~points:[|0.;0.;-.s;s/.2.;-.s;-.s/.2.|] ~fill_color:"yellow" root in
if index_on_right then 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
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 r, lazy_drawer
class h = fun ?packing size -> class h = fun ?packing size ->
let canvas = GnoCanvas.canvas ~aa:true ~width:size ~height:size ?packing () in let canvas = GnoCanvas.canvas ~aa:true ~width:size ~height:size ?packing () in
let _ = let _ =
canvas#set_center_scroll_region false; canvas#set_center_scroll_region false;
in in
@@ -103,12 +104,12 @@ class h = fun ?packing size ->
let pitch_scale = fun pitch -> pitch *. size2 *. 2. in let pitch_scale = fun pitch -> pitch *. size2 *. 2. in
let speed_scale = size2 /. 10. in let speed_scale = size2 /. 10. in
let alt_scale = size2 /. 50. in let alt_scale = size2 /. 50. in
let speed_width = size2/.5. in let speed_width = size2/.3. in
let alt_width = size2/.2.5 in let alt_width = size2/.2.25 in
let index_width = size2 /. 15. in let index_width = size2 /. 10. in
let xc = left_margin +. speed_width +. size2 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 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 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 _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 _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 in
let grads = fun ?(text=false) n s a b -> let grads = fun ?(text=false) n s a b ->
for i = 0 to n do 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);
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 if text then
let text = Printf.sprintf "%d" (truncate deg) let text = Printf.sprintf "%d" (truncate deg)
and x = 2.*.s in 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 disc);
ignore (GnoCanvas.text ~props:text_props ~text ~y:(-.y) ~x:(-.x) disc); ignore (GnoCanvas.text ~props:text_props ~text ~y:(-.y) ~x:(-.x) disc);
let text = "-"^text in let text = "-"^text in
ignore (GnoCanvas.text ~props:text_props ~text ~y ~x disc); ignore (GnoCanvas.text ~props:text_props ~text ~y ~x disc);
ignore (GnoCanvas.text ~props:text_props ~text ~y ~x:(-.x) disc); ignore (GnoCanvas.text ~props:text_props ~text ~y ~x:(-.x) disc);
done in done in
let _ =
let _ =
grads 10 (size2/.10.) 5. 2.5; grads 10 (size2/.10.) 5. 2.5;
grads 5 (size2/.7.) 10. 5.; grads 5 (size2/.7.) 10. 5.;
grads ~text:true 5 (size2/.5.) 10. 10. in 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 (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 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 points = floats_of_points (Array.append arc_above rest) in
let _ = let _ =
ignore (GnoCanvas.polygon ~fill_color:"black" ~points mask); ignore (GnoCanvas.polygon ~fill_color:"black" ~points mask);
for i = 0 to Array.length points / 2 - 1 do 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; done;
ignore (GnoCanvas.polygon ~fill_color:"black" ~points mask); ignore (GnoCanvas.polygon ~fill_color:"black" ~points mask);
let s = size2/. 5. in 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); ignore (GnoCanvas.line ~props:[`WIDTH_PIXELS 4] ~points:[|x;0.;x+.s;0.;x+.s;s|] ~fill_color:"black" mask);
(* Top and bottom graduations *) (* Top and bottom graduations *)
let g = fun a -> 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 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 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 let a = float (i*10) in
g a; g (-.a) g a; g (-.a)
done; 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 t#affine_relative (affine_pos_and_angle 0. 0. ((Deg>>Rad)a)) in
_30 30.; _30 (-30.) _30 30.; _30 (-30.)
in in
(* Speedometer on the left side *) (* 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 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 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 = let mx =
GnoCanvas.text ~x:(speed_width/.2.) ~y:(-0.85*.size2) ~props:text_props g GnoCanvas.text ~x:(speed_width/.2.) ~y:(-0.88*.size2) ~props:text_props g
and mi = 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"]; mx#set [`FILL_COLOR "yellow"];
mi#set [`FILL_COLOR "yellow"]; mi#set [`FILL_COLOR "yellow"];
lazy_ruler 0.; lazy_ruler 0.;
r, mi, mx, lazy_ruler r, mi, mx, lazy_ruler
(* Altimeter on the right side *) (* 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 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 ruler ~text_props ~max:3000 ~scale:alt_scale ~w:alt_width ~step:10 ~index_width ~h:(0.75*.size2) g
in in
object object
method set_attitude = fun roll pitch -> 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 max_speed = 0.
val mutable min_speed = max_float val mutable min_speed = max_float
method set_speed = fun (s:float) -> method set_speed = fun (s:float) ->
@@ -200,20 +209,20 @@ class h = fun ?packing size ->
lazy_speed s; lazy_speed s;
speed#affine_absolute (affine_pos 0. (speed_scale*.s)); speed#affine_absolute (affine_pos 0. (speed_scale*.s));
min_speed <- min min_speed 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)]; mi#set [`TEXT (sprintf "%.1f" min_speed)];
mx#set [`TEXT (sprintf "%.1f" max_speed)] mx#set [`TEXT (sprintf "%.1f" max_speed)]
initializer initializer
ignore (speed#connect#event (function ignore (speed#connect#event (function
`BUTTON_PRESS _ev -> `BUTTON_PRESS _ev ->
max_speed <- 0.; min_speed <- max_float; true max_speed <- 0.; min_speed <- max_float; true
| _ -> false)) | _ -> false))
method set_alt = fun (s:float) -> method set_alt = fun (s:float) ->
alt#affine_absolute (affine_pos 0. 0.); alt#affine_absolute (affine_pos 0. 0.);
lazy_alt s; lazy_alt s;
alt#affine_absolute (affine_pos 0. (alt_scale*.s)) alt#affine_absolute (affine_pos 0. (alt_scale*.s))
end end
(*****************************************************************************) (*****************************************************************************)
@@ -222,13 +231,11 @@ class h = fun ?packing size ->
class pfd ?(visible = fun _ -> true) (widget: GBin.frame) = class pfd ?(visible = fun _ -> true) (widget: GBin.frame) =
let horizon = new h ~packing: widget#add 150 in let horizon = new h ~packing: widget#add 150 in
let _lazy = fun f x -> if visible widget then f x in let _lazy = fun f x -> if visible widget then f x in
object object
method set_attitude roll pitch = 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_alt (a:float) = _lazy horizon#set_alt a
method set_climb (_c:float) = () method set_climb (_c:float) = ()
method set_speed (c:float) = _lazy horizon#set_speed c method set_speed (c:float) = _lazy horizon#set_speed c
end end
sw/ground_segment/cockpit/live.ml
+255 -256
View File
File diff suppressed because it is too large Load Diff
sw/ground_segment/cockpit/speech.ml
+35 -2
View File
@@ -1,5 +1,38 @@
let active = ref false 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 -> 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));
)
));;
sw/tools/gen_modules.ml
+5 -1
View File
@@ -91,7 +91,11 @@ let print_init_functions = fun modules ->
match Xml.tag i with match Xml.tag i with
"init" -> lprintf out_h "%s;\n" (Xml.attrib i "fun") "init" -> lprintf out_h "%s;\n" (Xml.attrib i "fun")
| "periodic" -> if not (is_status_lock i) then | "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)) (Xml.children m))
modules; modules;