mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-23 13:24:03 +08:00
made all infrared variable part of a structure
This commit is contained in:
Antoine Drouin
@@ -29,8 +29,8 @@
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="Infrared">
|
||||
<dl_setting MAX="15" MIN="-15" STEP="1" VAR="ir_roll_neutral" shortname="roll neutral" module="subsystems/sensors/infrared" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ir_pitch_neutral" shortname="pitch neutral" module="subsystems/sensors/infrared" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
<dl_setting MAX="15" MIN="-15" STEP="1" VAR="infrared.roll_neutral" shortname="roll neutral" module="subsystems/sensors/infrared" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch neutral" module="subsystems/sensors/infrared" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
|
||||
+10
-12
@@ -22,22 +22,20 @@
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="control">
|
||||
<dl_settings NAME="ir">
|
||||
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ir_roll_neutral" shortname="roll_neutral"/>
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ir_pitch_neutral" shortname="pitch_neutral"/>
|
||||
<dl_settings NAME="infrared">
|
||||
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" module="subsystems/sensors/infrared" />
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="ir_lateral_correction" shortname="360_lat_corr" module="subsystems/sensors/infrared"/>
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="ir_longitudinal_correction" shortname="360_log_corr"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_vertical_correction" shortname="360_vert_corr"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_left" shortname="corr_left"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_right" shortname="corr_right"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_up" shortname="corr_up"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_down" shortname="corr_down"/>
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.lateral_correction" shortname="lat_corr" param="IR_LATERAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.longitudinal_correction" shortname="log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.vertical_correction" shortname="vert_corr" param="IR_VERTICAL_CORRECTION"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_left" shortname="corr_left" param="IR_CORRECTION_LEFT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_right" shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_up" shortname="corr_up" param="IR_CORRECTION_UP"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_down" shortname="corr_down" param="IR_CORRECTION_DOWN"/>
|
||||
</dl_settings>
|
||||
|
||||
|
||||
<dl_settings NAME="attitude">
|
||||
<dl_setting MAX="1" MIN="0" STEP="0.05" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" module="stabilization/stabilization_attitude"/>
|
||||
<dl_setting MAX="000" MIN="-25000" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain"/>
|
||||
|
||||
@@ -30,18 +30,18 @@
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="control">
|
||||
<dl_settings NAME="ir">
|
||||
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ir_roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ir_pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
<dl_settings NAME="infrared">
|
||||
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" module="subsystems/sensors/infrared" />
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="ir_lateral_correction" shortname="360_lat_corr" module="subsystems/sensors/infrared" param="IR_LATERAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="ir_longitudinal_correction" shortname="360_log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_vertical_correction" shortname="360_vert_corr" param="IR_VERTICAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.lateral_correction" shortname="lat_corr" param="IR_LATERAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.longitudinal_correction" shortname="log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.vertical_correction" shortname="vert_corr" param="IR_VERTICAL_CORRECTION"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_left" shortname="corr_left" param="IR_CORRECTION_LEFT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_right" shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_up" shortname="corr_up" param="IR_CORRECTION_UP"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_down" shortname="corr_down" param="IR_CORRECTION_DOWN"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_left" shortname="corr_left" param="IR_CORRECTION_LEFT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_right" shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_up" shortname="corr_up" param="IR_CORRECTION_UP"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_down" shortname="corr_down" param="IR_CORRECTION_DOWN"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
|
||||
@@ -30,19 +30,18 @@
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="control">
|
||||
<dl_settings NAME="ir">
|
||||
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ir_roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ir_pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
<dl_settings NAME="infrared">
|
||||
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" module="subsystems/sensors/infrared" />
|
||||
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="ir_lateral_correction" shortname="360_lat_corr" module="subsystems/sensors/infrared" param="IR_LATERAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="ir_longitudinal_correction" shortname="360_log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_vertical_correction" shortname="360_vert_corr" param="IR_VERTICAL_CORRECTION"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_left" shortname="corr_left" param="IR_CORRECTION_LEFT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_right" shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_up" shortname="corr_up" param="IR_CORRECTION_UP"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="ir_correction_down" shortname="corr_down" param="IR_CORRECTION_DOWN"/>
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.lateral_correction" shortname="lat_corr" param="IR_LATERAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.longitudinal_correction" shortname="log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.vertical_correction" shortname="vert_corr" param="IR_VERTICAL_CORRECTION"/>
|
||||
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_left" shortname="corr_left" param="IR_CORRECTION_LEFT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_right" shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_up" shortname="corr_up" param="IR_CORRECTION_UP"/>
|
||||
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_down" shortname="corr_down" param="IR_CORRECTION_DOWN"/>
|
||||
</dl_settings>
|
||||
|
||||
|
||||
|
||||
@@ -6,8 +6,8 @@
|
||||
|
||||
<rc_settings>
|
||||
<rc_mode NAME="AUTO1">
|
||||
<rc_setting VAR="ir_pitch_neutral" RANGE="0.2" RC="gain_1_up" TYPE="float"/>
|
||||
<rc_setting VAR="ir_roll_neutral" RANGE="-0.2" RC="gain_1_down" TYPE="float"/>
|
||||
<rc_setting VAR="infrared.pitch_neutral" RANGE="0.2" RC="gain_1_up" TYPE="float"/>
|
||||
<rc_setting VAR="infrared.roll_neutral" RANGE="-0.2" RC="gain_1_down" TYPE="float"/>
|
||||
</rc_mode>
|
||||
<rc_mode NAME="AUTO2">
|
||||
<rc_setting VAR="h_ctl_course_pgain" RANGE="0.5" RC="gain_1_up" TYPE="float"/>
|
||||
|
||||
@@ -116,7 +116,7 @@
|
||||
#endif
|
||||
|
||||
#if defined USE_INFRARED || USE_INFRARED_I2C
|
||||
#define PERIODIC_SEND_IR_SENSORS(_chan) DOWNLINK_SEND_IR_SENSORS(_chan, &ir_ir1, &ir_ir2, &ir_pitch, &ir_roll, &ir_top);
|
||||
#define PERIODIC_SEND_IR_SENSORS(_chan) DOWNLINK_SEND_IR_SENSORS(_chan, &infrared.ir1, &infrared.ir2, &infrared.pitch, &infrared.roll, &infrared.top);
|
||||
#else
|
||||
#define PERIODIC_SEND_IR_SENSORS(_chan) ;
|
||||
#endif
|
||||
|
||||
@@ -21,9 +21,9 @@ void set_ir(double roll, double pitch) {
|
||||
double roll_sensor = roll + JSBSIM_IR_ROLL_NEUTRAL; // ir_roll_neutral;
|
||||
double pitch_sensor = pitch + JSBSIM_IR_PITCH_NEUTRAL; // ir_pitch_neutral;
|
||||
#ifdef USE_INFRARED
|
||||
ir_roll = sin(roll_sensor) * ir_contrast;
|
||||
ir_pitch = sin(pitch_sensor) * ir_contrast;
|
||||
ir_top = cos(roll_sensor) * cos(pitch_sensor) * ir_contrast;
|
||||
infrared.roll = sin(roll_sensor) * ir_contrast;
|
||||
infrared.pitch = sin(pitch_sensor) * ir_contrast;
|
||||
infrared.top = cos(roll_sensor) * cos(pitch_sensor) * ir_contrast;
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
@@ -22,9 +22,9 @@ value set_ir(value roll __attribute__ ((unused)),
|
||||
value air_speed
|
||||
) {
|
||||
#if defined USE_INFRARED || USE_INFRARED_I2C
|
||||
ir_roll = Int_val(roll);
|
||||
ir_pitch = Int_val(front);
|
||||
ir_top = Int_val(top);
|
||||
infrared.roll = Int_val(roll);
|
||||
infrared.pitch = Int_val(front);
|
||||
infrared.top = Int_val(top);
|
||||
#endif
|
||||
sim_air_speed = Double_val(air_speed);
|
||||
return Val_unit;
|
||||
|
||||
@@ -150,9 +150,9 @@ void dl_parse_msg(void) {
|
||||
/** Infrared and GPS sensors are replaced by messages on the datalink */
|
||||
if (msg_id == DL_HITL_INFRARED) {
|
||||
/** This code simulates infrared.c:ir_update() */
|
||||
ir_roll = DL_HITL_INFRARED_roll(dl_buffer);
|
||||
ir_pitch = DL_HITL_INFRARED_pitch(dl_buffer);
|
||||
ir_top = DL_HITL_INFRARED_top(dl_buffer);
|
||||
infrared.roll = DL_HITL_INFRARED_roll(dl_buffer);
|
||||
infrared.pitch = DL_HITL_INFRARED_pitch(dl_buffer);
|
||||
infrared.top = DL_HITL_INFRARED_top(dl_buffer);
|
||||
} else if (msg_id == DL_HITL_UBX) {
|
||||
/** This code simulates gps_ubx.c:parse_ubx() */
|
||||
if (gps_msg_received) {
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
/*
|
||||
* Paparazzi autopilot $Id$
|
||||
*
|
||||
* Copyright (C) 2004-2005 Pascal Brisset, Antoine Drouin
|
||||
* Copyright (C) 2004-2010 The Paparazzi Team
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -236,9 +236,9 @@ void estimator_update_state_gps( void ) {
|
||||
|
||||
#include "subsystems/sensors/infrared.h"
|
||||
void estimator_update_state_infrared( void ) {
|
||||
estimator_phi = atan2(ir_roll, ir_top) - ir_roll_neutral;
|
||||
estimator_phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
|
||||
|
||||
estimator_theta = atan2(ir_pitch, ir_top) - ir_pitch_neutral;
|
||||
estimator_theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
|
||||
|
||||
if (estimator_theta < -M_PI_2)
|
||||
estimator_theta += M_PI;
|
||||
@@ -246,13 +246,13 @@ void estimator_update_state_infrared( void ) {
|
||||
estimator_theta -= M_PI;
|
||||
|
||||
if (estimator_phi >= 0)
|
||||
estimator_phi *= ir_correction_right;
|
||||
estimator_phi *= infrared.correction_right;
|
||||
else
|
||||
estimator_phi *= ir_correction_left;
|
||||
estimator_phi *= infrared.correction_left;
|
||||
|
||||
if (estimator_theta >= 0)
|
||||
estimator_theta *= ir_correction_up;
|
||||
estimator_theta *= infrared.correction_up;
|
||||
else
|
||||
estimator_theta *= ir_correction_down;
|
||||
estimator_theta *= infrared.correction_down;
|
||||
|
||||
}
|
||||
|
||||
@@ -448,7 +448,7 @@ void periodic_task_ap( void ) {
|
||||
#endif
|
||||
|
||||
#ifdef USE_INFRARED
|
||||
ir_update();
|
||||
infrared_update();
|
||||
estimator_update_state_infrared();
|
||||
#endif /* USE_INFRARED */
|
||||
h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
|
||||
@@ -483,7 +483,7 @@ void init_ap( void ) {
|
||||
|
||||
/************* Sensors initialization ***************/
|
||||
#ifdef USE_INFRARED
|
||||
ir_init();
|
||||
infrared_init();
|
||||
#endif
|
||||
#ifdef USE_GYRO
|
||||
gyro_init();
|
||||
|
||||
@@ -21,7 +21,8 @@
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
/** \file infrared.c
|
||||
|
||||
/** \file subsystems/sensors/infrared.c
|
||||
* \brief Regroup all functions link to \a ir
|
||||
*/
|
||||
|
||||
@@ -32,31 +33,37 @@
|
||||
|
||||
#include "subsystems/sensors/infrared.h"
|
||||
#include "mcu_periph/adc.h"
|
||||
#include "ap_downlink.h"
|
||||
#include "sys_time.h"
|
||||
|
||||
#include BOARD_CONFIG
|
||||
#include "generated/airframe.h"
|
||||
|
||||
|
||||
#if defined IR_ESTIMATED_PHI_PI_4 || defined IR_ESTIMATED_PHI_MINUS_PI_4 || defined IR_ESTIMATED_THETA_PI_4
|
||||
#error "IR_ESTIMATED_PHI_PI_4 correction has been deprecated. Please remove the definition from your airframe config file"
|
||||
#endif
|
||||
#ifdef INFRARED
|
||||
#error "The flag INFRARED has been deprecated. Please replace it with USE_INFRARED."
|
||||
#endif
|
||||
#if defined IR_ADC_ROLL_NEUTRAL || defined IR_ADC_PITCH_NEUTRAL
|
||||
#error "Neutrals on ROLL and PITCH deprecated. Please define IR_ADC_IR1_NEUTRAL and IR_ADC_IR2_NEUTRAL"
|
||||
#endif
|
||||
#ifdef IR_360
|
||||
#warning "IR_360 flag deprecated. Now default"
|
||||
#endif
|
||||
#ifdef IR_360_LATERAL_CORRECTION
|
||||
#error "IR_360_LATERAL_CORRECTION now IR_LATERAL_CORRECTION"
|
||||
#endif
|
||||
|
||||
#ifdef IR_360_LONGITUDINAL_CORRECTION
|
||||
#error "IR_360_LONGITUDINAL_CORRECTION now IR_LONGITUDINAL_CORRECTION"
|
||||
#endif
|
||||
|
||||
#ifdef IR_360_VERTICAL_CORRECTION
|
||||
#error "IR_360_VERTICAL_CORRECTION now IR_VERTICAL_CORRECTION"
|
||||
#endif
|
||||
|
||||
|
||||
int16_t ir_ir1;
|
||||
int16_t ir_ir2;
|
||||
int16_t ir_roll;
|
||||
int16_t ir_pitch;
|
||||
int16_t ir_top;
|
||||
|
||||
float ir_roll_neutral;
|
||||
float ir_pitch_neutral;
|
||||
|
||||
float ir_correction_left;
|
||||
float ir_correction_right;
|
||||
float ir_correction_down;
|
||||
float ir_correction_up;
|
||||
struct Infrared infrared;
|
||||
|
||||
#if !defined IR_CORRECTION_LEFT
|
||||
#define IR_CORRECTION_LEFT 1.
|
||||
@@ -88,23 +95,6 @@ static struct adc_buf buf_ir_top;
|
||||
#define ADC_CHANNEL_IR_NB_SAMPLES DEFAULT_AV_NB_SAMPLE
|
||||
#endif
|
||||
|
||||
float ir_lateral_correction;
|
||||
float ir_longitudinal_correction;
|
||||
float ir_vertical_correction;
|
||||
|
||||
|
||||
#ifdef IR_360_LATERAL_CORRECTION
|
||||
#error "IR_360_LATERAL_CORRECTION now IR_LATERAL_CORRECTION"
|
||||
#endif
|
||||
|
||||
#ifdef IR_360_LONGITUDINAL_CORRECTION
|
||||
#error "IR_360_LONGITUDINAL_CORRECTION now IR_LONGITUDINAL_CORRECTION"
|
||||
#endif
|
||||
|
||||
#ifdef IR_360_VERTICAL_CORRECTION
|
||||
#error "IR_360_VERTICAL_CORRECTION now IR_VERTICAL_CORRECTION"
|
||||
#endif
|
||||
|
||||
#ifndef IR_LATERAL_CORRECTION
|
||||
#define IR_LATERAL_CORRECTION 1.
|
||||
#endif
|
||||
@@ -117,57 +107,48 @@ float ir_vertical_correction;
|
||||
#define IR_VERTICAL_CORRECTION 1.
|
||||
#endif
|
||||
|
||||
#ifdef IR_360
|
||||
#warning "IR_360 flag deprecated. Now default"
|
||||
#endif
|
||||
|
||||
|
||||
/** \brief Initialisation of \a ir */
|
||||
/** Initialize \a adc_buf_channel
|
||||
*/
|
||||
void ir_init(void) {
|
||||
void infrared_init(void) {
|
||||
#if ! (defined SITL || defined HITL)
|
||||
adc_buf_channel(ADC_CHANNEL_IR1, &buf_ir1, ADC_CHANNEL_IR_NB_SAMPLES);
|
||||
adc_buf_channel(ADC_CHANNEL_IR2, &buf_ir2, ADC_CHANNEL_IR_NB_SAMPLES);
|
||||
|
||||
#ifdef ADC_CHANNEL_IR_TOP
|
||||
adc_buf_channel(ADC_CHANNEL_IR_TOP, &buf_ir_top, ADC_CHANNEL_IR_NB_SAMPLES);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
ir_roll_neutral = RadOfDeg(IR_ROLL_NEUTRAL_DEFAULT);
|
||||
ir_pitch_neutral = RadOfDeg(IR_PITCH_NEUTRAL_DEFAULT);
|
||||
infrared.roll_neutral = RadOfDeg(IR_ROLL_NEUTRAL_DEFAULT);
|
||||
infrared.pitch_neutral = RadOfDeg(IR_PITCH_NEUTRAL_DEFAULT);
|
||||
|
||||
ir_correction_left = IR_CORRECTION_LEFT;
|
||||
ir_correction_right = IR_CORRECTION_RIGHT;
|
||||
ir_correction_up = IR_CORRECTION_UP;
|
||||
ir_correction_down = IR_CORRECTION_DOWN;
|
||||
infrared.correction_left = IR_CORRECTION_LEFT;
|
||||
infrared.correction_right = IR_CORRECTION_RIGHT;
|
||||
infrared.correction_up = IR_CORRECTION_UP;
|
||||
infrared.correction_down = IR_CORRECTION_DOWN;
|
||||
|
||||
ir_lateral_correction = IR_LATERAL_CORRECTION;
|
||||
ir_longitudinal_correction = IR_LONGITUDINAL_CORRECTION;
|
||||
ir_vertical_correction = IR_VERTICAL_CORRECTION;
|
||||
infrared.lateral_correction = IR_LATERAL_CORRECTION;
|
||||
infrared.longitudinal_correction = IR_LONGITUDINAL_CORRECTION;
|
||||
infrared.vertical_correction = IR_VERTICAL_CORRECTION;
|
||||
|
||||
#if ! (defined ADC_CHANNEL_IR_TOP || defined HITL || defined SITL)
|
||||
ir_top = IR_DEFAULT_CONTRAST;
|
||||
infrared.top = IR_DEFAULT_CONTRAST;
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined IR_ADC_ROLL_NEUTRAL || defined IR_ADC_PITCH_NEUTRAL
|
||||
#error "Neutrals on ROLL and PITCH deprecated. Please define IR_ADC_IR1_NEUTRAL and IR_ADC_IR2_NEUTRAL"
|
||||
#endif
|
||||
|
||||
#ifndef IR_IR1_SIGN
|
||||
#define IR_IR1_SIGN 1
|
||||
#endif // IR_IR1_SIGN
|
||||
#endif /* IR_IR1_SIGN */
|
||||
|
||||
#ifndef IR_IR2_SIGN
|
||||
#define IR_IR2_SIGN 1
|
||||
#endif // IR_IR2_SIGN
|
||||
#endif /* IR_IR2_SIGN */
|
||||
|
||||
#ifndef IR_TOP_SIGN
|
||||
#define IR_TOP_SIGN 1
|
||||
#endif // IR_TOP_SIGN
|
||||
#endif /* IR_TOP_SIGN */
|
||||
|
||||
/* Sensor installation */
|
||||
#if defined IR_HORIZ_SENSOR_ALIGNED
|
||||
@@ -192,15 +173,15 @@ void ir_init(void) {
|
||||
/** \brief Update \a ir_roll and ir_pitch from ADCs or from simulator
|
||||
* message in HITL and SITL modes
|
||||
*/
|
||||
void ir_update(void) {
|
||||
void infrared_update(void) {
|
||||
#if ! (defined SITL || defined HITL)
|
||||
ir_ir1 = (IR_IR1_SIGN)*((int32_t)(buf_ir1.sum/buf_ir1.av_nb_sample) - IR_ADC_IR1_NEUTRAL);
|
||||
ir_ir2 = (IR_IR2_SIGN)*((int32_t)(buf_ir2.sum/buf_ir2.av_nb_sample) - IR_ADC_IR2_NEUTRAL);
|
||||
ir_roll = ir_lateral_correction * IR_RollOfIrs(ir_ir1, ir_ir2);
|
||||
ir_pitch = ir_longitudinal_correction * IR_PitchOfIrs(ir_ir1, ir_ir2);
|
||||
infrared.ir1 = (IR_IR1_SIGN)*((int32_t)(buf_ir1.sum/buf_ir1.av_nb_sample) - IR_ADC_IR1_NEUTRAL);
|
||||
infrared.ir2 = (IR_IR2_SIGN)*((int32_t)(buf_ir2.sum/buf_ir2.av_nb_sample) - IR_ADC_IR2_NEUTRAL);
|
||||
infrared.roll = infrared.lateral_correction * IR_RollOfIrs(infrared.ir1, infrared.ir2);
|
||||
infrared.pitch = infrared.longitudinal_correction * IR_PitchOfIrs(infrared.ir1, infrared.ir2);
|
||||
#ifdef ADC_CHANNEL_IR_TOP
|
||||
const int16_t ir3 = (int32_t)(buf_ir_top.sum/buf_ir_top.av_nb_sample) - IR_ADC_TOP_NEUTRAL;
|
||||
ir_top = ir_vertical_correction * IR_TopOfIr(ir3);
|
||||
infrared.ir3 = (int32_t)(buf_ir_top.sum/buf_ir_top.av_nb_sample) - IR_ADC_TOP_NEUTRAL;
|
||||
infrared.top = infrared.vertical_correction * IR_TopOfIr(infrared.ir3);
|
||||
#endif // IR_TOP
|
||||
#endif /* !SITL && !HITL */
|
||||
/** #else ir_roll set by simulator in sim_ir.c */
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
/*
|
||||
* Paparazzi mcu0 $Id$
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
* Copyright (C) 2003-2010 The Paparazzi Team
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -28,26 +28,40 @@
|
||||
#include "std.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
extern float ir_roll_neutral; /* Rad */
|
||||
extern float ir_pitch_neutral; /* Rad */
|
||||
struct Infrared {
|
||||
/* the 3 channels of the sensor
|
||||
*/
|
||||
int16_t ir1;
|
||||
int16_t ir2;
|
||||
int16_t ir3;
|
||||
/* neutrals in radians
|
||||
*/
|
||||
float roll_neutral;
|
||||
float pitch_neutral;
|
||||
float pitch_vneutral;
|
||||
/* roll, pitch, yaw unscaled reading
|
||||
*/
|
||||
int16_t roll;
|
||||
int16_t pitch;
|
||||
int16_t top;
|
||||
/* coefficients used to compensate
|
||||
for sensors sensitivity
|
||||
*/
|
||||
float lateral_correction;
|
||||
float longitudinal_correction;
|
||||
float vertical_correction;
|
||||
/* coefficients used to compensate
|
||||
for masking
|
||||
*/
|
||||
float correction_left;
|
||||
float correction_right;
|
||||
float correction_up;
|
||||
float correction_down;
|
||||
};
|
||||
|
||||
extern int16_t ir_ir1; /* First horizontal channel */
|
||||
extern int16_t ir_ir2; /* Second horizontal channel */
|
||||
extern int16_t ir_roll; /* averaged roll adc */
|
||||
extern int16_t ir_pitch; /* averaged pitch adc */
|
||||
extern int16_t ir_top; /* averaged vertical ir adc */
|
||||
|
||||
extern float ir_correction_left;
|
||||
extern float ir_correction_right;
|
||||
extern float ir_correction_up;
|
||||
extern float ir_correction_down;
|
||||
|
||||
void ir_init(void);
|
||||
void ir_update(void);
|
||||
|
||||
extern float ir_lateral_correction;
|
||||
extern float ir_longitudinal_correction;
|
||||
extern float ir_vertical_correction;
|
||||
extern struct Infrared infrared;
|
||||
|
||||
void infrared_init(void);
|
||||
void infrared_update(void);
|
||||
|
||||
#endif /* SUBSYSTEMS_SENSORS_INFRARED_H */
|
||||
|
||||
Reference in New Issue
Block a user