Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 16:30:07 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

[modules] New magneto and pitot (#1665)

Browse Source 
This adds the functionality for a new type of Magneto + pitot. The module transmits PPRZ messages which contain magneto and airspeed information.
This commit is contained in:
Freek van Tienen
2016-05-18 14:29:09 +02:00
committed by Felix Ruess
parent 9dbee4e59c
commit b709198702
7 changed files with 250 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TUDELFT/tudelft_conf.xml
+1 -1
View File
@@ -216,7 +216,7 @@
telemetry="telemetry/rotorcraft_with_logger.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml"
settings_modules="modules/opa_controller.xml modules/geo_mag.xml modules/air_data.xml modules/temp_adc.xml modules/logger_sd_spi_direct.xml modules/gps_ubx_ucenter.xml"
settings_modules="modules/opa_controller.xml modules/air_data.xml modules/temp_adc.xml modules/logger_sd_spi_direct.xml modules/gps_ubx_ucenter.xml"
gui_color="#ffffdffac31f"
/>
<aircraft
conf/airframes/TUDELFT/tudelft_outback.xml
+18 -8
View File
@@ -29,8 +29,9 @@
<module name="intermcu" type="uart"/>
<module name="current_sensor"/>
<module name="opa_controller"/>
<module name="mag_pitot_uart" />
<module name="geo_mag"/>
<!--module name="geo_mag"/-->
<module name="air_data"/>
<module name="temp_adc"/>
<module name="logger_sd_spi_direct">
@@ -135,17 +136,26 @@
<define name="ACCEL_Z_NEUTRAL" value="-25"/>
<!-- replace this with your own calibration -->
<define name="MAG_X_NEUTRAL" value="-2"/>
<define name="MAG_Y_NEUTRAL" value="484"/>
<define name="MAG_Z_NEUTRAL" value="53"/>
<define name="MAG_X_SENS" value="4.02836351262" integer="16"/>
<define name="MAG_Y_SENS" value="3.97184580645" integer="16"/>
<define name="MAG_Z_SENS" value="4.08689235615" integer="16"/>
<define name="MAG_X_NEUTRAL" value="21"/>
<define name="MAG_Y_NEUTRAL" value="-40"/>
<define name="MAG_Z_NEUTRAL" value="23"/>
<define name="MAG_X_SENS" value="18.8762961741" integer="16"/>
<define name="MAG_Y_SENS" value="16.257695261" integer="16"/>
<define name="MAG_Z_SENS" value="16.4921417833" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
<!-- Rotate magneto compared to imu -90 degress -->
<define name="TO_MAG_PHI" value="0." unit="deg"/>
<define name="TO_MAG_THETA" value="0." unit="deg"/>
<define name="TO_MAG_PSI" value="-90." unit="deg"/>
<!-- Change sign to fix axis -->
<define name="MAG_X_SIGN" value="1"/>
<define name="MAG_Y_SIGN" value="1"/>
<define name="MAG_Z_SIGN" value="-1"/>
</section>
<section name="AHRS" prefix="AHRS_">
conf/boards/opa_ap_1.0.makefile
+8
View File
@@ -56,6 +56,14 @@ IMU_MPU_SPI_SLAVE_IDX ?= SPI_SLAVE1
# See baro_board.makefile
#
# Remote Magneto
#
MAG_PITOT_PORT ?= UART5
MAG_PITOT_BAUD ?= 125000
#
# default SPI logger configuration
#
conf/modules/mag_pitot_uart.xml
+33
View File
@@ -0,0 +1,33 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="mag_pitot_uart" dir="sensors">
<doc>
<description>Remotely located magnetometer and pitot tube over uart (RS232) communication</description>
<configure name="MAG_PITOT_PORT" value="UART5" description="select which uart it is connected to"/>
<configure name="MAG_PITOT_BAUD" value="125000" description="set the baudrate of the uart"/>
<define name="IMU_TO_MAG_PHI" value="0" description="rotation of magneto compared to the magneto in roll axis"/>
<define name="IMU_TO_MAG_THETA" value="0" description="rotation of magneto compared to the magneto in pitch axis"/>
<define name="IMU_TO_MAG_PSI" value="0" description="rotation of magneto compared to the magneto in yaw axis"/>
</doc>
<header>
<file name="mag_pitot_uart.h"/>
</header>
<init fun="mag_pitot_init()"/>
<event fun="mag_pitot_event()"/>
<makefile>
<!-- Configure default UART port and baudrate -->
<configure name="MAG_PITOT_PORT" default="UART5" case="upper|lower"/>
<configure name="MAG_PITOT_BAUD" default="125000"/>
<!-- Enable UART and set baudrate -->
<define name="USE_$(MAG_PITOT_PORT_UPPER)"/>
<define name="USE_$(MAG_PITOT_PORT_UPPER)_TX" value="FALSE"/>
<define name="$(MAG_PITOT_PORT_UPPER)_BAUD" value="$(MAG_PITOT_BAUD)"/>
<define name="MAG_PITOT_PORT" value="$(MAG_PITOT_PORT_LOWER)"/>
<!-- Sources and PPRZLink for transport -->
<file name="mag_pitot_uart.c"/>
<file name="pprz_transport.c" dir="pprzlink/src"/>
</makefile>
</module>
sw/airborne/modules/sensors/mag_pitot_uart.c
+132
View File
@@ -0,0 +1,132 @@
/*
* Copyright (C) C. De Wagter
* Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/sensors/mag_pitot_uart.c"
* @author C. De Wagter
* Remotely located magnetometer and pitot tube over uart (RS232) communication
*/
#include "modules/sensors/mag_pitot_uart.h"
#include "pprzlink/pprz_transport.h"
#include "pprzlink/intermcu_msg.h"
#include "mcu_periph/uart.h"
#include "subsystems/abi.h"
#include "subsystems/imu.h"
#include "state.h"
/* Main magneto structure */
static struct mag_pitot_t mag_pitot = {
.device = (&((MAG_PITOT_PORT).device)),
.msg_available = false
};
static uint8_t mp_msg_buf[128] __attribute__((aligned)); ///< The message buffer for the Magneto and pitot
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void mag_pitot_raw_downlink(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mag_unscaled.x, &imu.mag_unscaled.y,
&imu.mag_unscaled.z);
}
#endif
/* Initialize the magneto and pitot */
void mag_pitot_init() {
// Initialize transport protocol
pprz_transport_init(&mag_pitot.transport);
// Set the Imu to Magneto rotation
struct FloatEulers imu_to_mag_eulers =
{IMU_TO_MAG_PHI, IMU_TO_MAG_THETA, IMU_TO_MAG_PSI};
orientationSetEulers_f(&mag_pitot.imu_to_mag, &imu_to_mag_eulers);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_RAW, mag_pitot_raw_downlink);
#endif
}
/* Parse the InterMCU message */
static inline void mag_pitot_parse_msg(void)
{
uint32_t now_ts = get_sys_time_usec();
/* Parse the mag-pitot message */
uint8_t msg_id = mp_msg_buf[1];
switch (msg_id) {
/* Got a magneto message */
case DL_IMCU_REMOTE_MAG: {
struct Int32Vect3 raw_mag;
// Read the raw magneto information
raw_mag.x = DL_IMCU_REMOTE_MAG_mag_x(mp_msg_buf);
raw_mag.y = DL_IMCU_REMOTE_MAG_mag_y(mp_msg_buf);
raw_mag.z = DL_IMCU_REMOTE_MAG_mag_z(mp_msg_buf);
// Rotate the magneto
struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&mag_pitot.imu_to_mag);
int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &raw_mag);
// Send and set the magneto IMU data
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MAG_PITOT_ID, now_ts, &imu.mag);
break;
}
/* Got a barometer message */
case DL_IMCU_REMOTE_BARO: {
float pitot_stat = DL_IMCU_REMOTE_BARO_pitot_stat(mp_msg_buf);
float pitot_temp = DL_IMCU_REMOTE_BARO_pitot_temp(mp_msg_buf);
AbiSendMsgBARO_ABS(IMU_MAG_PITOT_ID, pitot_stat);
AbiSendMsgTEMPERATURE(IMU_MAG_PITOT_ID, pitot_temp);
break;
}
/* Got an airspeed message */
case DL_IMCU_REMOTE_AIRSPEED: {
// Should be updated to differential pressure
float pitot_ias = DL_IMCU_REMOTE_AIRSPEED_pitot_IAS(mp_msg_buf);
stateSetAirspeed_f(pitot_ias);
break;
}
default:
break;
}
}
/* We need to wait for incomming messages */
void mag_pitot_event() {
// Check if we got some message from the Magneto or Pitot
pprz_check_and_parse(mag_pitot.device, &mag_pitot.transport, mp_msg_buf, &mag_pitot.msg_available);
// If we have a message we should parse it
if (mag_pitot.msg_available) {
mag_pitot_parse_msg();
mag_pitot.msg_available = false;
}
}
sw/airborne/modules/sensors/mag_pitot_uart.h
+54
View File
@@ -0,0 +1,54 @@
/*
* Copyright (C) C. De Wagter
* Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/sensors/mag_pitot_uart.h"
* @author C. De Wagter
* Remotely located magnetometer and pitot tube over uart (RS232) communication
*/
#ifndef MAG_PITOT_UART_H
#define MAG_PITOT_UART_H
#include "std.h"
#include "generated/airframe.h"
#include "pprzlink/pprz_transport.h"
#include "math/pprz_orientation_conversion.h"
/* Set default rotation to 0 */
#if !defined IMU_TO_MAG_PHI && !defined IMU_TO_MAG_THETA && !defined IMU_TO_MAG_PSI
#define IMU_TO_MAG_PHI 0
#define IMU_TO_MAG_THETA 0
#define IMU_TO_MAG_PSI 0
#endif
/* Main magneto pitot strcuture */
struct mag_pitot_t {
struct link_device *device; ///< The device which is uses for communication
struct pprz_transport transport; ///< The transport layer (PPRZ)
struct OrientationReps imu_to_mag; ///< IMU to magneto translation
bool msg_available; ///< If we received a message
};
extern void mag_pitot_init(void);
extern void mag_pitot_event(void);
#endif
sw/airborne/subsystems/abi_sender_ids.h
+4
View File
@@ -111,6 +111,10 @@
#define MAG_HMC58XX_SENDER_ID 2
#endif
#ifndef IMU_MAG_PITOT_ID
#define IMU_MAG_PITOT_ID 50
#endif
/*
* IDs of GPS sensors
*/