Add wasm tacle-bench targets
This commit is contained in:
@ -0,0 +1,120 @@
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
|
||||
#include "std.h"
|
||||
#include "ad7714.h"
|
||||
#include "spi.h"
|
||||
|
||||
/* unipolar, 16 bits, current boost, filter 4000 */
|
||||
//#define AD_HIGH_FILTER_CFG 0xAF
|
||||
//#define AD_LOW_FILTER_CFG 0xA0
|
||||
|
||||
/* filter 2000 */
|
||||
#define AD_HIGH_FILTER_CFG 0xA7
|
||||
#define AD_LOW_FILTER_CFG 0xD0
|
||||
|
||||
/* filter 19 */
|
||||
//const uint8_t AD_HIGH_FILTER_CFG = 0xA0;
|
||||
//const uint8_t AD_LOW_FILTER_CFG = 0x13;
|
||||
|
||||
struct AdMsg {
|
||||
uint8_t len;
|
||||
const uint8_t *data;
|
||||
};
|
||||
|
||||
#define AD7714_SETUP_LEN 6
|
||||
const uint8_t ad7714_setup_data[ AD7714_SETUP_LEN ] = {
|
||||
AD_F_HIGH_REG + AD_FD0, /* select high filter register */
|
||||
AD_HIGH_FILTER_CFG,
|
||||
AD_F_LOW_REG + AD_FD0, /* select low filter register */
|
||||
AD_LOW_FILTER_CFG,
|
||||
AD_MODE_REG + AD_FD0, /* select com register */
|
||||
AD_SELFCAL_MOD + AD_GAIN_128,
|
||||
};
|
||||
const struct AdMsg ad7714_setup = {AD7714_SETUP_LEN, ad7714_setup_data};
|
||||
|
||||
#define AD7714_READ_LEN 3
|
||||
const uint8_t ad7714_read_data[ ] = {
|
||||
AD_DATA_REG + AD_WR + AD_FD0, /* transmit read request */
|
||||
0x00, /* transmit a dumb value just to get the result */
|
||||
0xAA /* transmit a dumb value just to get the result */
|
||||
};
|
||||
const struct AdMsg ad7714_read = {AD7714_READ_LEN, ad7714_read_data};
|
||||
|
||||
#define AD7714_SELECT_CHANNEL_LEN 2
|
||||
const uint8_t ad7714_select_channel_data[ AD7714_SELECT_CHANNEL_LEN ] = {
|
||||
AD_MODE_REG + AD_FD0,
|
||||
// AD_BG_CAL_MOD + AD_GAIN_128
|
||||
AD_NOR_MOD + AD_GAIN_128
|
||||
};
|
||||
const struct AdMsg ad7714_select_channel = {AD7714_SELECT_CHANNEL_LEN, ad7714_select_channel_data};
|
||||
|
||||
|
||||
static struct AdMsg *msg;
|
||||
static uint8_t idx;
|
||||
uint16_t ad7714_sample;
|
||||
uint8_t ad7714_sample_read;
|
||||
|
||||
|
||||
void ad7714_start_transmitting ( const struct AdMsg *amsg )
|
||||
{
|
||||
/* Enable SPI, Master, MSB first, clock idle high, sample on trailing edge, clock rate fck/128 */
|
||||
SPI_START( _BV( SPE ) | _BV( MSTR ) | _BV( SPR1 ) | _BV( CPOL ) | _BV(
|
||||
CPHA ) | _BV( SPR0 ) ); //| _BV(SPR0)
|
||||
SPI_SELECT_SLAVE1();
|
||||
msg = ( struct AdMsg * )amsg;
|
||||
SPDR = msg->data[ 0 ];
|
||||
idx = 0;
|
||||
}
|
||||
|
||||
void ad7714_on_spi_it( void )
|
||||
{
|
||||
uint8_t spi_read = SPDR;
|
||||
if ( msg == &ad7714_read ) {
|
||||
if ( idx == 1 )
|
||||
ad7714_sample = spi_read << 8;
|
||||
else
|
||||
if ( idx == 2 ) {
|
||||
ad7714_sample += spi_read;
|
||||
ad7714_sample_read = TRUE;
|
||||
}
|
||||
}
|
||||
idx++;
|
||||
if ( idx < msg->len )
|
||||
SPI_SEND( msg->data[ idx ] );
|
||||
else {
|
||||
SPI_UNSELECT_SLAVE1();
|
||||
SPI_STOP();
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t ad7714_status = 0;
|
||||
void ad7714_on_it( void )
|
||||
{
|
||||
if ( ad7714_status == 0 )
|
||||
ad7714_start_transmitting( &ad7714_setup );
|
||||
else
|
||||
if ( ad7714_status == 1 )
|
||||
ad7714_start_transmitting( &ad7714_select_channel );
|
||||
else
|
||||
ad7714_start_transmitting( &ad7714_read );
|
||||
ad7714_status++;
|
||||
}
|
||||
|
||||
void ad7714_init( void )
|
||||
{
|
||||
/* setupt interrupt on falling edge */
|
||||
cbi( EICRB, ISC60 );
|
||||
sbi( EICRB, ISC61 );
|
||||
/* clear interrupt flag */
|
||||
// if (bit_is_set(EIFR, INTF6))
|
||||
// EIFR != _BV(INTF6);
|
||||
Ad7714_Enable_It();
|
||||
}
|
||||
|
||||
|
||||
SIGNAL( SIG_INTERRUPT6 )
|
||||
{
|
||||
ad7714_on_it();
|
||||
}
|
||||
@ -0,0 +1,56 @@
|
||||
#ifndef AD_7714_H
|
||||
#define AD_7714_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
/* AD7714 COM register */
|
||||
#define AD_COM_REG (0<<4)
|
||||
#define AD_MODE_REG (1<<4)
|
||||
#define AD_F_HIGH_REG (2<<4)
|
||||
#define AD_F_LOW_REG (3<<4)
|
||||
#define AD_TEST_REG (4<<4)
|
||||
#define AD_DATA_REG (5<<4)
|
||||
#define AD_ZS_CAL_REG (6<<4)
|
||||
#define AD_FS_CAL_REG (7<<4)
|
||||
|
||||
#define AD_WR (1<<3)
|
||||
|
||||
#define AD_FD0 (4<<0)
|
||||
#define AD_FD1 (5<<0)
|
||||
#define AD_FD2 (6<<0)
|
||||
|
||||
/* AD7714 MODE register */
|
||||
#define AD_NOR_MOD (0<<5)
|
||||
#define AD_SELFCAL_MOD (1<<5)
|
||||
#define AD_ZS_SYSCAL_MOD (2<<5)
|
||||
#define AD_FS_SYSCAL_MOD (3<<5)
|
||||
#define AD_SYSOFFCAL_MOD (4<<5)
|
||||
#define AD_BG_CAL_MOD (5<<5)
|
||||
#define AD_ZS_SELFCAL_MOD (6<<5)
|
||||
#define AD_FS_SELFCAL_MOD (7<<5)
|
||||
|
||||
#define AD_GAIN_1 (0<<2)
|
||||
#define AD_GAIN_2 (1<<2)
|
||||
#define AD_GAIN_4 (2<<2)
|
||||
#define AD_GAIN_8 (3<<2)
|
||||
#define AD_GAIN_16 (4<<2)
|
||||
#define AD_GAIN_32 (5<<2)
|
||||
#define AD_GAIN_64 (6<<2)
|
||||
#define AD_GAIN_128 (7<<2)
|
||||
|
||||
#define AD_BO (1<<1)
|
||||
#define AD_FSYNC (1<<0)
|
||||
|
||||
#define Ad7714_Disable_It() { cbi(EIMSK, INT6); }
|
||||
|
||||
#define Ad7714_Enable_It() { sbi(EIMSK, INT6); }
|
||||
|
||||
#define Ad7714_Ready() (bit_is_clear(EIFR, INTF6))
|
||||
|
||||
extern uint16_t ad7714_sample;
|
||||
extern uint8_t ad7714_sample_read;
|
||||
void ad7714_init( void );
|
||||
void ad7714_on_spi_it( void );
|
||||
void ad7714_on_it( void );
|
||||
|
||||
#endif /* AD_7714_H */
|
||||
@ -0,0 +1,130 @@
|
||||
/*
|
||||
Paparazzi mcu0 adc functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
#include <arch/io.h>
|
||||
#include "airframe.h"
|
||||
#include "std.h"
|
||||
#include "adc.h"
|
||||
|
||||
|
||||
/*************************************************************************
|
||||
|
||||
Analog to digital conversion code.
|
||||
|
||||
We allow interrupts during the 2048 usec windows. If we run the
|
||||
ADC clock faster than Clk/64 we have too much overhead servicing
|
||||
the interrupts from it and end up with servo jitter.
|
||||
|
||||
For now we've slowed the clock to Clk/128 because it lets us
|
||||
be lazy in the interrupt routine.
|
||||
*/
|
||||
#define VOLTAGE_TIME 0x07
|
||||
#define ANALOG_PORT PORTF
|
||||
#define ANALOG_PORT_DIR DDRF
|
||||
|
||||
|
||||
#ifdef CTL_BRD_V1_1
|
||||
#define ANALOG_VREF 0
|
||||
#endif
|
||||
|
||||
#if defined CTL_BRD_V1_2 || defined CTL_BRD_V1_2_1
|
||||
#define ANALOG_VREF _BV(REFS0)
|
||||
#endif
|
||||
|
||||
uint16_t adc_samples[ NB_ADC ];
|
||||
|
||||
static struct adc_buf *buffers[ NB_ADC ];
|
||||
|
||||
void adc_buf_channel( uint8_t adc_channel, struct adc_buf *s )
|
||||
{
|
||||
buffers[ adc_channel ] = s;
|
||||
}
|
||||
|
||||
void
|
||||
adc_init( void )
|
||||
{
|
||||
uint8_t i;
|
||||
/* Ensure that our port is for input with no pull-ups */
|
||||
ANALOG_PORT = 0x00;
|
||||
ANALOG_PORT_DIR = 0x00;
|
||||
|
||||
/* Select our external voltage ref, which is tied to Vcc */
|
||||
ADMUX = ANALOG_VREF;
|
||||
|
||||
/* Turn off the analog comparator */
|
||||
sbi( ACSR, ACD );
|
||||
|
||||
/* Select out clock, turn on the ADC interrupt and start conversion */
|
||||
ADCSR = 0
|
||||
| VOLTAGE_TIME
|
||||
| ( 1 << ADEN )
|
||||
| ( 1 << ADIE )
|
||||
| ( 1 << ADSC );
|
||||
|
||||
/* Init to 0 (usefull ?) */
|
||||
_Pragma( "loopbound min 8 max 8" )
|
||||
for ( i = 0; i < NB_ADC; i++ )
|
||||
buffers[ i ] = ( struct adc_buf * )0;
|
||||
}
|
||||
|
||||
/**
|
||||
Called when the voltage conversion is finished
|
||||
|
||||
8.913kHz on mega128@16MHz 1kHz/channel ??
|
||||
*/
|
||||
|
||||
|
||||
SIGNAL( SIG_ADC )
|
||||
{
|
||||
uint8_t adc_input = ADMUX & 0x7;
|
||||
struct adc_buf *buf = buffers[ adc_input ];
|
||||
uint16_t adc_value = ADCW;
|
||||
/* Store result */
|
||||
adc_samples[ adc_input ] = adc_value;
|
||||
|
||||
if ( buf ) {
|
||||
uint8_t new_head = buf->head + 1;
|
||||
if ( new_head >= AV_NB_SAMPLE ) new_head = 0;
|
||||
buf->sum -= buf->values[ new_head ];
|
||||
buf->values[ new_head ] = adc_value;
|
||||
buf->sum += adc_value;
|
||||
buf->head = new_head;
|
||||
}
|
||||
|
||||
/* Find the next input */
|
||||
adc_input++;
|
||||
if ( adc_input >= 8 )
|
||||
adc_input = 0;
|
||||
/* Select it */
|
||||
ADMUX = adc_input | ANALOG_VREF;
|
||||
/* Restart the conversion */
|
||||
sbi( ADCSR, ADSC );
|
||||
|
||||
}
|
||||
@ -0,0 +1,53 @@
|
||||
/*
|
||||
Paparazzi mcu0 adc functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef _ADC_H_
|
||||
#define _ADC_H_
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
|
||||
#define NB_ADC 8
|
||||
|
||||
/* Array containing the last measured value */
|
||||
extern uint16_t adc_samples[ NB_ADC ];
|
||||
|
||||
void adc_init( void );
|
||||
|
||||
#define AV_NB_SAMPLE 0x20
|
||||
|
||||
struct adc_buf {
|
||||
uint16_t sum;
|
||||
uint16_t values[ AV_NB_SAMPLE ];
|
||||
uint8_t head;
|
||||
};
|
||||
|
||||
/* Facility to store last values in a circular buffer for a specific
|
||||
channel: allocate a (struct adc_buf) and register it with the following
|
||||
function */
|
||||
void adc_buf_channel( uint8_t adc_channel, struct adc_buf *s );
|
||||
#endif
|
||||
@ -0,0 +1,123 @@
|
||||
/*
|
||||
$Id: autopilot.h,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef AUTOPILOT_H
|
||||
#define AUTOPILOT_H
|
||||
|
||||
#include "link_autopilot.h"
|
||||
|
||||
#define TRESHOLD1 TRESHOLD_MANUAL_PPRZ
|
||||
#define TRESHOLD2 200 * CLOCK
|
||||
|
||||
|
||||
#define PPRZ_MODE_MANUAL 0
|
||||
#define PPRZ_MODE_AUTO1 1
|
||||
#define PPRZ_MODE_AUTO2 2
|
||||
#define PPRZ_MODE_HOME 3
|
||||
#define PPRZ_MODE_NB 4
|
||||
|
||||
#define PPRZ_MODE_OF_PULSE(pprz, mega8_status) \
|
||||
(pprz > TRESHOLD2 ? PPRZ_MODE_AUTO2 : \
|
||||
(pprz > TRESHOLD1 ? PPRZ_MODE_AUTO1 : PPRZ_MODE_MANUAL))
|
||||
|
||||
extern uint8_t pprz_mode;
|
||||
|
||||
|
||||
#define VERTICAL_MODE_MANUAL 0
|
||||
#define VERTICAL_MODE_AUTO_GAZ 1
|
||||
#define VERTICAL_MODE_AUTO_CLIMB 2
|
||||
#define VERTICAL_MODE_AUTO_ALT 3
|
||||
#define VERTICAL_MODE_NB 4
|
||||
|
||||
#define LATERAL_MODE_MANUAL 0
|
||||
#define LATERAL_MODE_ROLL_RATE 1
|
||||
#define LATERAL_MODE_ROLL 2
|
||||
#define LATERAL_MODE_COURSE 3
|
||||
#define LATERAL_MODE_NB 4
|
||||
|
||||
#define VERTICAL_MODE_OF_PULSE(pprz) (pprz < TRESHOLD2 ? VERTICAL_MODE_MANUAL: \
|
||||
VERTICAL_MODE_AUTO_ALT)
|
||||
|
||||
#define IR_ESTIM_MODE_OFF 0
|
||||
#define IR_ESTIM_MODE_ON 1
|
||||
|
||||
#define IR_ESTIM_MODE_OF_PULSE(pprz) (pprz < TRESHOLD2 ? IR_ESTIM_MODE_OFF: \
|
||||
IR_ESTIM_MODE_ON)
|
||||
|
||||
extern uint8_t ir_estim_mode;
|
||||
|
||||
#define STICK_PUSHED(pprz) (pprz < TRESHOLD1 || pprz > TRESHOLD2)
|
||||
|
||||
|
||||
#define TRIM_PPRZ(pprz) (pprz < MIN_PPRZ ? MIN_PPRZ : \
|
||||
(pprz > MAX_PPRZ ? MAX_PPRZ : \
|
||||
pprz))
|
||||
|
||||
#define TRIM_UPPRZ(pprz) (pprz < 0 ? 0 : \
|
||||
(pprz > MAX_PPRZ ? MAX_PPRZ : \
|
||||
pprz))
|
||||
|
||||
|
||||
#define FLOAT_OF_PPRZ(pprz, center, travel) ((float)pprz / (float)MAX_PPRZ * travel + center)
|
||||
|
||||
extern uint8_t fatal_error_nb;
|
||||
|
||||
#define GAZ_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9)
|
||||
|
||||
extern uint8_t inflight_calib_mode;
|
||||
//extern uint16_t flight_time;
|
||||
extern uint8_t vertical_mode;
|
||||
extern bool_t auto_pitch;
|
||||
extern uint8_t lateral_mode;
|
||||
extern uint8_t vsupply;
|
||||
|
||||
extern bool_t rc_event_1, rc_event_2;
|
||||
|
||||
extern float slider_1_val, slider_2_val;
|
||||
|
||||
extern bool_t launch;
|
||||
|
||||
|
||||
#define ModeUpdate(_mode, _value) { \
|
||||
uint8_t new_mode = _value; \
|
||||
if (_mode != new_mode) { _mode = new_mode; return TRUE; } \
|
||||
return FALSE; \
|
||||
}
|
||||
|
||||
#define CheckEvent(_event) (_event ? _event = FALSE, TRUE : FALSE)
|
||||
|
||||
#ifdef CTL_BRD_V1_1
|
||||
extern struct adc_buf buf_bat;
|
||||
#endif
|
||||
|
||||
void periodic_task( void );
|
||||
void use_gps_pos( void );
|
||||
void radio_control_task( void );
|
||||
/*receive_gps_data_task */
|
||||
void send_gps_pos( void );
|
||||
void send_radIR( void );
|
||||
void send_takeOff( void );
|
||||
/*end receive_gps_data_task*/
|
||||
void stabilisation_task( void );
|
||||
#endif /* AUTOPILOT_H */
|
||||
@ -0,0 +1,35 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: downlink.h,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef DOWNLINK_H
|
||||
#define DOWNLINK_H
|
||||
|
||||
#include "modem.h"
|
||||
|
||||
#define STX 0x05
|
||||
#define ETX 0x06
|
||||
|
||||
#include "messages.h"
|
||||
|
||||
#endif /* DOWNLINK_H */
|
||||
@ -0,0 +1,202 @@
|
||||
/*
|
||||
Paparazzi autopilot $Id: estimator.c,v 1.2 2011-01-21 11:52:44 moellmer Exp $
|
||||
|
||||
Copyright (C) 2004 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <inttypes.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "estimator.h"
|
||||
#include "gps.h"
|
||||
#include "pid.h"
|
||||
#include "infrared.h"
|
||||
#include "autopilot.h"
|
||||
|
||||
|
||||
/* position in meters */
|
||||
float estimator_x;
|
||||
float estimator_y;
|
||||
float estimator_z;
|
||||
|
||||
/* attitude in radian */
|
||||
float estimator_phi;
|
||||
float estimator_psi;
|
||||
float estimator_theta;
|
||||
|
||||
/* speed in meters per second */
|
||||
float estimator_x_dot;
|
||||
float estimator_y_dot;
|
||||
float estimator_z_dot;
|
||||
|
||||
/* rotational speed in radians per second */
|
||||
float estimator_phi_dot;
|
||||
float estimator_psi_dot;
|
||||
float estimator_theta_dot;
|
||||
|
||||
/* flight time in seconds */
|
||||
uint16_t estimator_flight_time;
|
||||
/* flight time in seconds */
|
||||
float estimator_t;
|
||||
|
||||
/* horizontal speed in module and dir */
|
||||
float estimator_hspeed_mod;
|
||||
float estimator_hspeed_dir;
|
||||
|
||||
float estimator_rad_of_ir, estimator_ir, estimator_rad;
|
||||
|
||||
#define EstimatorSetPos(x, y, z) { estimator_x = x; estimator_y = y; estimator_z = z; }
|
||||
#define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; }
|
||||
|
||||
|
||||
// FIXME maybe vz = -climb for NED??
|
||||
#define EstimatorSetSpeedCart(vx, vy, vz) { \
|
||||
estimator_vx = vx; \
|
||||
estimator_vy = vy; \
|
||||
estimator_vz = vz; \
|
||||
}
|
||||
// estimator_hspeed_mod = sqrt( estimator_vx * estimator_vx + estimator_vy * estimator_vy);
|
||||
// estimator_hspeed_dir = atan2(estimator_vy, estimator_vx);
|
||||
|
||||
|
||||
#define EstimatorSetSpeedPol(vhmod, vhdir, vz) { \
|
||||
estimator_hspeed_mod = vhmod; \
|
||||
estimator_hspeed_dir = vhdir; \
|
||||
estimator_z_dot = vz; \
|
||||
}
|
||||
//FIXME is this true ?? estimator_vx = estimator_hspeed_mod * cos(estimator_hspeed_dir);
|
||||
//FIXME is this true ?? estimator_vy = estimator_hspeed_mod * sin(estimator_hspeed_dir);
|
||||
|
||||
#define EstimatorSetRotSpeed(phi_dot, psi_dot, theta_dot) { \
|
||||
estimator_phi_dot = phi_dot; \
|
||||
estimator_psi_dot = psi_dot; \
|
||||
estimator_theta_dot = theta_dot; \
|
||||
}
|
||||
|
||||
inline void estimator_update_lls( void );
|
||||
|
||||
void estimator_init( void )
|
||||
{
|
||||
|
||||
EstimatorSetPos ( 0., 0., 0. );
|
||||
|
||||
EstimatorSetAtt ( 0., 0., 0 );
|
||||
|
||||
EstimatorSetSpeedPol ( 0., 0., 0. );
|
||||
|
||||
EstimatorSetRotSpeed ( 0., 0., 0. );
|
||||
|
||||
estimator_flight_time = 0;
|
||||
|
||||
estimator_rad_of_ir = ir_rad_of_ir;
|
||||
}
|
||||
|
||||
#define EstimatorIrGainIsCorrect() (TRUE)
|
||||
|
||||
void estimator_update_state_infrared( void )
|
||||
{
|
||||
float rad_of_ir = ( ir_estim_mode == IR_ESTIM_MODE_ON &&
|
||||
EstimatorIrGainIsCorrect() ) ?
|
||||
estimator_rad_of_ir : ir_rad_of_ir;
|
||||
estimator_phi = rad_of_ir * ir_roll;
|
||||
|
||||
estimator_theta = rad_of_ir * ir_pitch;
|
||||
}
|
||||
|
||||
#define INIT_WEIGHT 100. /* The number of times the initial value has to be taken */
|
||||
#define RHO 0.999 /* The higher, the slower the estimation is changing */
|
||||
|
||||
#define g 9.81
|
||||
|
||||
|
||||
void estimator_update_ir_estim( void )
|
||||
{
|
||||
static float last_hspeed_dir;
|
||||
static float last_t;
|
||||
static bool_t initialized = FALSE;
|
||||
static float sum_xy, sum_xx;
|
||||
float absphi;
|
||||
float init_ir2;
|
||||
|
||||
if ( initialized ) {
|
||||
float dt = gps_ftow - last_t;
|
||||
if ( dt > 0.1 ) { // Against division by zero
|
||||
float phi = ( estimator_hspeed_dir - last_hspeed_dir );
|
||||
//NORM_RAD_ANGLE(phi);
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( phi > M_PI ) phi -= 2 * M_PI;
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( phi < -M_PI ) phi += 2 * M_PI;
|
||||
phi = phi / dt * NOMINAL_AIRSPEED / g; /* tan linearized */
|
||||
//NORM_RAD_ANGLE(phi);
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( phi > M_PI ) phi -= 2 * M_PI;
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( phi < -M_PI ) phi += 2 * M_PI;
|
||||
estimator_ir = ( float )ir_roll;
|
||||
estimator_rad = phi;
|
||||
absphi = fabs( phi );
|
||||
if ( absphi < 1.0 && absphi > 0.05 && ( - ir_contrast / 2 < ir_roll &&
|
||||
ir_roll < ir_contrast / 2 ) ) {
|
||||
sum_xy = estimator_rad * estimator_ir + RHO * sum_xy;
|
||||
sum_xx = estimator_ir * estimator_ir + RHO * sum_xx;
|
||||
#if defined IR_RAD_OF_IR_MIN_VALUE & defined IR_RAD_OF_IR_MAX_VALUE
|
||||
float result = sum_xy / sum_xx;
|
||||
if ( result < IR_RAD_OF_IR_MIN_VALUE )
|
||||
estimator_rad_of_ir = IR_RAD_OF_IR_MIN_VALUE;
|
||||
else
|
||||
if ( result > IR_RAD_OF_IR_MAX_VALUE )
|
||||
estimator_rad_of_ir = IR_RAD_OF_IR_MAX_VALUE;
|
||||
else
|
||||
estimator_rad_of_ir = result;
|
||||
#else
|
||||
estimator_rad_of_ir = sum_xy / sum_xx;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
} else {
|
||||
initialized = TRUE;
|
||||
init_ir2 = ir_contrast;
|
||||
init_ir2 = init_ir2 * init_ir2;
|
||||
sum_xy = INIT_WEIGHT * estimator_rad_of_ir * init_ir2;
|
||||
sum_xx = INIT_WEIGHT * init_ir2;
|
||||
}
|
||||
|
||||
last_hspeed_dir = estimator_hspeed_dir;
|
||||
last_t = gps_ftow;
|
||||
}
|
||||
|
||||
|
||||
void estimator_update_state_gps( void )
|
||||
{
|
||||
if ( GPS_FIX_VALID( gps_mode ) ) {
|
||||
EstimatorSetPos( gps_east, gps_north, gps_falt );
|
||||
EstimatorSetSpeedPol( gps_fspeed, gps_fcourse, gps_fclimb );
|
||||
|
||||
if ( estimator_flight_time )
|
||||
estimator_update_ir_estim();
|
||||
}
|
||||
}
|
||||
|
||||
void estimator_propagate_state( void )
|
||||
{
|
||||
|
||||
}
|
||||
@ -0,0 +1,67 @@
|
||||
/*
|
||||
$Id: estimator.h,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
|
||||
Copyright (C) 2004 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef ESTIMATOR_H
|
||||
#define ESTIMATOR_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
/* position in meters */
|
||||
extern float estimator_x;
|
||||
extern float estimator_y;
|
||||
extern float estimator_z;
|
||||
|
||||
/* attitude in radians */
|
||||
extern float estimator_phi;
|
||||
extern float estimator_psi;
|
||||
extern float estimator_theta;
|
||||
|
||||
/* speed in meters per second */
|
||||
extern float estimator_x_dot;
|
||||
extern float estimator_y_dot;
|
||||
extern float estimator_z_dot;
|
||||
|
||||
/* rotational speed in radians per second */
|
||||
extern float estimator_phi_dot;
|
||||
extern float estimator_psi_dot;
|
||||
extern float estimator_teta_dot;
|
||||
|
||||
/* flight time in seconds */
|
||||
extern uint16_t estimator_flight_time;
|
||||
extern float estimator_t;
|
||||
|
||||
/* horizontal speed in module and dir (m/s, rad) */
|
||||
extern float estimator_hspeed_mod;
|
||||
extern float estimator_hspeed_dir;
|
||||
|
||||
void estimator_init( void );
|
||||
void estimator_update_state_infrared( void );
|
||||
void estimator_update_state_gps( void );
|
||||
void estimator_propagate_state( void );
|
||||
|
||||
extern float estimator_rad_of_ir, estimator_ir, estimator_rad;
|
||||
|
||||
|
||||
|
||||
#endif /* ESTIMATOR_H */
|
||||
@ -0,0 +1,58 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: gps.h,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
/*
|
||||
Parse SIRF protocol from ublox SAM module
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#ifndef GPS_H
|
||||
#define GPS_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
extern uint8_t gps_mode;
|
||||
extern float gps_ftow; /* ms */
|
||||
extern float gps_falt; /* m */
|
||||
extern float gps_fspeed; /* m/s */
|
||||
extern float gps_fclimb; /* m/s */
|
||||
extern float gps_fcourse; /* rad */
|
||||
extern int32_t gps_utm_east, gps_utm_north;
|
||||
extern float gps_east, gps_north; /* m */
|
||||
|
||||
void gps_init( void );
|
||||
void parse_gps_msg( void );
|
||||
extern volatile uint8_t gps_msg_received;
|
||||
extern bool_t gps_pos_available;
|
||||
extern uint8_t gps_nb_ovrn;
|
||||
|
||||
#ifdef UBX
|
||||
#include "ubx.h"
|
||||
#else
|
||||
#include "sirf.h"
|
||||
#endif
|
||||
|
||||
|
||||
#endif /* GPS_H */
|
||||
@ -0,0 +1,318 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: gps_sirf.c,v 1.2 2011-01-25 09:40:36 plazar Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <inttypes.h>
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
#include <string.h>
|
||||
#include "math_papabench.h"
|
||||
|
||||
#include "uart.h"
|
||||
#include "gps.h"
|
||||
|
||||
|
||||
float gps_falt;
|
||||
float gps_fspeed;
|
||||
float gps_fclimb;
|
||||
float gps_fcourse;
|
||||
uint8_t gps_mode;
|
||||
volatile bool_t gps_msg_received;
|
||||
bool_t gps_pos_available;
|
||||
|
||||
|
||||
#define SIRF_MAX_PAYLOAD 255
|
||||
uint8_t sirf_msg_buf[ SIRF_MAX_PAYLOAD ];
|
||||
|
||||
#define READ_INT32_AT_OFFSET(offset, dest) \
|
||||
{ \
|
||||
dest[ 0 ] = sirf_msg_buf[ offset+3 ]; \
|
||||
dest[ 1 ] = sirf_msg_buf[ offset+2 ]; \
|
||||
dest[ 2 ] = sirf_msg_buf[ offset+1 ]; \
|
||||
dest[ 3 ] = sirf_msg_buf[ offset ]; \
|
||||
} \
|
||||
/* ext nav type = 0x62
|
||||
offset len
|
||||
type 0 1
|
||||
lat 1 4
|
||||
lon 5 4
|
||||
alt 9 4
|
||||
speed 13 4
|
||||
climb 17 4
|
||||
course 21 4
|
||||
mode 25 1
|
||||
*/
|
||||
void parse_gps_msg( void )
|
||||
{
|
||||
static int32_t tmp_int32;
|
||||
uint8_t *tmp = ( uint8_t * )&tmp_int32;
|
||||
|
||||
READ_INT32_AT_OFFSET( 1, tmp );
|
||||
gps_lat = tmp_int32;
|
||||
|
||||
READ_INT32_AT_OFFSET( 5, tmp );
|
||||
gps_lon = tmp_int32;
|
||||
|
||||
READ_INT32_AT_OFFSET( 9, tmp );
|
||||
gps_falt = ( float )tmp_int32 / 1e3;
|
||||
|
||||
READ_INT32_AT_OFFSET( 13, tmp );
|
||||
gps_fspeed = ( float )tmp_int32 / 1e3;
|
||||
|
||||
READ_INT32_AT_OFFSET( 17, tmp );
|
||||
gps_fclimb = ( float )tmp_int32 / 1e3;
|
||||
|
||||
READ_INT32_AT_OFFSET( 21, tmp );
|
||||
gps_fcourse = ( float )tmp_int32 / 1e8;
|
||||
|
||||
gps_mode = sirf_msg_buf[ 25 ];
|
||||
|
||||
gps_pos_available = TRUE;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void gps_init( void )
|
||||
{
|
||||
/* Enable uart */
|
||||
#ifdef SIMUL
|
||||
uart0_init();
|
||||
#else
|
||||
uart1_init();
|
||||
#endif
|
||||
}
|
||||
|
||||
#define SIRF_START1 0xA0
|
||||
#define SIRF_START2 0xA2
|
||||
#define SIRF_END1 0xB0
|
||||
#define SIRF_END2 0xB3
|
||||
|
||||
#ifdef SIMUL
|
||||
#define IR_START 0xA1 /* simulator/mc.ml */
|
||||
volatile int16_t simul_ir_roll;
|
||||
volatile int16_t simul_ir_pitch;
|
||||
#endif
|
||||
|
||||
#define SIRF_TYP_NAV 0x02
|
||||
#define SIRF_TYP_EXT_NAV 0x62
|
||||
|
||||
#define UNINIT 0
|
||||
#define GOT_START1 1
|
||||
#define GOT_START2 2
|
||||
#define GOT_LEN1 3
|
||||
#define GOT_LEN2 4
|
||||
#define GOT_PAYLOAD 5
|
||||
#define GOT_CHECKSUM1 6
|
||||
#define GOT_CHECKSUM2 7
|
||||
#define GOT_END1 8
|
||||
#ifdef SIMUL
|
||||
#define GOT_IR_START 9
|
||||
#define GOT_IR1 10
|
||||
#define GOT_IR2 11
|
||||
#define GOT_IR3 12
|
||||
#endif
|
||||
|
||||
static uint8_t sirf_status;
|
||||
static uint16_t sirf_len;
|
||||
static uint16_t sirf_checksum;
|
||||
static uint8_t sirf_type;
|
||||
static uint8_t sirf_msg_idx;
|
||||
|
||||
|
||||
static inline void parse_sirf( uint8_t c )
|
||||
{
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch (sirf_status) {
|
||||
case UNINIT:
|
||||
if (c == SIRF_START1)
|
||||
sirf_status++;
|
||||
#ifdef SIMUL
|
||||
if (c == IR_START)
|
||||
sirf_status = GOT_IR_START;
|
||||
#endif
|
||||
break;
|
||||
case GOT_START1:
|
||||
if (c != SIRF_START2)
|
||||
goto error;
|
||||
sirf_status++;
|
||||
break;
|
||||
case GOT_START2:
|
||||
sirf_len = (c<<8) & 0xFF00;
|
||||
sirf_status++;
|
||||
break;
|
||||
case GOT_LEN1:
|
||||
sirf_len += (c & 0x00FF);
|
||||
if (sirf_len > SIRF_MAX_PAYLOAD)
|
||||
goto error;
|
||||
sirf_msg_idx = 0;
|
||||
sirf_status++;
|
||||
break;
|
||||
case GOT_LEN2:
|
||||
if (sirf_msg_idx==0) {
|
||||
sirf_type = c;
|
||||
}
|
||||
if (sirf_type == SIRF_TYP_EXT_NAV)
|
||||
sirf_msg_buf[ sirf_msg_idx ] = c;
|
||||
sirf_msg_idx++;
|
||||
if (sirf_msg_idx >= sirf_len) {
|
||||
sirf_status++;
|
||||
}
|
||||
break;
|
||||
case GOT_PAYLOAD:
|
||||
sirf_checksum = (c<<8) & 0xFF00;
|
||||
sirf_status++;
|
||||
break;
|
||||
case GOT_CHECKSUM1:
|
||||
sirf_checksum += (c & 0x00FF);
|
||||
// fixme: check correct
|
||||
sirf_status++;
|
||||
break;
|
||||
case GOT_CHECKSUM2:
|
||||
if (c != SIRF_END1)
|
||||
goto error;
|
||||
sirf_status++;
|
||||
break;
|
||||
case GOT_END1:
|
||||
if (c != SIRF_END2)
|
||||
goto error;
|
||||
|
||||
if (sirf_type == SIRF_TYP_EXT_NAV)
|
||||
gps_msg_received = TRUE;
|
||||
goto restart;
|
||||
break;
|
||||
#ifdef SIMUL
|
||||
case GOT_IR_START:
|
||||
simul_ir_roll = c << 8;
|
||||
sirf_status++;
|
||||
break;
|
||||
case GOT_IR1:
|
||||
simul_ir_roll |= c;
|
||||
sirf_status++;
|
||||
break;
|
||||
case GOT_IR2:
|
||||
simul_ir_pitch = c << 8;
|
||||
sirf_status++;
|
||||
break;
|
||||
case GOT_IR3:
|
||||
simul_ir_pitch |= c;
|
||||
goto restart;
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
#else*/
|
||||
if ( sirf_status == UNINIT ) {
|
||||
if ( c == SIRF_START1 )
|
||||
sirf_status++;
|
||||
#ifdef SIMUL
|
||||
if ( c == IR_START )
|
||||
sirf_status = GOT_IR_START;
|
||||
#endif
|
||||
} else
|
||||
if ( sirf_status == GOT_START1 ) {
|
||||
if ( c != SIRF_START2 )
|
||||
goto error;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_START2 ) {
|
||||
sirf_len = ( c << 8 ) & 0xFF00;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_LEN1 ) {
|
||||
sirf_len += ( c & 0x00FF );
|
||||
if ( sirf_len > SIRF_MAX_PAYLOAD )
|
||||
goto error;
|
||||
sirf_msg_idx = 0;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_LEN2 ) {
|
||||
if ( sirf_msg_idx == 0 )
|
||||
sirf_type = c;
|
||||
if ( sirf_type == SIRF_TYP_EXT_NAV )
|
||||
sirf_msg_buf[ sirf_msg_idx ] = c;
|
||||
sirf_msg_idx++;
|
||||
if ( sirf_msg_idx >= sirf_len )
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_PAYLOAD ) {
|
||||
sirf_checksum = ( c << 8 ) & 0xFF00;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_CHECKSUM1 ) {
|
||||
sirf_checksum += ( c & 0x00FF );
|
||||
/* fixme: check correct */
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_CHECKSUM2 ) {
|
||||
if ( c != SIRF_END1 )
|
||||
goto error;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_END1 ) {
|
||||
if ( c != SIRF_END2 )
|
||||
goto error;
|
||||
|
||||
if ( sirf_type == SIRF_TYP_EXT_NAV )
|
||||
gps_msg_received = TRUE;
|
||||
goto restart;
|
||||
}
|
||||
#ifdef SIMUL
|
||||
else
|
||||
if ( sirf_status == GOT_IR_START ) {
|
||||
simul_ir_roll = c << 8;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_IR1 ) {
|
||||
simul_ir_roll |= c;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_IR2 ) {
|
||||
simul_ir_pitch = c << 8;
|
||||
sirf_status++;
|
||||
} else
|
||||
if ( sirf_status == GOT_IR3 ) {
|
||||
simul_ir_pitch |= c;
|
||||
goto restart;
|
||||
}
|
||||
#endif
|
||||
else {}
|
||||
//#endif
|
||||
return;
|
||||
error:
|
||||
// modem_putc('r');
|
||||
restart:
|
||||
// modem_putc('\n');
|
||||
sirf_status = UNINIT;
|
||||
sirf_checksum = 0;
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
#ifdef SIMUL
|
||||
ReceiveUart0( parse_sirf );
|
||||
#else
|
||||
ReceiveUart1( parse_sirf );
|
||||
#endif
|
||||
@ -0,0 +1,265 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: gps_ubx.c,v 1.4 2011-01-25 09:40:36 plazar Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <inttypes.h>
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
//#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "flight_plan.h"
|
||||
#include "uart.h"
|
||||
#include "gps.h"
|
||||
#include "ubx_protocol.h"
|
||||
#include "flight_plan.h"
|
||||
|
||||
float gps_ftow;
|
||||
float gps_falt;
|
||||
float gps_fspeed;
|
||||
float gps_fclimb;
|
||||
float gps_fcourse;
|
||||
int32_t gps_utm_east, gps_utm_north;
|
||||
float gps_east, gps_north;
|
||||
uint8_t gps_mode;
|
||||
volatile bool_t gps_msg_received;
|
||||
bool_t gps_pos_available;
|
||||
const int32_t utm_east0 = NAV_UTM_EAST0;
|
||||
const int32_t utm_north0 = NAV_UTM_NORTH0;
|
||||
|
||||
#define UBX_MAX_PAYLOAD 255
|
||||
static uint8_t ubx_msg_buf[ UBX_MAX_PAYLOAD ];
|
||||
|
||||
#define RadianOfDeg(d) ((d)/180.*3.1415927)
|
||||
|
||||
#ifdef SIMUL
|
||||
#include "infrared.h"
|
||||
#define IR_START 0xA1 /* simulator/mc.ml */
|
||||
volatile int16_t simul_ir_roll;
|
||||
volatile int16_t simul_ir_pitch;
|
||||
#endif
|
||||
|
||||
#define UNINIT 0
|
||||
#define GOT_SYNC1 1
|
||||
#define GOT_SYNC2 2
|
||||
#define GOT_CLASS 3
|
||||
#define GOT_ID 4
|
||||
#define GOT_LEN1 5
|
||||
#define GOT_LEN2 6
|
||||
#define GOT_PAYLOAD 7
|
||||
#define GOT_CHECKSUM1 8
|
||||
#ifdef SIMUL
|
||||
#define GOT_IR_START 20
|
||||
#define GOT_IR1 21
|
||||
#define GOT_IR2 22
|
||||
#define GOT_IR3 23
|
||||
#endif
|
||||
|
||||
static uint8_t ubx_status;
|
||||
static uint16_t ubx_len;
|
||||
static uint8_t ubx_msg_idx;
|
||||
// was static
|
||||
uint8_t ck_a, ck_b, ubx_id, ubx_class;
|
||||
|
||||
void gps_init( void )
|
||||
{
|
||||
/* Enable uart */
|
||||
#ifdef SIMUL
|
||||
uart0_init();
|
||||
simul_ir_roll = ir_roll_neutral;
|
||||
simul_ir_pitch = ir_pitch_neutral;
|
||||
#else
|
||||
uart1_init();
|
||||
#endif
|
||||
ubx_status = UNINIT;
|
||||
}
|
||||
|
||||
void parse_gps_msg( void )
|
||||
{
|
||||
if ( ubx_class == UBX_NAV_ID ) {
|
||||
if ( ubx_id == UBX_NAV_POSUTM_ID ) {
|
||||
gps_utm_east = UBX_NAV_POSUTM_EAST( ubx_msg_buf );
|
||||
gps_utm_north = UBX_NAV_POSUTM_NORTH( ubx_msg_buf );
|
||||
gps_falt = ( float )UBX_NAV_POSUTM_ALT( ubx_msg_buf ) / 100.;
|
||||
} else
|
||||
if ( ubx_id == UBX_NAV_STATUS_ID )
|
||||
gps_mode = UBX_NAV_STATUS_GPSfix( ubx_msg_buf );
|
||||
|
||||
else
|
||||
if ( ubx_id == UBX_NAV_VELNED_ID ) {
|
||||
gps_fspeed = ( ( float )UBX_NAV_VELNED_GSpeed( ubx_msg_buf ) ) / 1e2;
|
||||
gps_fclimb = ( ( float )UBX_NAV_VELNED_VEL_D( ubx_msg_buf ) ) / -1e2;
|
||||
gps_fcourse = RadianOfDeg( ( ( float )UBX_NAV_VELNED_Heading(
|
||||
ubx_msg_buf ) ) / 1e5 );
|
||||
gps_ftow = ( ( float )UBX_NAV_VELNED_ITOW( ubx_msg_buf ) ) / 1e3;
|
||||
|
||||
gps_east = gps_utm_east / 100 - NAV_UTM_EAST0;
|
||||
gps_north = gps_utm_north / 100 - NAV_UTM_NORTH0;
|
||||
|
||||
|
||||
gps_pos_available = TRUE; /* The 3 UBX messages are sent in one rafale */
|
||||
}
|
||||
}
|
||||
#ifdef SIMUL
|
||||
if ( ubx_class == UBX_USR_ID ) {
|
||||
if ( ubx_id == UBX_USR_IRSIM_ID ) {
|
||||
simul_ir_roll = UBX_USR_IRSIM_ROLL( ubx_msg_buf );
|
||||
simul_ir_pitch = UBX_USR_IRSIM_PITCH( ubx_msg_buf );
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
uint8_t gps_nb_ovrn;
|
||||
|
||||
|
||||
static void parse_ubx( uint8_t c )
|
||||
{
|
||||
if ( ubx_status < GOT_PAYLOAD ) {
|
||||
ck_a += c;
|
||||
ck_b += ck_a;
|
||||
}
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch (ubx_status) {
|
||||
case UNINIT:
|
||||
if (c == UBX_SYNC1)
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_SYNC1:
|
||||
if (c != UBX_SYNC2)
|
||||
goto error;
|
||||
ck_a = 0;
|
||||
ck_b = 0;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_SYNC2:
|
||||
if (gps_msg_received) {
|
||||
// Previous message has not yet been parsed: discard this one
|
||||
gps_nb_ovrn++;
|
||||
goto error;
|
||||
}
|
||||
ubx_class = c;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_CLASS:
|
||||
ubx_id = c;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_ID:
|
||||
ubx_len = c;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_LEN1:
|
||||
ubx_len |= (c<<8);
|
||||
if (ubx_len > UBX_MAX_PAYLOAD)
|
||||
goto error;
|
||||
ubx_msg_idx = 0;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_LEN2:
|
||||
ubx_msg_buf[ ubx_msg_idx ] = c;
|
||||
ubx_msg_idx++;
|
||||
if (ubx_msg_idx >= ubx_len) {
|
||||
ubx_status++;
|
||||
}
|
||||
break;
|
||||
case GOT_PAYLOAD:
|
||||
if (c != ck_a)
|
||||
goto error;
|
||||
ubx_status++;
|
||||
break;
|
||||
case GOT_CHECKSUM1:
|
||||
if (c != ck_b)
|
||||
goto error;
|
||||
gps_msg_received = TRUE;
|
||||
goto restart;
|
||||
break;
|
||||
}
|
||||
#else */
|
||||
if ( ubx_status == UNINIT ) {
|
||||
if ( c == UBX_SYNC1 )
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_SYNC1 ) {
|
||||
if ( c != UBX_SYNC2 )
|
||||
goto error;
|
||||
ck_a = 0;
|
||||
ck_b = 0;
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_SYNC2 ) {
|
||||
if ( gps_msg_received ) {
|
||||
/* Previous message has not yet been parsed: discard this one */
|
||||
gps_nb_ovrn++;
|
||||
goto error;
|
||||
}
|
||||
ubx_class = c;
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_CLASS ) {
|
||||
ubx_id = c;
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_ID ) {
|
||||
ubx_len = c;
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_LEN1 ) {
|
||||
ubx_len |= ( c << 8 );
|
||||
if ( ubx_len > UBX_MAX_PAYLOAD )
|
||||
goto error;
|
||||
ubx_msg_idx = 0;
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_LEN2 ) {
|
||||
ubx_msg_buf[ ubx_msg_idx ] = c;
|
||||
ubx_msg_idx++;
|
||||
if ( ubx_msg_idx >= ubx_len )
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_PAYLOAD ) {
|
||||
if ( c != ck_a )
|
||||
goto error;
|
||||
ubx_status++;
|
||||
} else
|
||||
if ( ubx_status == GOT_CHECKSUM1 ) {
|
||||
if ( c != ck_b )
|
||||
goto error;
|
||||
gps_msg_received = TRUE;
|
||||
goto restart;
|
||||
} else {}
|
||||
//#endif
|
||||
return;
|
||||
error:
|
||||
restart:
|
||||
ubx_status = UNINIT;
|
||||
return;
|
||||
}
|
||||
|
||||
#ifdef SIMUL
|
||||
ReceiveUart0( parse_ubx )
|
||||
#else
|
||||
ReceiveUart1( parse_ubx )
|
||||
#endif
|
||||
@ -0,0 +1,94 @@
|
||||
/*
|
||||
$Id: if_calib.c,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
Flight-time calibration facility
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <inttypes.h>
|
||||
#include "radio.h"
|
||||
#include "autopilot.h"
|
||||
#include "if_calib.h"
|
||||
#include "infrared.h"
|
||||
#include "pid.h"
|
||||
#include "nav.h"
|
||||
|
||||
|
||||
#define ParamValInt16(param_init_val, param_travel, cur_pulse, init_pulse) \
|
||||
(param_init_val + (int16_t)(((float)(cur_pulse - init_pulse)) * param_travel / (float)MAX_PPRZ))
|
||||
|
||||
#define ParamValFloat(param_init_val, param_travel, cur_pulse, init_pulse) \
|
||||
(param_init_val + ((float)(cur_pulse - init_pulse)) * param_travel / (float)MAX_PPRZ)
|
||||
|
||||
|
||||
|
||||
uint8_t inflight_calib_mode = IF_CALIB_MODE_NONE;
|
||||
|
||||
static int16_t slider1_init, slider2_init;
|
||||
|
||||
#include "inflight_calib.h"
|
||||
|
||||
|
||||
/***
|
||||
inline uint8_t inflight_calib(void) {
|
||||
static int16_t slider1_init, slider2_init;
|
||||
//static float ir_gain_init;
|
||||
//static float roll_pgain_init;
|
||||
static float course_pgain_init;
|
||||
static int16_t roll_neutral_init;
|
||||
static float pitch_pgain_init;
|
||||
static int16_t pitch_neutral_init;
|
||||
|
||||
int8_t mode_changed = inflight_calib_mode_update();
|
||||
|
||||
if (inflight_calib_mode == IF_CALIB_MODE_NEUTRAL) {
|
||||
if (mode_changed) {
|
||||
pitch_neutral_init = ir_pitch_neutral;
|
||||
roll_neutral_init = ir_roll_neutral;
|
||||
slider1_init = from_fbw.channels[ RADIO_GAIN1 ];
|
||||
slider2_init = from_fbw.channels[ RADIO_GAIN2 ];
|
||||
}
|
||||
ir_pitch_neutral = PARAM_VAL_INT16( pitch_neutral_init, -60., from_fbw.channels[ RADIO_GAIN1 ], slider1_init);
|
||||
ir_roll_neutral = PARAM_VAL_INT16( roll_neutral_init, 60., from_fbw.channels[ RADIO_GAIN2 ], slider2_init);
|
||||
}
|
||||
else if (inflight_calib_mode == IF_CALIB_MODE_GAIN) {
|
||||
if (mode_changed) {
|
||||
// ir_gain_init = ir_gain;
|
||||
course_pgain_init = course_pgain;
|
||||
// roll_pgain_init = roll_pgain;
|
||||
pitch_pgain_init = pitch_pgain;
|
||||
slider1_init = from_fbw.channels[ RADIO_GAIN1 ];
|
||||
slider2_init = from_fbw.channels[ RADIO_GAIN2 ];
|
||||
}
|
||||
course_pgain = PARAM_VAL_FLOAT( course_pgain_init, -0.1, from_fbw.channels[ RADIO_GAIN1 ], slider1_init);
|
||||
// ir_gain = PARAM_VAL_FLOAT( ir_gain_init, 0.0015, from_fbw.channels[ RADIO_GAIN2 ], slider2_init);
|
||||
// roll_pgain = PARAM_VAL_FLOAT( roll_pgain_init, -5000., from_fbw.channels[ RADIO_GAIN2 ], slider1_init);
|
||||
pitch_pgain = PARAM_VAL_FLOAT( pitch_pgain_init, -5000., from_fbw.channels[ RADIO_GAIN1 ], slider1_init);
|
||||
}
|
||||
return (mode_changed);
|
||||
}
|
||||
***/
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,22 @@
|
||||
#ifndef IF_CALIB_H
|
||||
|
||||
#include "link_fbw.h"
|
||||
|
||||
extern uint8_t inflight_calib_mode;
|
||||
void inflight_calib( bool_t calib_mode_changed );
|
||||
|
||||
|
||||
#define IF_CALIB_MODE_NONE 0
|
||||
#define IF_CALIB_MODE_DOWN 1
|
||||
#define IF_CALIB_MODE_UP 2
|
||||
|
||||
#ifdef ANTON_T7
|
||||
#define IF_CALIB_MODE_OF_PULSE(pprz) (pprz < TRESHOLD1 ? IF_CALIB_MODE_UP : \
|
||||
IF_CALIB_MODE_NONE)
|
||||
#else
|
||||
#define IF_CALIB_MODE_OF_PULSE(pprz) (pprz < TRESHOLD1 ? IF_CALIB_MODE_UP : \
|
||||
(pprz < TRESHOLD2 ? IF_CALIB_MODE_NONE : \
|
||||
IF_CALIB_MODE_DOWN))
|
||||
#endif /* ANTON_T7 */
|
||||
|
||||
#endif // IF_CALIB_H
|
||||
@ -0,0 +1,76 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: infrared.c,v 1.2 2011-01-18 14:55:52 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 "adc.h"
|
||||
#include "infrared.h"
|
||||
#include "autopilot.h"
|
||||
#include "estimator.h"
|
||||
|
||||
int16_t ir_roll;
|
||||
int16_t ir_pitch;
|
||||
|
||||
int16_t ir_contrast = IR_DEFAULT_CONTRAST;
|
||||
int16_t ir_roll_neutral = IR_ROLL_NEUTRAL_DEFAULT;
|
||||
int16_t ir_pitch_neutral = IR_PITCH_NEUTRAL_DEFAULT;
|
||||
|
||||
#define RadOfIrFromConstrast(c) ir_rad_of_ir = IR_RAD_OF_IR_CONTRAST / c;
|
||||
|
||||
//float ir_rad_of_ir = IR_RAD_OF_IR_CONTRAST / IR_DEFAULT_CONTRAST;
|
||||
float ir_rad_of_ir = 0.00375;
|
||||
|
||||
|
||||
static struct adc_buf buf_ir1;
|
||||
static struct adc_buf buf_ir2;
|
||||
|
||||
void ir_init( void )
|
||||
{
|
||||
RadOfIrFromConstrast( IR_DEFAULT_CONTRAST );
|
||||
adc_buf_channel( ADC_CHANNEL_IR1, &buf_ir1 );
|
||||
adc_buf_channel( ADC_CHANNEL_IR2, &buf_ir2 );
|
||||
}
|
||||
|
||||
void ir_update( void )
|
||||
{
|
||||
#ifndef SIMUL
|
||||
int16_t x1_mean = buf_ir1.sum / AV_NB_SAMPLE;
|
||||
int16_t x2_mean = buf_ir2.sum / AV_NB_SAMPLE;
|
||||
ir_roll = IR_RollOfIrs( x1_mean, x2_mean ) - ir_roll_neutral;
|
||||
ir_pitch = IR_PitchOfIrs( x1_mean, x2_mean ) - ir_pitch_neutral;
|
||||
#else
|
||||
extern volatile int16_t simul_ir_roll, simul_ir_pitch;
|
||||
ir_roll = simul_ir_roll - ir_roll_neutral;
|
||||
ir_pitch = simul_ir_pitch - ir_pitch_neutral;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
Contrast measurement
|
||||
*/
|
||||
|
||||
void ir_gain_calib( void ) // Plane nose down
|
||||
{
|
||||
/* plane nose down -> negativ value */
|
||||
ir_contrast = - ir_pitch;
|
||||
RadOfIrFromConstrast( ir_contrast );
|
||||
}
|
||||
@ -0,0 +1,42 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: infrared.h,v 1.2 2011-01-18 14:55:52 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef INFRARED_H
|
||||
#define INFRARED_H
|
||||
|
||||
|
||||
extern int16_t ir_roll; /* averaged roll adc */
|
||||
extern int16_t ir_pitch; /* averaged pitch adc */
|
||||
|
||||
|
||||
extern float ir_rad_of_ir;
|
||||
extern int16_t ir_contrast;
|
||||
extern int16_t ir_roll_neutral;
|
||||
extern int16_t ir_pitch_neutral;
|
||||
|
||||
void ir_init( void );
|
||||
void ir_update( void );
|
||||
void ir_gain_calib( void );
|
||||
|
||||
#endif /* INFRARED_H */
|
||||
@ -0,0 +1,125 @@
|
||||
/*
|
||||
$Id: link_fbw.c,v 1.3 2011-01-25 09:40:37 plazar Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
|
||||
#include "link_fbw.h"
|
||||
#include "spi.h"
|
||||
|
||||
struct inter_mcu_msg from_fbw;
|
||||
struct inter_mcu_msg to_fbw;
|
||||
volatile uint8_t link_fbw_receive_complete = TRUE;
|
||||
volatile uint8_t link_fbw_receive_valid = FALSE;
|
||||
volatile uint8_t link_fbw_nb_err;
|
||||
uint8_t link_fbw_fbw_nb_err;
|
||||
|
||||
static uint8_t idx_buf;
|
||||
static uint8_t xor_in, xor_out;
|
||||
|
||||
void link_fbw_init( void )
|
||||
{
|
||||
link_fbw_nb_err;
|
||||
link_fbw_receive_complete = FALSE;
|
||||
}
|
||||
|
||||
void _Pragma( "entrypoint" ) link_fbw_send( void )
|
||||
{
|
||||
if ( spi_cur_slave != SPI_NONE ) {
|
||||
spi_nb_ovrn++;
|
||||
return;
|
||||
}
|
||||
|
||||
/* Enable SPI, Master, set clock rate fck/16 */
|
||||
SPI_START( _BV( SPE ) | _BV( MSTR ) | _BV( SPR0 ) ); // | _BV(SPR1);
|
||||
SPI_SELECT_SLAVE0();
|
||||
|
||||
idx_buf = 0;
|
||||
xor_in = 0;
|
||||
xor_out = ( ( uint8_t * )&to_fbw )[ idx_buf ];
|
||||
SPDR = xor_out;
|
||||
link_fbw_receive_valid = FALSE;
|
||||
// Other bytes will follow SIG_SPI interrupts
|
||||
}
|
||||
|
||||
void link_fbw_on_spi_it( void )
|
||||
{
|
||||
/* setup OCR1A to pop in 200 clock cycles */
|
||||
/* this leaves time for the slave (fbw) */
|
||||
/* to process the byte we've sent and to */
|
||||
/* prepare a new one to be sent */
|
||||
OCR1A = TCNT1 + 200;
|
||||
/* clear interrupt flag */
|
||||
sbi( TIFR, OCF1A );
|
||||
/* enable OC1A interrupt */
|
||||
sbi( TIMSK, OCIE1A );
|
||||
}
|
||||
|
||||
|
||||
/* send the next byte */
|
||||
SIGNAL( SIG_OUTPUT_COMPARE1A )
|
||||
{
|
||||
uint8_t tmp;
|
||||
|
||||
/* disable OC1A interrupt */
|
||||
cbi( TIMSK, OCIE1A );
|
||||
|
||||
idx_buf++;
|
||||
|
||||
/* we have sent/received a complete frame */
|
||||
if ( idx_buf == FRAME_LENGTH ) {
|
||||
/* read checksum from receive register */
|
||||
tmp = SPDR;
|
||||
/* notify valid frame */
|
||||
if ( tmp == xor_in ) {
|
||||
link_fbw_receive_valid = TRUE;
|
||||
link_fbw_fbw_nb_err = from_fbw.nb_err;
|
||||
} else
|
||||
link_fbw_nb_err++;
|
||||
link_fbw_receive_complete = TRUE;
|
||||
/* unselect slave0 */
|
||||
SPI_UNSELECT_SLAVE0();
|
||||
SPI_STOP();
|
||||
return;
|
||||
}
|
||||
|
||||
/* we are sending/receiving payload */
|
||||
if ( idx_buf < FRAME_LENGTH - 1 ) {
|
||||
/* place new payload byte in send register */
|
||||
tmp = ( ( uint8_t * )&to_fbw )[ idx_buf ];
|
||||
SPI_SEND( tmp );
|
||||
xor_out ^= tmp;
|
||||
}
|
||||
/* we are done sending the payload */
|
||||
else { // idx_buf == FRAME_LENGTH - 1
|
||||
/* place checksum in send register */
|
||||
SPI_SEND( xor_out );
|
||||
}
|
||||
|
||||
/* read the byte from receive register */
|
||||
tmp = SPDR;
|
||||
( ( uint8_t * )&from_fbw )[ idx_buf - 1 ] = tmp;
|
||||
xor_in ^= tmp;
|
||||
}
|
||||
@ -0,0 +1,44 @@
|
||||
/*
|
||||
$Id: link_fbw.h,v 1.1 2011-01-18 12:48:38 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef LINK_FBW_H
|
||||
#define LINK_FBW_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#include "link_autopilot.h"
|
||||
|
||||
void link_fbw_init( void );
|
||||
void link_fbw_send( void );
|
||||
void link_fbw_on_spi_it( void );
|
||||
|
||||
extern volatile uint8_t link_fbw_nb_err;
|
||||
extern uint8_t link_fbw_fbw_nb_err;
|
||||
|
||||
extern struct inter_mcu_msg from_fbw;
|
||||
extern struct inter_mcu_msg to_fbw;
|
||||
extern volatile uint8_t link_fbw_receive_complete;
|
||||
extern volatile uint8_t link_fbw_receive_valid;
|
||||
|
||||
#endif /* LINK_FBW_H */
|
||||
@ -0,0 +1,677 @@
|
||||
/*
|
||||
$Id: main.c,v 1.2 2011-01-18 14:55:52 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 main.c
|
||||
\brief Regroup main functions
|
||||
|
||||
*/
|
||||
#include <inttypes.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "link_autopilot.h"
|
||||
|
||||
#include "timer.h"
|
||||
#include "adc.h"
|
||||
#include "pid.h"
|
||||
#include "gps.h"
|
||||
#include "infrared.h"
|
||||
#include "downlink.h"
|
||||
#include "nav.h"
|
||||
#include "autopilot.h"
|
||||
#include "estimator.h"
|
||||
#include "if_calib.h"
|
||||
|
||||
//
|
||||
//
|
||||
// FIXME estimator_flight_time should not be manipuled here anymore
|
||||
//
|
||||
/** Define minimal speed for takeoff in m/s */
|
||||
#define MIN_SPEED_FOR_TAKEOFF 5.
|
||||
|
||||
|
||||
uint8_t fatal_error_nb = 0;
|
||||
static const uint16_t version = 1;
|
||||
|
||||
/** in seconds */
|
||||
static uint16_t cputime = 0;
|
||||
|
||||
uint8_t pprz_mode = PPRZ_MODE_MANUAL;
|
||||
uint8_t vertical_mode = VERTICAL_MODE_MANUAL;
|
||||
uint8_t lateral_mode = LATERAL_MODE_MANUAL;
|
||||
uint8_t ir_estim_mode = IR_ESTIM_MODE_ON;
|
||||
bool_t auto_pitch = FALSE;
|
||||
|
||||
bool_t rc_event_1, rc_event_2;
|
||||
|
||||
uint8_t vsupply;
|
||||
|
||||
static uint8_t mcu1_status, mcu1_ppm_cpt;
|
||||
|
||||
static bool_t low_battery = FALSE;
|
||||
|
||||
float slider_1_val, slider_2_val;
|
||||
|
||||
bool_t launch = FALSE;
|
||||
static uint8_t boot = TRUE;
|
||||
static uint8_t count;
|
||||
//#define Min(x, y) (x < y ? x : y)
|
||||
//#define Max(x, y) (x > y ? x : y)
|
||||
|
||||
|
||||
#define NO_CALIB 0 /**< \enum No calibration state */
|
||||
#define WAITING_CALIB_CONTRAST 1 /**< \enum Waiting calibration contrast state */
|
||||
#define CALIB_DONE 2 /**< \enum Calibration done state */
|
||||
|
||||
/** Maximal delay for calibration */
|
||||
#define MAX_DELAY_FOR_CALIBRATION 10
|
||||
|
||||
/** \fn inline void ground_calibrate( void )
|
||||
\brief Calibrate contrast if paparazzi mode is
|
||||
set to auto1 before MAX_DELAY_FOR_CALIBRATION secondes */
|
||||
/**User must put verticaly the uav (nose bottom) and push
|
||||
radio roll stick to get new calibration
|
||||
If not, the default calibration is used.
|
||||
*/
|
||||
inline void ground_calibrate( void )
|
||||
{
|
||||
static uint8_t calib_status = NO_CALIB;
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch (calib_status) {
|
||||
case NO_CALIB:
|
||||
if (cputime < MAX_DELAY_FOR_CALIBRATION && pprz_mode == PPRZ_MODE_AUTO1 ) {
|
||||
calib_status = WAITING_CALIB_CONTRAST;
|
||||
DOWNLINK_SEND_CALIB_START();
|
||||
}
|
||||
break;
|
||||
case WAITING_CALIB_CONTRAST:
|
||||
if (STICK_PUSHED(from_fbw.channels[ RADIO_ROLL ])) {
|
||||
ir_gain_calib();
|
||||
estimator_rad_of_ir = ir_rad_of_ir;
|
||||
DOWNLINK_SEND_RAD_OF_IR(&estimator_ir, &estimator_rad, &estimator_rad_of_ir, &ir_roll_neutral, &ir_pitch_neutral);
|
||||
calib_status = CALIB_DONE;
|
||||
DOWNLINK_SEND_CALIB_CONTRAST(&ir_contrast);
|
||||
}
|
||||
break;
|
||||
case CALIB_DONE:
|
||||
break;
|
||||
}
|
||||
#else*/
|
||||
if ( calib_status == NO_CALIB ) {
|
||||
if ( cputime < MAX_DELAY_FOR_CALIBRATION && pprz_mode == PPRZ_MODE_AUTO1 ) {
|
||||
calib_status = WAITING_CALIB_CONTRAST;
|
||||
DOWNLINK_SEND_CALIB_START();
|
||||
}
|
||||
} else
|
||||
if ( calib_status == WAITING_CALIB_CONTRAST ) {
|
||||
if ( STICK_PUSHED( from_fbw.channels[ RADIO_ROLL ] ) ) {
|
||||
ir_gain_calib();
|
||||
estimator_rad_of_ir = ir_rad_of_ir;
|
||||
DOWNLINK_SEND_RAD_OF_IR( &estimator_ir, &estimator_rad, &estimator_rad_of_ir,
|
||||
&ir_roll_neutral, &ir_pitch_neutral );
|
||||
calib_status = CALIB_DONE;
|
||||
DOWNLINK_SEND_CALIB_CONTRAST( &ir_contrast );
|
||||
}
|
||||
} else {}
|
||||
|
||||
//#endif
|
||||
}
|
||||
|
||||
|
||||
/** \fn inline uint8_t pprz_mode_update( void )
|
||||
\brief Update paparazzi mode
|
||||
*/
|
||||
inline uint8_t pprz_mode_update( void )
|
||||
{
|
||||
/** We remain in home mode until explicit reset from the RC */
|
||||
if ( pprz_mode != PPRZ_MODE_HOME || CheckEvent( rc_event_1 ) )
|
||||
ModeUpdate( pprz_mode, PPRZ_MODE_OF_PULSE( from_fbw.channels[ RADIO_MODE ],
|
||||
from_fbw.status ) );
|
||||
|
||||
else
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
#ifdef RADIO_LLS
|
||||
/** \fn inline uint8_t ir_estim_mode_update( void )
|
||||
\brief update ir estimation if RADIO_LLS is true \n
|
||||
*/
|
||||
inline uint8_t ir_estim_mode_update( void )
|
||||
{
|
||||
ModeUpdate( ir_estim_mode,
|
||||
IR_ESTIM_MODE_OF_PULSE( from_fbw.channels[ RADIO_LLS ] ) );
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
/** \fn inline uint8_t mcu1_status_update( void )
|
||||
\brief @@@@@ A FIXER @@@@@
|
||||
*/
|
||||
inline uint8_t mcu1_status_update( void )
|
||||
{
|
||||
uint8_t new_mode = from_fbw.status;
|
||||
if ( mcu1_status != new_mode ) {
|
||||
bool_t changed = ( ( mcu1_status & MASK_FBW_CHANGED ) !=
|
||||
( new_mode & MASK_FBW_CHANGED ) );
|
||||
mcu1_status = new_mode;
|
||||
return changed;
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
/** Delay between @@@@@ A FIXER @@@@@ */
|
||||
#define EVENT_DELAY 20
|
||||
|
||||
/** \def EventUpdate(_cpt, _cond, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define EventUpdate(_cpt, _cond, _event) \
|
||||
if (_cond) { \
|
||||
if (_cpt < EVENT_DELAY) { \
|
||||
_cpt++; \
|
||||
if (_cpt == EVENT_DELAY) \
|
||||
_event = TRUE; \
|
||||
} \
|
||||
} else { \
|
||||
_cpt = 0; \
|
||||
_event = FALSE; \
|
||||
}
|
||||
/** \def EventPos(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define EventPos(_cpt, _channel, _event) \
|
||||
EventUpdate(_cpt, (inflight_calib_mode==IF_CALIB_MODE_NONE && from_fbw.channels[ _channel ]>(int)(0.75*MAX_PPRZ)), _event)
|
||||
|
||||
/** \def EventNeg(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define EventNeg(_cpt, _channel, _event) \
|
||||
EventUpdate(_cpt, (inflight_calib_mode==IF_CALIB_MODE_NONE && from_fbw.channels[ _channel ]<(int)(-0.75*MAX_PPRZ)), _event)
|
||||
|
||||
/** \fn static inline void events_update( void )
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
static inline void events_update( void )
|
||||
{
|
||||
static uint16_t event1_cpt = 0;
|
||||
static uint16_t event2_cpt = 0;
|
||||
EventPos( event1_cpt, RADIO_GAIN1, rc_event_1 );
|
||||
EventNeg( event2_cpt, RADIO_GAIN1, rc_event_2 );
|
||||
}
|
||||
|
||||
|
||||
/** \fn inline void copy_from_to_fbw ( void )
|
||||
\brief Send back uncontrolled channels (only rudder)
|
||||
*/
|
||||
inline void copy_from_to_fbw ( void )
|
||||
{
|
||||
to_fbw.channels[ RADIO_YAW ] = from_fbw.channels[ RADIO_YAW ];
|
||||
#ifdef ANTON_T7
|
||||
to_fbw.channels[ RADIO_PITCH ] = from_fbw.channels[ RADIO_PITCH ];
|
||||
#endif
|
||||
to_fbw.status = 0;
|
||||
}
|
||||
|
||||
#ifdef EST_TEST
|
||||
float est_pos_x;
|
||||
float est_pos_y;
|
||||
float est_fcourse;
|
||||
uint8_t ticks_last_est; // 20Hz
|
||||
#endif /* EST_TEST */
|
||||
|
||||
|
||||
|
||||
/*
|
||||
called at 20Hz.
|
||||
sends a serie of initialisation messages followed by a stream of periodic ones
|
||||
*/
|
||||
|
||||
/** Define number of message at initialisation */
|
||||
#define INIT_MSG_NB 2
|
||||
/** @@@@@ A FIXER @@@@ */
|
||||
#define HI_FREQ_PHASE_NB 5
|
||||
|
||||
//static char signature[ 16 ] = MESSAGES_MD5SUM;
|
||||
|
||||
/** \def PERIODIC_SEND_BAT()
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define PERIODIC_SEND_BAT() DOWNLINK_SEND_BAT(&vsupply, &estimator_flight_time, &low_battery, &block_time, &stage_time)
|
||||
/** \def EventPos(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define PERIODIC_SEND_DEBUG() DOWNLINK_SEND_DEBUG(&link_fbw_nb_err, &link_fbw_fbw_nb_err, &modem_nb_ovrn, &gps_nb_ovrn, &mcu1_ppm_cpt);
|
||||
/** \def EventPos(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define PERIODIC_SEND_ATTITUDE() DOWNLINK_SEND_ATTITUDE(&estimator_phi, &estimator_psi, &estimator_theta);
|
||||
/** \def EventPos(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define PERIODIC_SEND_ADC() DOWNLINK_SEND_ADC(&ir_roll, &ir_pitch);
|
||||
/** \def EventPos(_cpt, _channel, _event)
|
||||
@@@@@ A FIXER @@@@@
|
||||
*/
|
||||
#define PERIODIC_SEND_STABILISATION() DOWNLINK_SEND_STABILISATION(&roll_pgain, &pitch_pgain);
|
||||
#define PERIODIC_SEND_CLIMB_PID() DOWNLINK_SEND_CLIMB_PID(&desired_gaz, &desired_climb, &climb_sum_err, &climb_pgain);
|
||||
#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode);
|
||||
#define PERIODIC_SEND_DESIRED() DOWNLINK_SEND_DESIRED(&desired_roll, &desired_pitch, &desired_x, &desired_y, &desired_altitude);
|
||||
#define PERIODIC_SEND_PITCH() DOWNLINK_SEND_PITCH(&ir_pitch, &ir_pitch_neutral, &ir_gain);
|
||||
|
||||
#define PERIODIC_SEND_NAVIGATION_REF() DOWNLINK_SEND_NAVIGATION_REF(&utm_east0, &utm_north0);
|
||||
|
||||
#ifdef RADIO_CALIB
|
||||
#define PERIODIC_SEND_SETTINGS() if (inflight_calib_mode != IF_CALIB_MODE_NONE) DOWNLINK_SEND_SETTINGS(&inflight_calib_mode, &slider_1_val, &slider_2_val);
|
||||
#else
|
||||
#define PERIODIC_SEND_SETTINGS()
|
||||
#endif
|
||||
|
||||
|
||||
/** \fn inline void reporting_task( void )
|
||||
\brief Send a serie of initialisation messages followed by a stream of periodic ones\n
|
||||
Called at 20Hz.
|
||||
*/
|
||||
|
||||
void send_boot( void )
|
||||
{
|
||||
/** initialisation phase during boot */
|
||||
if ( boot ) {
|
||||
DOWNLINK_SEND_BOOT( &version );
|
||||
DOWNLINK_SEND_RAD_OF_IR( &estimator_ir, &estimator_rad, &estimator_rad_of_ir,
|
||||
&ir_roll_neutral, &ir_pitch_neutral );
|
||||
boot = FALSE;
|
||||
}
|
||||
}
|
||||
void send_attitude( void ) //500ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
count++;
|
||||
if ( count == 250 ) count = 0;
|
||||
if ( count % 5 == 0 )
|
||||
PERIODIC_SEND_ATTITUDE();
|
||||
}
|
||||
}
|
||||
|
||||
void send_adc( void ) //500ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 5 == 1 ) PERIODIC_SEND_ADC();
|
||||
}
|
||||
}
|
||||
void send_settings( void ) //500ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 5 == 2 ) PERIODIC_SEND_SETTINGS();
|
||||
}
|
||||
}
|
||||
void send_desired( void ) //1000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 10 == 3 ) PERIODIC_SEND_DESIRED();
|
||||
}
|
||||
}
|
||||
void send_bat( void ) //2000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 20 == 8 ) PERIODIC_SEND_BAT();
|
||||
}
|
||||
}
|
||||
void send_climb( void ) //2000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 20 == 18 ) PERIODIC_SEND_CLIMB_PID();
|
||||
}
|
||||
}
|
||||
void send_mode( void ) //5000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 50 == 9 ) PERIODIC_SEND_PPRZ_MODE();
|
||||
}
|
||||
}
|
||||
void send_debug( void ) //5000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 50 == 29 ) PERIODIC_SEND_DEBUG();
|
||||
}
|
||||
}
|
||||
void send_nav_ref( void ) //10000ms
|
||||
{
|
||||
if ( !boot ) {
|
||||
if ( count % 100 == 49 ) PERIODIC_SEND_NAVIGATION_REF();
|
||||
}
|
||||
}
|
||||
|
||||
/** \fn inline uint8_t inflight_calib_mode_update ( void )
|
||||
\brief @@@@@ A FIXER @@@@@
|
||||
*/
|
||||
inline uint8_t inflight_calib_mode_update ( void )
|
||||
{
|
||||
ModeUpdate( inflight_calib_mode,
|
||||
IF_CALIB_MODE_OF_PULSE( from_fbw.channels[ RADIO_CALIB ] ) );
|
||||
}
|
||||
|
||||
|
||||
/** \fn inline void radio_control_task( void )
|
||||
\brief @@@@@ A FIXER @@@@@
|
||||
*/
|
||||
void _Pragma( "entrypoint" ) radio_control_task( void )
|
||||
{
|
||||
bool_t calib_mode_changed;
|
||||
if ( link_fbw_receive_valid ) {
|
||||
uint8_t mode_changed = FALSE;
|
||||
copy_from_to_fbw();
|
||||
if ( ( bit_is_set( from_fbw.status, RADIO_REALLY_LOST ) &&
|
||||
( pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL ) ) ||
|
||||
too_far_from_home ) {
|
||||
pprz_mode = PPRZ_MODE_HOME;
|
||||
mode_changed = TRUE;
|
||||
}
|
||||
if ( bit_is_set( from_fbw.status, AVERAGED_CHANNELS_SENT ) ) {
|
||||
bool_t pprz_mode_changed = pprz_mode_update();
|
||||
mode_changed |= pprz_mode_changed;
|
||||
#ifdef RADIO_LLS
|
||||
mode_changed |= ir_estim_mode_update();
|
||||
#endif
|
||||
#ifdef RADIO_CALIB
|
||||
calib_mode_changed = inflight_calib_mode_update();
|
||||
inflight_calib( calib_mode_changed || pprz_mode_changed );
|
||||
mode_changed |= calib_mode_changed;
|
||||
#endif
|
||||
}
|
||||
mode_changed |= mcu1_status_update();
|
||||
if ( mode_changed )
|
||||
DOWNLINK_SEND_PPRZ_MODE( &pprz_mode, &vertical_mode, &inflight_calib_mode,
|
||||
&mcu1_status, &ir_estim_mode );
|
||||
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO1 ) {
|
||||
desired_roll = FLOAT_OF_PPRZ( from_fbw.channels[ RADIO_ROLL ], 0., -0.6 );
|
||||
desired_pitch = FLOAT_OF_PPRZ( from_fbw.channels[ RADIO_PITCH ], 0., 0.5 );
|
||||
} // else asynchronously set by course_pid_run()
|
||||
if ( pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1 ) {
|
||||
desired_gaz = from_fbw.channels[ RADIO_THROTTLE ];
|
||||
#ifdef ANTON_MAGICAL_MISTERY_GAINS
|
||||
roll_pgain = ROLL_PGAIN * ( 1 - 5. / 7. * from_fbw.channels[ RADIO_THROTTLE ] /
|
||||
MAX_PPRZ );
|
||||
pitch_pgain = PITCH_PGAIN * ( 1 - 1. / 3. * from_fbw.channels[ RADIO_THROTTLE ] /
|
||||
MAX_PPRZ );
|
||||
#endif /* ANTON_MAGICAL_MISTERY_GAINS */
|
||||
}
|
||||
// else asynchronously set by climb_pid_run();
|
||||
|
||||
mcu1_ppm_cpt = from_fbw.ppm_cpt;
|
||||
vsupply = from_fbw.vsupply;
|
||||
|
||||
events_update();
|
||||
|
||||
if ( !estimator_flight_time ) {
|
||||
ground_calibrate();
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 &&
|
||||
from_fbw.channels[ RADIO_THROTTLE ] > GAZ_THRESHOLD_TAKEOFF )
|
||||
launch = TRUE;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/** \fn void navigation_task( void )
|
||||
\brief Compute desired_course
|
||||
*/
|
||||
void navigation_update( void )
|
||||
{
|
||||
|
||||
/* Default to keep compatibility with previous behaviour */
|
||||
lateral_mode = LATERAL_MODE_COURSE;
|
||||
if ( pprz_mode == PPRZ_MODE_HOME )
|
||||
nav_home();
|
||||
else
|
||||
nav_update();
|
||||
}
|
||||
|
||||
void send_nav_values( void )
|
||||
{
|
||||
|
||||
DOWNLINK_SEND_NAVIGATION( &nav_block, &nav_stage, &estimator_x, &estimator_y,
|
||||
&desired_course, &dist2_to_wp, &course_pgain, &dist2_to_home );
|
||||
}
|
||||
|
||||
void course_run( void )
|
||||
{
|
||||
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME ) {
|
||||
if ( lateral_mode >= LATERAL_MODE_COURSE )
|
||||
course_pid_run(); /* aka compute nav_desired_roll */
|
||||
desired_roll = nav_desired_roll;
|
||||
}
|
||||
}
|
||||
|
||||
void _Pragma( "entrypoint" ) altitude_control_task( void )
|
||||
{
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME ) {
|
||||
if ( vertical_mode == VERTICAL_MODE_AUTO_ALT )
|
||||
altitude_pid_run();
|
||||
}
|
||||
}
|
||||
void _Pragma( "entrypoint" ) climb_control_task( void )
|
||||
{
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME ) {
|
||||
if ( vertical_mode >= VERTICAL_MODE_AUTO_CLIMB )
|
||||
climb_pid_run();
|
||||
if ( vertical_mode == VERTICAL_MODE_AUTO_GAZ )
|
||||
desired_gaz = nav_desired_gaz;
|
||||
if ( low_battery || ( !estimator_flight_time && !launch ) )
|
||||
desired_gaz = 0.;
|
||||
}
|
||||
}
|
||||
#define PERIOD (256. * 1024. / CLOCK / 1000000.)
|
||||
|
||||
/** Maximum time allowed for low battery level */
|
||||
#define LOW_BATTERY_DELAY 5
|
||||
|
||||
/** \fn inline void periodic_task( void )
|
||||
\brief Do periodic tasks at 61 Hz
|
||||
*/
|
||||
/**There are four @@@@@ boucles @@@@@:
|
||||
- 20 Hz:
|
||||
- lets use \a reporting_task at 10 Hz
|
||||
- updates ir with \a ir_update
|
||||
- updates estimator of ir with \a estimator_update_state_infrared
|
||||
- set \a desired_aileron and \a desired_elevator with \a roll_pitch_pid_run
|
||||
- sends to \a fbw \a desired_gaz, \a desired_aileron and
|
||||
\a desired_elevator \note \a desired_gaz is set upon GPS
|
||||
message reception
|
||||
- 10 Hz: to get a \a stage_time_ds
|
||||
- 4 Hz:
|
||||
- calls \a estimator_propagate_state
|
||||
- do navigation with \a navigation_task
|
||||
|
||||
*/
|
||||
|
||||
#ifdef PAPABENCH_SINGLE
|
||||
uint8_t _20Hz = 0;
|
||||
uint8_t _1Hz = 0;
|
||||
#else
|
||||
static uint8_t _20Hz = 0;
|
||||
static uint8_t _1Hz = 0;
|
||||
#endif
|
||||
void periodic_task( void )
|
||||
{
|
||||
static uint8_t _10Hz = 0;
|
||||
static uint8_t _4Hz = 0;
|
||||
static uint8_t t = 0;
|
||||
|
||||
estimator_t += PERIOD;
|
||||
|
||||
_20Hz++;
|
||||
if ( _20Hz >= 3 ) _20Hz = 0;
|
||||
_10Hz++;
|
||||
if ( _10Hz >= 6 ) _10Hz = 0;
|
||||
_4Hz++;
|
||||
if ( _4Hz >= 15 ) _4Hz = 0;
|
||||
_1Hz++;
|
||||
if ( _1Hz >= 61 ) _1Hz = 0;
|
||||
|
||||
if ( !_10Hz )
|
||||
stage_time_ds = stage_time_ds + .1;
|
||||
if ( !_1Hz ) {
|
||||
if ( estimator_flight_time ) estimator_flight_time++;
|
||||
cputime++;
|
||||
stage_time_ds = ( int16_t )( stage_time_ds + .5 );
|
||||
stage_time++;
|
||||
block_time++;
|
||||
|
||||
if ( vsupply < LOW_BATTERY ) t++;
|
||||
else t = 0;
|
||||
low_battery |= ( t >= LOW_BATTERY_DELAY );
|
||||
}
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch(_4Hz) {
|
||||
case 0:
|
||||
estimator_propagate_state();
|
||||
navigation_task();
|
||||
altitude_control_task();//added 4-05-06
|
||||
climb_control_task();//added 04-05-06
|
||||
break;
|
||||
// default:
|
||||
}
|
||||
switch (_20Hz) {
|
||||
case 0:
|
||||
break;
|
||||
case 1: {
|
||||
static uint8_t odd;
|
||||
odd++;
|
||||
if (odd & 0x01)
|
||||
reporting_task();
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
stabilisation_task();
|
||||
link_fbw_send();
|
||||
break;
|
||||
default:
|
||||
fatal_error_nb++;
|
||||
}
|
||||
#else */
|
||||
if ( _4Hz == 0 ) {
|
||||
estimator_propagate_state();
|
||||
/*navigation_task */
|
||||
navigation_update();
|
||||
send_nav_values();
|
||||
course_run();
|
||||
/*end navigation*/
|
||||
altitude_control_task();
|
||||
climb_control_task();
|
||||
}
|
||||
|
||||
if ( _20Hz == 0 )
|
||||
{}
|
||||
else
|
||||
if ( _20Hz == 1 ) {
|
||||
static uint8_t odd;
|
||||
odd++;
|
||||
if ( odd & 0x01 ) {
|
||||
/*reporting_task()*/
|
||||
send_boot();
|
||||
send_attitude();
|
||||
send_adc();
|
||||
send_settings();
|
||||
send_desired();
|
||||
send_bat();
|
||||
send_climb();
|
||||
send_mode();
|
||||
send_debug();
|
||||
send_nav_ref();
|
||||
}
|
||||
} else
|
||||
if ( _20Hz == 2 ) {
|
||||
stabilisation_task();
|
||||
link_fbw_send();
|
||||
} else
|
||||
fatal_error_nb++;
|
||||
//#endif
|
||||
}
|
||||
|
||||
void _Pragma( "entrypoint" ) stabilisation_task( void )
|
||||
{
|
||||
ir_update();
|
||||
estimator_update_state_infrared();
|
||||
roll_pitch_pid_run(); // Set desired_aileron & desired_elevator
|
||||
to_fbw.channels[ RADIO_THROTTLE ] =
|
||||
desired_gaz; // desired_gaz is set upon GPS message reception
|
||||
to_fbw.channels[ RADIO_ROLL ] = desired_aileron;
|
||||
#ifndef ANTON_T7
|
||||
to_fbw.channels[ RADIO_PITCH ] = desired_elevator;
|
||||
#endif
|
||||
|
||||
// Code for camera stabilization, FIXME put that elsewhere
|
||||
to_fbw.channels[ RADIO_GAIN1 ] = TRIM_PPRZ( MAX_PPRZ / 0.75 *
|
||||
( -estimator_phi ) );
|
||||
}
|
||||
|
||||
/*void receive_gps_data_task(void)
|
||||
{
|
||||
parse_gps_msg();
|
||||
gps_msg_received = FALSE;
|
||||
if (gps_pos_available)
|
||||
{
|
||||
use_gps_pos();
|
||||
gps_pos_available = FALSE;
|
||||
}
|
||||
}*/
|
||||
/** \fn void use_gps_pos( void )
|
||||
\brief use GPS
|
||||
*/
|
||||
/**Send by downlink the GPS and rad_of_ir messages with \a DOWNLINK_SEND_GPS
|
||||
and \a DOWNLINK_SEND_RAD_OF_IR \n
|
||||
If \a estimator_flight_time is null and \a estimator_hspeed_mod is greater
|
||||
than \a MIN_SPEED_FOR_TAKEOFF, set the \a estimator_flight_time to 1 and \a
|
||||
launch to true (which is not set in non auto launch. Then call
|
||||
\a DOWNLINK_SEND_TAKEOFF
|
||||
*/
|
||||
void send_gps_pos( void )
|
||||
{
|
||||
gps_msg_received = FALSE;
|
||||
if ( gps_pos_available ) {
|
||||
DOWNLINK_SEND_GPS( &gps_mode, &gps_utm_east, &gps_utm_north, &gps_fcourse,
|
||||
&gps_falt, &gps_fspeed, &gps_fclimb, &gps_ftow );
|
||||
estimator_update_state_gps();
|
||||
}
|
||||
}
|
||||
|
||||
void send_radIR( void )
|
||||
{
|
||||
if ( gps_pos_available )
|
||||
DOWNLINK_SEND_RAD_OF_IR( &estimator_ir, &estimator_rad, &estimator_rad_of_ir,
|
||||
&ir_roll_neutral, &ir_pitch_neutral );
|
||||
}
|
||||
|
||||
void send_takeOff( void )
|
||||
{
|
||||
if ( gps_pos_available ) {
|
||||
if ( !estimator_flight_time &&
|
||||
( estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF ) ) {
|
||||
estimator_flight_time = 1;
|
||||
launch = TRUE; /* Not set in non auto launch */
|
||||
DOWNLINK_SEND_TAKEOFF( &cputime );
|
||||
}
|
||||
gps_pos_available = FALSE;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,101 @@
|
||||
/*
|
||||
$Id: mainloop.c,v 1.3 2011-01-25 09:40:37 plazar Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <arch/interrupt.h>
|
||||
#include "std.h"
|
||||
|
||||
#include "timer.h"
|
||||
#include "modem.h"
|
||||
#include "adc.h"
|
||||
#include "airframe.h"
|
||||
#include "autopilot.h"
|
||||
#include "spi.h"
|
||||
#include "link_fbw.h"
|
||||
#include "gps.h"
|
||||
#include "nav.h"
|
||||
#include "infrared.h"
|
||||
#include "estimator.h"
|
||||
#include "downlink.h"
|
||||
|
||||
#ifndef PAPABENCH_SINGLE
|
||||
void fbw_init( void );
|
||||
void fbw_schedule( void );
|
||||
#endif
|
||||
|
||||
int main( void )
|
||||
{
|
||||
uint8_t init_cpt;
|
||||
/* init peripherals */
|
||||
timer_init();
|
||||
modem_init();
|
||||
adc_init();
|
||||
#ifdef CTL_BRD_V1_1
|
||||
adc_buf_channel( ADC_CHANNEL_BAT, &buf_bat );
|
||||
#endif
|
||||
spi_init();
|
||||
link_fbw_init();
|
||||
gps_init();
|
||||
nav_init();
|
||||
ir_init();
|
||||
estimator_init();
|
||||
# ifdef PAPABENCH_SINGLE
|
||||
fbw_init();
|
||||
# endif
|
||||
|
||||
/* start interrupt task */
|
||||
//sei(); /*Fadia*/
|
||||
|
||||
/* Wait 0.5s (for modem init ?) */
|
||||
init_cpt = 30;
|
||||
_Pragma( "loopbound min 31 max 31" )
|
||||
while ( init_cpt ) {
|
||||
if ( timer_periodic() )
|
||||
init_cpt--;
|
||||
}
|
||||
|
||||
/* enter mainloop */
|
||||
#ifndef NO_MAINLOOP
|
||||
while ( 1 ) {
|
||||
#endif
|
||||
if ( timer_periodic() ) {
|
||||
periodic_task();
|
||||
# if PAPABENCH_SINGLE
|
||||
fbw_schedule();
|
||||
# endif
|
||||
}
|
||||
if ( gps_msg_received ) {
|
||||
/*receive_gps_data_task()*/
|
||||
parse_gps_msg();
|
||||
send_gps_pos();
|
||||
send_radIR();
|
||||
send_takeOff();
|
||||
}
|
||||
if ( link_fbw_receive_complete ) {
|
||||
link_fbw_receive_complete = FALSE;
|
||||
radio_control_task();
|
||||
}
|
||||
#ifndef NO_MAINLOOP
|
||||
}
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,95 @@
|
||||
/*
|
||||
Paparazzi mcu0 cmx469 modem functions
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <inttypes.h>
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
//#include <stdlib.h>
|
||||
#include "modem.h"
|
||||
#include "std.h"
|
||||
|
||||
uint8_t modem_nb_ovrn;
|
||||
|
||||
uint8_t tx_head;
|
||||
volatile uint8_t tx_tail;
|
||||
uint8_t tx_buf[ TX_BUF_SIZE ];
|
||||
|
||||
uint8_t tx_byte;
|
||||
uint8_t tx_byte_idx;
|
||||
|
||||
uint8_t ck_a, ck_b;
|
||||
|
||||
void modem_init( void )
|
||||
{
|
||||
/*
|
||||
on >= V1.2 boards, 4MHz modem clock is generated
|
||||
by one PWM module.
|
||||
*/
|
||||
#if defined CTL_BRD_V1_2 || defined CTL_BRD_V1_2_1
|
||||
MODEM_OSC_DDR |= _BV( MODEM_OSC );
|
||||
OCR0 = 1; /* 4MhZ */
|
||||
TCCR0 = _BV( WGM01 ) | _BV( COM00 ) | _BV( CS00 );
|
||||
#endif
|
||||
|
||||
/* setup TX_EN and TX_DATA pin as output */
|
||||
MODEM_TX_DDR |= _BV( MODEM_TX_EN ) | _BV( MODEM_TX_DATA );
|
||||
/* data idles hight */
|
||||
sbi( MODEM_TX_PORT, MODEM_TX_DATA );
|
||||
/* enable transmitter */
|
||||
cbi( MODEM_TX_PORT, MODEM_TX_EN );
|
||||
/* set interrupt on failing edge of clock */
|
||||
MODEM_CLK_INT_REG |= MODEM_CLK_INT_CFG;
|
||||
}
|
||||
|
||||
SIGNAL( MODEM_CLK_INT_SIG )
|
||||
{
|
||||
/* start bit */
|
||||
if ( tx_byte_idx == 0 )
|
||||
cbi( MODEM_TX_PORT, MODEM_TX_DATA );
|
||||
/* 8 data bits */
|
||||
else
|
||||
if ( tx_byte_idx < 9 ) {
|
||||
if ( tx_byte & 0x01 )
|
||||
sbi( MODEM_TX_PORT, MODEM_TX_DATA );
|
||||
else
|
||||
cbi( MODEM_TX_PORT, MODEM_TX_DATA );
|
||||
tx_byte >>= 1;
|
||||
}
|
||||
/* stop_bit */
|
||||
else
|
||||
sbi( MODEM_TX_PORT, MODEM_TX_DATA );
|
||||
tx_byte_idx++;
|
||||
/* next byte */
|
||||
if ( tx_byte_idx >= 10 ) {
|
||||
/* if we have nothing left to transmit */
|
||||
if ( tx_head == tx_tail ) {
|
||||
/* disable clock interrupt */
|
||||
cbi( EIMSK, MODEM_CLK_INT );
|
||||
} else {
|
||||
/* else load next byte */
|
||||
MODEM_LOAD_NEXT_BYTE();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,140 @@
|
||||
/*
|
||||
Paparazzi mcu0 cmx469 modem functions
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef MODEM_H
|
||||
#define MODEM_H
|
||||
|
||||
#include "airframe.h"
|
||||
|
||||
void modem_init( void );
|
||||
|
||||
extern uint8_t modem_nb_ovrn;
|
||||
|
||||
#define TX_BUF_SIZE 255
|
||||
extern uint8_t tx_head;
|
||||
extern volatile uint8_t tx_tail;
|
||||
extern uint8_t tx_buf[ TX_BUF_SIZE ];
|
||||
|
||||
extern uint8_t tx_byte;
|
||||
extern uint8_t tx_byte_idx;
|
||||
|
||||
extern uint8_t ck_a, ck_b;
|
||||
|
||||
#define ModemStartMessage(id) \
|
||||
{ MODEM_PUT_1_BYTE(STX); MODEM_PUT_1_BYTE(id); ck_a = id; ck_b = id; }
|
||||
|
||||
#define ModemEndMessage() \
|
||||
{ MODEM_PUT_1_BYTE(ck_a); MODEM_PUT_1_BYTE(ck_b); MODEM_CHECK_RUNNING(); }
|
||||
|
||||
|
||||
#define MODEM_TX_PORT PORTD
|
||||
#define MODEM_TX_DDR DDRD
|
||||
#define MODEM_TX_EN 7
|
||||
#define MODEM_TX_DATA 6
|
||||
|
||||
#ifdef CTL_BRD_V1_1
|
||||
#define MODEM_CLK_DDR DDRD
|
||||
#define MODEM_CLK_PORT PORTD
|
||||
#define MODEM_CLK 0
|
||||
#define MODEM_CLK_INT INT0
|
||||
#define MODEM_CLK_INT_REG EICRA
|
||||
#define MODEM_CLK_INT_CFG _BV(ISC01)
|
||||
#define MODEM_CLK_INT_SIG SIG_INTERRUPT0
|
||||
#endif /* CTL_BRD_V1_1 */
|
||||
|
||||
#ifdef CTL_BRD_V1_2
|
||||
#define MODEM_CLK_DDR DDRD
|
||||
#define MODEM_CLK_PORT PORTD
|
||||
#define MODEM_CLK 0
|
||||
#define MODEM_CLK_INT INT0
|
||||
#define MODEM_CLK_INT_REG EICRA
|
||||
#define MODEM_CLK_INT_CFG _BV(ISC01)
|
||||
#define MODEM_CLK_INT_SIG SIG_INTERRUPT0
|
||||
|
||||
#define MODEM_OSC_DDR DDRB
|
||||
#define MODEM_OSC_PORT PORTB
|
||||
#define MODEM_OSC 4
|
||||
#endif /* CTL_BRD_V1_2 */
|
||||
|
||||
#ifdef CTL_BRD_V1_2_1
|
||||
#define MODEM_CLK_DDR DDRE
|
||||
#define MODEM_CLK_PORT PORTE
|
||||
#define MODEM_CLK 4
|
||||
#define MODEM_CLK_INT INT4
|
||||
#define MODEM_CLK_INT_REG EICRB
|
||||
#define MODEM_CLK_INT_CFG _BV(ISC41)
|
||||
#define MODEM_CLK_INT_SIG SIG_INTERRUPT4
|
||||
#define MODEM_OSC_DDR DDRB
|
||||
#define MODEM_OSC_PORT PORTB
|
||||
#define MODEM_OSC 4
|
||||
#endif /* CTL_BRD_V1_2_1 */
|
||||
|
||||
|
||||
|
||||
#define MODEM_CHECK_FREE_SPACE(_space) (tx_head>=tx_tail? _space < (TX_BUF_SIZE - (tx_head - tx_tail)) : _space < (tx_tail - tx_head))
|
||||
|
||||
#define MODEM_PUT_1_BYTE(_byte) { \
|
||||
tx_buf[tx_head] = _byte; \
|
||||
tx_head++; \
|
||||
if (tx_head >= TX_BUF_SIZE) tx_head = 0; \
|
||||
}
|
||||
|
||||
#define MODEM_PUT_1_BYTE_BY_ADDR(_byte) { \
|
||||
tx_buf[tx_head] = *(_byte); \
|
||||
ck_a += *(_byte); \
|
||||
ck_b += ck_a; \
|
||||
tx_head++; \
|
||||
if (tx_head >= TX_BUF_SIZE) tx_head = 0; \
|
||||
}
|
||||
|
||||
#define MODEM_PUT_2_BYTE_BY_ADDR(_byte) { \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR(_byte); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR(_byte+1); \
|
||||
}
|
||||
|
||||
#define MODEM_PUT_4_BYTE_BY_ADDR(_byte) { \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR(_byte); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR(_byte+1); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR(_byte+2); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR(_byte+3); \
|
||||
}
|
||||
|
||||
#define MODEM_LOAD_NEXT_BYTE() { \
|
||||
tx_byte = tx_buf[tx_tail]; \
|
||||
tx_byte_idx = 0; \
|
||||
tx_tail++; \
|
||||
if( tx_tail >= TX_BUF_SIZE ) \
|
||||
tx_tail = 0; \
|
||||
}
|
||||
|
||||
#define MODEM_CHECK_RUNNING() { \
|
||||
if (!(EIMSK & _BV(MODEM_CLK_INT))) { \
|
||||
MODEM_LOAD_NEXT_BYTE() \
|
||||
sbi(EIFR, INTF0); \
|
||||
sbi(EIMSK, MODEM_CLK_INT); \
|
||||
} \
|
||||
}
|
||||
|
||||
|
||||
#endif /* MODEM_H */
|
||||
@ -0,0 +1,216 @@
|
||||
/*
|
||||
$Id: nav.c,v 1.3 2011-01-21 11:52:44 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#define NAV_C
|
||||
|
||||
#include "math.h"
|
||||
|
||||
#include "nav.h"
|
||||
#include "estimator.h"
|
||||
#include "pid.h"
|
||||
#include "autopilot.h"
|
||||
#include "link_fbw.h"
|
||||
#include "airframe.h"
|
||||
|
||||
uint8_t nav_stage, nav_block;
|
||||
uint8_t excpt_stage; /*To save the current stage when an exception is raised */
|
||||
static float last_x, last_y;
|
||||
static uint8_t last_wp;
|
||||
float rc_pitch;
|
||||
uint16_t stage_time, block_time;
|
||||
float stage_time_ds;
|
||||
|
||||
#define RcRoll(travel) (from_fbw.channels[ RADIO_ROLL ]* (float)travel /(float)MAX_PPRZ)
|
||||
|
||||
#define RcEvent1() CheckEvent(rc_event_1)
|
||||
#define RcEvent2() CheckEvent(rc_event_2)
|
||||
#define Block(x) case x: nav_block=x;
|
||||
#define InitBlock() { nav_stage = 0; block_time = 0; InitStage(); }
|
||||
#define NextBlock() { nav_block++; InitBlock(); }
|
||||
#define GotoBlock(b) { nav_block=b; InitBlock(); }
|
||||
|
||||
#define Stage(s) case s: nav_stage=s;
|
||||
#define InitStage() { last_x = estimator_x; last_y = estimator_y; stage_time = 0; stage_time_ds = 0.0; return; }
|
||||
#define NextStage() { nav_stage++; InitStage() }
|
||||
#define NextStageFrom(wp) { last_wp = wp; NextStage() }
|
||||
#define GotoStage(s) { nav_stage = s; InitStage() }
|
||||
|
||||
#define Label(x) label_ ## x:
|
||||
#define Goto(x) { goto label_ ## x; }
|
||||
|
||||
#define Exception(x) { excpt_stage = nav_stage; goto label_ ## x; }
|
||||
#define ReturnFromException(_) { GotoStage(excpt_stage) }
|
||||
|
||||
static bool_t approaching( uint8_t );
|
||||
static inline void fly_to_xy( float x, float y );
|
||||
static void fly_to( uint8_t wp );
|
||||
static void route_to( uint8_t last_wp, uint8_t wp );
|
||||
//static void glide_to(uint8_t last_wp, uint8_t wp);
|
||||
|
||||
#define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))
|
||||
|
||||
#define DegOfRad(x) ((x) / M_PI * 180.)
|
||||
#define RadOfDeg(x) ((x)/180. * M_PI)
|
||||
#define NormCourse(x) { _Pragma("loopbound min 0 max 0") while (x < 0) x += 360; _Pragma("loopbound min 0 max 0") while (x >= 360) x -= 360;}
|
||||
|
||||
static float qdr; /* Degrees from 0 to 360 */
|
||||
|
||||
#define CircleXY(x,y,radius) { float alpha = atan2(estimator_y - y, estimator_x - x);float alpha_carrot = alpha + CARROT / (-radius * estimator_hspeed_mod); fly_to_xy(x+cos(alpha_carrot)*fabs(radius),y+sin(alpha_carrot)*fabs(radius)); qdr = DegOfRad(M_PI/2 - alpha_carrot); NormCourse(qdr);}
|
||||
|
||||
#define MAX_DIST_CARROT 250.
|
||||
#define MIN_HEIGHT_CARROT 50.
|
||||
#define MAX_HEIGHT_CARROT 150.
|
||||
|
||||
#define Goto3D(radius) { static float carrot_x, carrot_y; int16_t pitch; int16_t roll; if (pprz_mode == PPRZ_MODE_AUTO2) { int16_t yaw = from_fbw.channels[ RADIO_YAW ]; if (yaw > MIN_DX || yaw < -MIN_DX) { carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.);carrot_x = Min(carrot_x, MAX_DIST_CARROT); carrot_x = Max(carrot_x, -MAX_DIST_CARROT); } pitch = from_fbw.channels[ RADIO_PITCH ]; if (pitch > MIN_DX || pitch < -MIN_DX) { carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); carrot_y = Min(carrot_y, MAX_DIST_CARROT); carrot_y = Max(carrot_y, -MAX_DIST_CARROT);} vertical_mode = VERTICAL_MODE_AUTO_ALT; roll = from_fbw.channels[ RADIO_ROLL ]; if (roll > MIN_DX || roll < -MIN_DX) { desired_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); desired_altitude = Max(desired_altitude, MIN_HEIGHT_CARROT+GROUND_ALT); desired_altitude = Min(desired_altitude, MAX_HEIGHT_CARROT+GROUND_ALT); } } CircleXY(carrot_x, carrot_y, radius); }
|
||||
|
||||
#define Circle(wp, radius) CircleXY(waypoints[ wp ].x, waypoints[ wp ].y, radius)
|
||||
|
||||
#define And(x, y) ((x) && (y))
|
||||
#define Or(x, y) ((x) || (y))
|
||||
//#define Min(x,y) (x < y ? x : y)
|
||||
//#define Max(x,y) (x > y ? x : y)
|
||||
#define Qdr(x) (Min(x, 350) < qdr && qdr < x+10)
|
||||
|
||||
#include "flight_plan.h"
|
||||
|
||||
|
||||
#define MIN_DIST2_WP (15.*15.)
|
||||
|
||||
#define DISTANCE2(p1_x, p1_y, p2) ((p1_x-p2.x)*(p1_x-p2.x)+(p1_y-p2.y)*(p1_y-p2.y))
|
||||
|
||||
const int32_t nav_east0 = NAV_UTM_EAST0;
|
||||
const int32_t nav_north0 = NAV_UTM_NORTH0;
|
||||
|
||||
float desired_altitude = GROUND_ALT + MIN_HEIGHT_CARROT;
|
||||
float desired_x, desired_y;
|
||||
uint16_t nav_desired_gaz;
|
||||
float nav_pitch = NAV_PITCH;
|
||||
float nav_desired_roll;
|
||||
|
||||
float dist2_to_wp, dist2_to_home;
|
||||
bool_t too_far_from_home;
|
||||
const uint8_t nb_waypoint = NB_WAYPOINT;
|
||||
|
||||
struct point waypoints[ NB_WAYPOINT + 1 ] = WAYPOINTS
|
||||
|
||||
static float carrot;
|
||||
static bool_t approaching( uint8_t wp )
|
||||
{
|
||||
float pw_x = waypoints[ wp ].x - estimator_x;
|
||||
float pw_y = waypoints[ wp ].y - estimator_y;
|
||||
float scal_prod;//changes for PowerPc Fev-06
|
||||
|
||||
dist2_to_wp = pw_x * pw_x + pw_y * pw_y;
|
||||
carrot = CARROT * estimator_hspeed_mod;
|
||||
carrot = ( carrot < 40 ? 40 : carrot );
|
||||
if ( dist2_to_wp < carrot * carrot )
|
||||
return TRUE;
|
||||
|
||||
scal_prod = ( waypoints[ wp ].x - last_x ) * pw_x + ( waypoints[ wp ].y - last_y ) *
|
||||
pw_y;
|
||||
|
||||
return ( scal_prod < 0 );
|
||||
}
|
||||
|
||||
static inline void fly_to_xy( float x, float y )
|
||||
{
|
||||
desired_x = x;
|
||||
desired_y = y;
|
||||
desired_course = M_PI / 2. - atan2( y - estimator_y, x - estimator_x );
|
||||
}
|
||||
|
||||
static void fly_to( uint8_t wp )
|
||||
{
|
||||
fly_to_xy( waypoints[ wp ].x, waypoints[ wp ].y );
|
||||
}
|
||||
|
||||
static float alpha, leg;
|
||||
static void route_to( uint8_t _last_wp, uint8_t wp )
|
||||
{
|
||||
float last_wp_x = waypoints[ _last_wp ].x;
|
||||
float last_wp_y = waypoints[ _last_wp ].y;
|
||||
float leg_x = waypoints[ wp ].x - last_wp_x;
|
||||
float leg_y = waypoints[ wp ].y - last_wp_y;
|
||||
float leg2 = leg_x * leg_x + leg_y * leg_y;
|
||||
alpha = ( ( estimator_x - last_wp_x ) * leg_x + ( estimator_y - last_wp_y ) *
|
||||
leg_y ) / leg2;
|
||||
alpha = Max( alpha, 0. );
|
||||
leg = sqrt( leg2 );
|
||||
alpha += Max( carrot / leg, 0. ); /* carrot computed in approaching() */
|
||||
alpha = Min( 1., alpha );
|
||||
fly_to_xy( last_wp_x + alpha * leg_x, last_wp_y + alpha * leg_y );
|
||||
}
|
||||
|
||||
/*static void glide_to(uint8_t _last_wp, uint8_t wp) {
|
||||
float last_alt = waypoints[ _last_wp ].a;
|
||||
desired_altitude = last_alt + alpha * (waypoints[ wp ].a - last_alt);
|
||||
pre_climb = NOMINAL_AIRSPEED * (waypoints[ wp ].a - last_alt) / leg;
|
||||
}*/
|
||||
|
||||
static inline void compute_dist2_to_home( void )
|
||||
{
|
||||
float ph_x = waypoints[ WP_HOME ].x - estimator_x;
|
||||
float ph_y = waypoints[ WP_HOME ].y - estimator_y;
|
||||
dist2_to_home = ph_x * ph_x + ph_y * ph_y;
|
||||
too_far_from_home = dist2_to_home > ( MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME );
|
||||
}
|
||||
|
||||
void nav_home( void )
|
||||
{
|
||||
//Circle(WP_HOME, 50); /* FIXME: radius should be defined elsewhere */
|
||||
{
|
||||
float alpha = atan2( estimator_y - waypoints[ WP_HOME ].y,
|
||||
estimator_x - waypoints[ WP_HOME ].x );
|
||||
float alpha_carrot = alpha + CARROT / ( -50 * estimator_hspeed_mod );
|
||||
fly_to_xy( waypoints[ WP_HOME ].x + cos( alpha_carrot )*fabs( 50 ),
|
||||
waypoints[ WP_HOME ].y + sin( alpha_carrot )*fabs( 50 ) );
|
||||
qdr = DegOfRad( M_PI / 2 - alpha_carrot );
|
||||
{
|
||||
_Pragma( "loopbounds min 0 max 2" )
|
||||
while ( qdr < 0 ) qdr += 360;
|
||||
_Pragma( "loopbounds min 0 max 2" )
|
||||
while ( qdr >= 360 ) qdr -= 360;
|
||||
}
|
||||
}
|
||||
|
||||
nav_pitch = 0.; /* Nominal speed */
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = GROUND_ALT + 50;
|
||||
compute_dist2_to_home();
|
||||
dist2_to_wp = dist2_to_home;
|
||||
}
|
||||
|
||||
void nav_update( void )
|
||||
{
|
||||
compute_dist2_to_home();
|
||||
|
||||
auto_nav();
|
||||
}
|
||||
|
||||
|
||||
void nav_init( void )
|
||||
{
|
||||
nav_block = 0;
|
||||
nav_stage = 0;
|
||||
}
|
||||
@ -0,0 +1,58 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: nav.h,v 1.1 2011-01-18 12:48:39 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef NAV_H
|
||||
#define NAV_H
|
||||
|
||||
#include <std.h>
|
||||
|
||||
struct point {
|
||||
float x;
|
||||
float y;
|
||||
float a;
|
||||
};
|
||||
extern float cur_pos_x;
|
||||
extern float cur_pos_y;
|
||||
extern uint8_t nav_stage, nav_block;
|
||||
extern float dist2_to_wp, dist2_to_home;
|
||||
|
||||
extern const int32_t nav_east0;
|
||||
extern const int32_t nav_north0;
|
||||
|
||||
extern const uint8_t nb_waypoint;
|
||||
extern struct point waypoints[];
|
||||
extern float desired_altitude, desired_x, desired_y;
|
||||
|
||||
extern uint16_t nav_desired_gaz;
|
||||
extern float nav_pitch, rc_pitch;
|
||||
extern bool_t too_far_from_home;
|
||||
extern uint16_t stage_time, block_time; /* s */
|
||||
extern float stage_time_ds; /* s */
|
||||
extern float nav_desired_roll;
|
||||
|
||||
void nav_update( void );
|
||||
void nav_home( void );
|
||||
void nav_init( void );
|
||||
|
||||
#endif /* NAV_H */
|
||||
@ -0,0 +1,139 @@
|
||||
/*
|
||||
$Id: pid.c,v 1.2 2011-01-21 11:52:44 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 pid.c
|
||||
\brief PID controllers (roll, pitch, climb, altitude, course).
|
||||
|
||||
*/
|
||||
|
||||
//#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "pid.h"
|
||||
|
||||
#include "autopilot.h"
|
||||
#include "infrared.h"
|
||||
#include "estimator.h"
|
||||
#include "nav.h"
|
||||
|
||||
float desired_roll = 0.;
|
||||
float desired_pitch = 0.;
|
||||
int16_t desired_gaz, desired_aileron, desired_elevator;
|
||||
float roll_pgain = ROLL_PGAIN;
|
||||
float pitch_pgain = PITCH_PGAIN;
|
||||
float pitch_of_roll = PITCH_OF_ROLL;
|
||||
|
||||
float pitch_of_vz_pgain = CLIMB_PITCH_OF_VZ_PGAIN;
|
||||
float pitch_of_vz = 0.;
|
||||
|
||||
|
||||
/** \brief Computes ::desired_aileron and ::desired_elevator from attitude estimation and expected attitude. */
|
||||
void roll_pitch_pid_run( void )
|
||||
{
|
||||
float err = estimator_phi - desired_roll;
|
||||
desired_aileron = TRIM_PPRZ( roll_pgain * err );
|
||||
if ( pitch_of_roll < 0. )
|
||||
pitch_of_roll = 0.;
|
||||
err = -( estimator_theta - desired_pitch - pitch_of_roll * fabs(
|
||||
estimator_phi ) );
|
||||
desired_elevator = TRIM_PPRZ( pitch_pgain * err );
|
||||
}
|
||||
|
||||
float course_pgain = COURSE_PGAIN;
|
||||
float desired_course = 0.;
|
||||
float max_roll = MAX_ROLL;
|
||||
|
||||
void course_pid_run( void )
|
||||
{
|
||||
float err = estimator_hspeed_dir - desired_course;
|
||||
//NORM_RAD_ANGLE(err);
|
||||
_Pragma( "loopbound min 0 max 1" )
|
||||
while ( err > M_PI ) err -= 2 * M_PI;
|
||||
_Pragma( "loopbound min 0 max 1" )
|
||||
while ( err < -M_PI ) err += 2 * M_PI;
|
||||
nav_desired_roll = course_pgain * err; // * fspeed / AIR_SPEED;
|
||||
if ( nav_desired_roll > max_roll )
|
||||
nav_desired_roll = max_roll;
|
||||
else
|
||||
if ( nav_desired_roll < -max_roll )
|
||||
nav_desired_roll = -max_roll;
|
||||
}
|
||||
|
||||
const float climb_pgain = CLIMB_PGAIN;
|
||||
const float climb_igain = CLIMB_IGAIN;
|
||||
float desired_climb = 0., pre_climb = 0.;
|
||||
static const float level_gaz = CLIMB_LEVEL_GAZ;
|
||||
float climb_sum_err = 0;
|
||||
|
||||
float climb_pitch_pgain = CLIMB_PITCH_PGAIN;
|
||||
float climb_pitch_igain = CLIMB_PITCH_IGAIN;
|
||||
float climb_pitch_sum_err = 0.;
|
||||
float max_pitch = MAX_PITCH;
|
||||
float min_pitch = MIN_PITCH;
|
||||
|
||||
|
||||
#define MAX_CLIMB_SUM_ERR 100
|
||||
#define MAX_PITCH_CLIMB_SUM_ERR 100
|
||||
|
||||
/** \brief Computes desired_gaz and desired_pitch from desired_climb */
|
||||
void climb_pid_run ( void )
|
||||
{
|
||||
float err = estimator_z_dot - desired_climb;
|
||||
float fgaz;
|
||||
if ( auto_pitch ) { /* gaz constant */
|
||||
desired_gaz = nav_desired_gaz;
|
||||
desired_pitch = climb_pitch_pgain * ( err + climb_pitch_igain *
|
||||
climb_pitch_sum_err );
|
||||
if ( desired_pitch > max_pitch )
|
||||
desired_pitch = max_pitch;
|
||||
if ( desired_pitch < min_pitch )
|
||||
desired_pitch = min_pitch;
|
||||
climb_pitch_sum_err += err;
|
||||
if ( climb_pitch_sum_err > MAX_PITCH_CLIMB_SUM_ERR )
|
||||
climb_pitch_sum_err = MAX_PITCH_CLIMB_SUM_ERR;
|
||||
if ( climb_pitch_sum_err < - MAX_PITCH_CLIMB_SUM_ERR )
|
||||
climb_pitch_sum_err = - MAX_PITCH_CLIMB_SUM_ERR;
|
||||
} else { /* pitch almost constant */
|
||||
/* pitch offset for climb */
|
||||
pitch_of_vz = ( desired_climb > 0 ) ? desired_climb * pitch_of_vz_pgain : 0.;
|
||||
fgaz = climb_pgain * ( err + climb_igain * climb_sum_err ) + CLIMB_LEVEL_GAZ +
|
||||
CLIMB_GAZ_OF_CLIMB * desired_climb;
|
||||
climb_sum_err += err;
|
||||
if ( climb_sum_err > MAX_CLIMB_SUM_ERR ) climb_sum_err = MAX_CLIMB_SUM_ERR;
|
||||
if ( climb_sum_err < - MAX_CLIMB_SUM_ERR ) climb_sum_err = - MAX_CLIMB_SUM_ERR;
|
||||
desired_gaz = TRIM_UPPRZ( fgaz * MAX_PPRZ );
|
||||
desired_pitch = nav_pitch + pitch_of_vz;
|
||||
}
|
||||
}
|
||||
|
||||
float altitude_pgain = ALTITUDE_PGAIN;
|
||||
|
||||
|
||||
void altitude_pid_run( void )
|
||||
{
|
||||
float err = estimator_z - desired_altitude;
|
||||
desired_climb = pre_climb + altitude_pgain * err;
|
||||
if ( desired_climb < -CLIMB_MAX ) desired_climb = -CLIMB_MAX;
|
||||
if ( desired_climb > CLIMB_MAX ) desired_climb = CLIMB_MAX;
|
||||
}
|
||||
@ -0,0 +1,61 @@
|
||||
/*
|
||||
Paparazzi mcu0 $Id: pid.h,v 1.1 2011-01-18 12:48:39 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef PID_H
|
||||
#define PID_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#define NORM_RAD_ANGLE(x) { \
|
||||
while (x > M_PI) x -= 2 * M_PI; \
|
||||
while (x < -M_PI) x += 2 * M_PI; \
|
||||
}
|
||||
|
||||
extern float desired_roll;
|
||||
extern float max_roll;
|
||||
extern float desired_pitch;
|
||||
extern float roll_pgain;
|
||||
extern float pitch_pgain;
|
||||
extern float pitch_of_roll;
|
||||
void roll_pitch_pid_run( void );
|
||||
|
||||
|
||||
extern float course_pgain;
|
||||
extern float desired_course;
|
||||
void course_pid_run( void );
|
||||
|
||||
|
||||
extern const float climb_pgain;
|
||||
extern const float climb_igain;
|
||||
extern float climb_sum_err;
|
||||
extern float desired_climb, pre_climb;
|
||||
extern int16_t desired_gaz, desired_aileron, desired_elevator;
|
||||
|
||||
extern float pitch_of_vz_pgain;
|
||||
extern float pitch_of_vz;
|
||||
|
||||
void climb_pid_run( void );
|
||||
void altitude_pid_run( void );
|
||||
|
||||
#endif /* PID_H */
|
||||
@ -0,0 +1,36 @@
|
||||
/*
|
||||
Paparazzi autopilot $Id: sirf.h,v 1.1 2011-01-18 12:48:39 moellmer Exp $
|
||||
|
||||
Copyright (C) 2004 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
/*
|
||||
SIRF protocol specific code
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#ifndef SIRF_H
|
||||
#define SIRF_H
|
||||
|
||||
#define GPS_FIX_VALID(gps_mode) (gps_mode & 1<<5)
|
||||
|
||||
#endif /* SIRF_H */
|
||||
@ -0,0 +1,43 @@
|
||||
#include <inttypes.h>
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
|
||||
|
||||
#include "spi.h"
|
||||
#include "autopilot.h"
|
||||
#include "link_fbw.h"
|
||||
#include "ad7714.h"
|
||||
|
||||
volatile uint8_t spi_cur_slave;
|
||||
uint8_t spi_nb_ovrn;
|
||||
|
||||
void spi_init( void )
|
||||
{
|
||||
/* Set MOSI and SCK output, all others input */
|
||||
SPI_DDR |= _BV( SPI_MOSI_PIN ) | _BV( SPI_SCK_PIN );
|
||||
|
||||
/* enable pull up for miso */
|
||||
// SPI_PORT |= _BV(SPI_MISO_PIN);
|
||||
|
||||
/* Set SS0 output */
|
||||
sbi( SPI_SS0_DDR, SPI_SS0_PIN );
|
||||
/* SS0 idles high (don't select slave yet)*/
|
||||
SPI_UNSELECT_SLAVE0();
|
||||
|
||||
/* Set SS1 output */
|
||||
sbi( SPI_SS1_DDR, SPI_SS1_PIN );
|
||||
/* SS1 idles high (don't select slave yet)*/
|
||||
SPI_UNSELECT_SLAVE1();
|
||||
|
||||
spi_cur_slave = SPI_NONE;
|
||||
}
|
||||
|
||||
|
||||
SIGNAL( SIG_SPI )
|
||||
{
|
||||
if ( spi_cur_slave == SPI_SLAVE0 )
|
||||
link_fbw_on_spi_it();
|
||||
else
|
||||
fatal_error_nb++;
|
||||
}
|
||||
@ -0,0 +1,74 @@
|
||||
#ifndef SPI_H
|
||||
#define SPI_H
|
||||
|
||||
//#include "link_autopilot.h"
|
||||
|
||||
#define SPI_SS0_PIN 0
|
||||
#define SPI_SS0_PORT PORTB
|
||||
#define SPI_SS0_DDR DDRB
|
||||
#define SPI_IT0_PIN 1
|
||||
#define SPI_IT0_PORT PORTD
|
||||
#define SPI_IT0_DDR DDRD
|
||||
|
||||
#define SPI_SS1_PIN 7
|
||||
#define SPI_SS1_PORT PORTE
|
||||
#define SPI_SS1_DDR DDRE
|
||||
#define SPI_IT1_PIN 6
|
||||
#define SPI_IT1_PORT PORTE
|
||||
#define SPI_IT1_DDR DDRE
|
||||
|
||||
#define SPI_SCK_PIN 1
|
||||
#define SPI_MOSI_PIN 2
|
||||
#define SPI_MISO_PIN 3
|
||||
#define SPI_PORT PORTB
|
||||
#define SPI_DDR DDRB
|
||||
|
||||
|
||||
|
||||
#define SPI_NONE 0
|
||||
#define SPI_SLAVE0 1
|
||||
#define SPI_SLAVE1 2
|
||||
|
||||
extern volatile uint8_t spi_cur_slave;
|
||||
extern uint8_t spi_nb_ovrn;
|
||||
|
||||
void spi_init( void );
|
||||
|
||||
#define SPI_START(_SPCR_VAL) { \
|
||||
uint8_t foo; \
|
||||
SPCR = _SPCR_VAL; \
|
||||
if (bit_is_set(SPSR, SPIF)) \
|
||||
foo = SPDR; \
|
||||
SPCR |= _BV(SPIE); \
|
||||
}
|
||||
|
||||
#define SPI_SELECT_SLAVE0() { \
|
||||
spi_cur_slave = SPI_SLAVE0; \
|
||||
cbi( SPI_SS0_PORT, SPI_SS0_PIN );\
|
||||
}
|
||||
|
||||
#define SPI_UNSELECT_SLAVE0() { \
|
||||
spi_cur_slave = SPI_NONE; \
|
||||
sbi( SPI_SS0_PORT, SPI_SS0_PIN );\
|
||||
}
|
||||
|
||||
#define SPI_SELECT_SLAVE1() { \
|
||||
spi_cur_slave = SPI_SLAVE1; \
|
||||
cbi( SPI_SS1_PORT, SPI_SS1_PIN );\
|
||||
}
|
||||
|
||||
#define SPI_UNSELECT_SLAVE1() { \
|
||||
spi_cur_slave = SPI_NONE; \
|
||||
sbi( SPI_SS1_PORT, SPI_SS1_PIN );\
|
||||
}
|
||||
|
||||
#define SPI_SEND(data) { \
|
||||
SPDR = data; \
|
||||
}
|
||||
|
||||
#define SPI_STOP() { \
|
||||
cbi(SPCR,SPIE); \
|
||||
cbi(SPCR, SPE); \
|
||||
}
|
||||
|
||||
#endif /* SPI_H */
|
||||
@ -0,0 +1,92 @@
|
||||
/*
|
||||
Paparazzi mcu0 timer functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef TIMER_H
|
||||
#define TIMER_H
|
||||
|
||||
#include "std.h"
|
||||
#include <arch/signal.h>
|
||||
#include <arch/io.h>
|
||||
|
||||
|
||||
/*
|
||||
Enable Timer1 (16-bit) running at Clk/1 for the global system
|
||||
clock. This will be used for computing the servo pulse widths,
|
||||
PPM decoding, etc.
|
||||
|
||||
Low frequency periodic tasks will be signaled by timer 0
|
||||
running at Clk/1024. For 16 Mhz clock, this will be every
|
||||
262144 microseconds, or 61 Hz.
|
||||
*/
|
||||
static inline void timer_init( void )
|
||||
{
|
||||
|
||||
/* Timer0: Modem clock is started in modem.h in ctc mode*/
|
||||
|
||||
/* Timer1 @ Clk/1: System clock, ppm and servos */
|
||||
TCCR1A = 0x00;
|
||||
TCCR1B = 0x01;
|
||||
|
||||
/* Timer2 @ Clk/1024: Periodic clock */
|
||||
TCCR2 = 0x05;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
Retrieve the current time from the global clock in Timer1,
|
||||
disabling interrupts to avoid stomping on the TEMP register.
|
||||
If interrupts are already off, the non_atomic form can be used.
|
||||
*/
|
||||
static inline uint16_t
|
||||
timer_now( void )
|
||||
{
|
||||
return TCNT1;
|
||||
}
|
||||
|
||||
static inline uint16_t
|
||||
timer_now_non_atomic( void )
|
||||
{
|
||||
return TCNT1L;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
Periodic tasks occur when Timer2 overflows. Check and unset
|
||||
the overflow bit. We cycle through four possible periodic states,
|
||||
so each state occurs every 30 Hz.
|
||||
*/
|
||||
static inline bool_t
|
||||
timer_periodic( void )
|
||||
{
|
||||
if ( !bit_is_set( TIFR, TOV2 ) )
|
||||
return FALSE;
|
||||
|
||||
TIFR = 1 << TOV2;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,149 @@
|
||||
/*
|
||||
Paparazzi $Id: uart.c,v 1.3 2011-01-25 09:40:37 plazar Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
#include <arch/io.h>
|
||||
#include "uart.h"
|
||||
#include "std.h"
|
||||
|
||||
#define TX_BUF_SIZE 256
|
||||
static uint8_t tx_head0; /* next free in buf */
|
||||
static volatile uint8_t tx_tail0; /* next char to send */
|
||||
static uint8_t tx_buf0[ TX_BUF_SIZE ];
|
||||
|
||||
static uint8_t tx_head1; /* next free in buf */
|
||||
static volatile uint8_t tx_tail1; /* next char to send */
|
||||
static uint8_t tx_buf1[ TX_BUF_SIZE ];
|
||||
|
||||
void uart0_transmit( const unsigned char data )
|
||||
{
|
||||
if ( UCSR0B & _BV( TXCIE ) ) {
|
||||
/* we are waiting for the last char to be sent : buffering */
|
||||
if ( tx_tail0 == tx_head0 + 1 ) { /* BUF_SIZE = 256 */
|
||||
/* Buffer is full (almost, but tx_head = tx_tail means "empty" */
|
||||
return;
|
||||
}
|
||||
tx_buf0[ tx_head0 ] = data;
|
||||
tx_head0++; /* BUF_SIZE = 256 */
|
||||
} else { /* Channel is free: just send */
|
||||
UDR0 = data;
|
||||
sbi( UCSR0B, TXCIE );
|
||||
}
|
||||
}
|
||||
|
||||
void uart1_transmit( const unsigned char data )
|
||||
{
|
||||
if ( UCSR1B & _BV( TXCIE ) ) {
|
||||
/* we are waiting for the last char to be sent : buffering */
|
||||
if ( tx_tail1 == tx_head1 + 1 ) { /* BUF_SIZE = 256 */
|
||||
/* Buffer is full (almost, but tx_head = tx_tail means "empty" */
|
||||
return;
|
||||
}
|
||||
tx_buf1[ tx_head1 ] = data;
|
||||
tx_head1++; /* BUF_SIZE = 256 */
|
||||
} else { /* Channel is free: just send */
|
||||
UDR1 = data;
|
||||
sbi( UCSR1B, TXCIE );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void uart0_print_string( const uint8_t *s )
|
||||
{
|
||||
uint8_t i = 0;
|
||||
_Pragma( "loopbound min 0 max 0" )
|
||||
while ( s[ i ] ) {
|
||||
uart0_transmit( s[ i ] );
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
void uart0_print_hex( const uint8_t c )
|
||||
{
|
||||
const uint8_t hex[ 16 ] = { '0', '1', '2', '3', '4', '5', '6', '7',
|
||||
'8', '9', 'A', 'B', 'C', 'D', 'E', 'F'
|
||||
};
|
||||
uint8_t high = ( c & 0xF0 ) >> 4;
|
||||
uint8_t low = c & 0x0F;
|
||||
uart0_transmit( hex[ high ] );
|
||||
uart0_transmit( hex[ low ] );
|
||||
}
|
||||
|
||||
|
||||
SIGNAL( SIG_UART0_TRANS )
|
||||
{
|
||||
if ( tx_head0 == tx_tail0 ) {
|
||||
/* Nothing more to send */
|
||||
cbi( UCSR0B, TXCIE ); /* disable interrupt */
|
||||
} else {
|
||||
UDR0 = tx_buf0[ tx_tail0 ];
|
||||
tx_tail0++; /* warning tx_buf_len is 256 */
|
||||
}
|
||||
}
|
||||
|
||||
SIGNAL( SIG_UART1_TRANS )
|
||||
{
|
||||
if ( tx_head1 == tx_tail1 ) {
|
||||
/* Nothing more to send */
|
||||
cbi( UCSR1B, TXCIE ); /* disable interrupt */
|
||||
} else {
|
||||
UDR1 = tx_buf1[ tx_tail1 ];
|
||||
tx_tail1++; /* warning tx_buf_len is 256 */
|
||||
}
|
||||
}
|
||||
|
||||
void uart0_init( void )
|
||||
{
|
||||
/* Baudrate is 38.4k */
|
||||
UBRR0H = 0;
|
||||
UBRR0L = 25; // 38.4
|
||||
// UBRR0L = 103; //9600
|
||||
/* single speed */
|
||||
UCSR0A = 0;
|
||||
/* Enable receiver and transmitter */
|
||||
UCSR0B = _BV( RXEN ) | _BV( TXEN );
|
||||
/* Set frame format: 8data, 1stop bit */
|
||||
UCSR0C = _BV( UCSZ1 ) | _BV( UCSZ0 );
|
||||
/* Enable uart receive interrupt */
|
||||
sbi( UCSR0B, RXCIE );
|
||||
}
|
||||
|
||||
void uart1_init( void )
|
||||
{
|
||||
/* Baudrate is 38.4k */
|
||||
UBRR1H = 0;
|
||||
UBRR1L = 25; // 38.4
|
||||
// UBRR1L = 103; //9600
|
||||
|
||||
|
||||
/* single speed */
|
||||
UCSR1A = 0;
|
||||
/* Enable receiver and transmitter */
|
||||
UCSR1B = _BV( RXEN ) | _BV( TXEN );
|
||||
/* Set frame format: 8data, 1stop bit */
|
||||
UCSR1C = _BV( UCSZ1 ) | _BV( UCSZ0 );
|
||||
/* Enable uart receive interrupt */
|
||||
sbi( UCSR1B, RXCIE );
|
||||
}
|
||||
|
||||
@ -0,0 +1,48 @@
|
||||
/*
|
||||
Paparazzi $Id: uart.h,v 1.2 2011-01-18 14:55:52 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
#ifndef _UART_H_
|
||||
#define _UART_H_
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
extern void uart0_init( void );
|
||||
extern void uart1_init( void );
|
||||
|
||||
extern void uart0_print_string( const uint8_t * );
|
||||
extern void uart0_print_hex( const uint8_t );
|
||||
void uart0_transmit( const uint8_t );
|
||||
void uart1_transmit( const uint8_t );
|
||||
|
||||
#define ReceiveUart0(cb) \
|
||||
SIGNAL( SIG_UART0_RECV ) { \
|
||||
uint8_t c = UDR0; \
|
||||
cb(c); \
|
||||
}
|
||||
#define ReceiveUart1(cb) \
|
||||
SIGNAL( SIG_UART1_RECV ) { \
|
||||
uint8_t c = UDR1; \
|
||||
cb(c); \
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,38 @@
|
||||
/*
|
||||
Paparazzi autopilot $Id: ubx.h,v 1.1 2011-01-18 12:48:39 moellmer Exp $
|
||||
|
||||
Copyright (C) 2004 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
/*
|
||||
UBX protocol specific code
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#ifndef UBX_H
|
||||
#define UBX_H
|
||||
|
||||
#define GPS_FIX_VALID(gps_mode) (gps_mode == 3)
|
||||
extern const int32_t utm_east0;
|
||||
extern const int32_t utm_north0;
|
||||
|
||||
#endif /* UBX_H */
|
||||
@ -0,0 +1,121 @@
|
||||
/*
|
||||
Paparazzi fly by wire adc functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
//// ADC3 MVSUP
|
||||
//// ADC6 MVSERVO
|
||||
|
||||
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
#include <arch/io.h>
|
||||
#include "adc_fbw.h"
|
||||
#include "std.h"
|
||||
|
||||
|
||||
#if defined CTL_BRD_V1_2 || defined CTL_BRD_V1_2_1
|
||||
|
||||
#define VOLTAGE_TIME 0x07
|
||||
#define ANALOG_PORT PORTC
|
||||
#define ANALOG_PORT_DIR DDRC
|
||||
|
||||
|
||||
//
|
||||
#define ANALOG_VREF _BV(REFS0) | _BV(REFS1)
|
||||
|
||||
uint16_t adc_samples[ NB_ADC ];
|
||||
|
||||
static struct adc_buf *buffers[ NB_ADC ];
|
||||
|
||||
void fbw_adc_buf_channel( uint8_t adc_channel, struct adc_buf *s )
|
||||
{
|
||||
buffers[ adc_channel ] = s;
|
||||
}
|
||||
|
||||
void
|
||||
fbw_adc_init( void )
|
||||
{
|
||||
uint8_t i;
|
||||
/* Ensure that our port is for input with no pull-ups */
|
||||
ANALOG_PORT = 0x00;
|
||||
ANALOG_PORT_DIR = 0x00;
|
||||
|
||||
/* Select our external voltage ref */
|
||||
ADMUX = ANALOG_VREF;
|
||||
|
||||
/* Select out clock, turn on the ADC interrupt and start conversion */
|
||||
ADCSRA = 0
|
||||
| VOLTAGE_TIME
|
||||
| _BV( ADEN )
|
||||
| _BV( ADIE )
|
||||
| _BV( ADSC );
|
||||
|
||||
/* Init to 0 (usefull ?) */
|
||||
_Pragma( "loopbound min 8 max 8" )
|
||||
for ( i = 0; i < NB_ADC; i++ )
|
||||
buffers[ i ] = ( struct adc_buf * )0;
|
||||
}
|
||||
|
||||
/**
|
||||
Called when the voltage conversion is finished
|
||||
|
||||
8.913kHz on mega128@16MHz 1kHz/channel ??
|
||||
*/
|
||||
|
||||
SIGNAL( SIG_ADC )
|
||||
{
|
||||
uint8_t adc_input = ADMUX & 0x7;
|
||||
struct adc_buf *buf = buffers[ adc_input ];
|
||||
uint16_t adc_value = ADCW;
|
||||
/* Store result */
|
||||
adc_samples[ adc_input ] = adc_value;
|
||||
|
||||
if ( buf ) {
|
||||
uint8_t new_head = buf->head + 1;
|
||||
if ( new_head >= AV_NB_SAMPLE ) new_head = 0;
|
||||
buf->sum -= buf->values[ new_head ];
|
||||
buf->values[ new_head ] = adc_value;
|
||||
buf->sum += adc_value;
|
||||
buf->head = new_head;
|
||||
}
|
||||
|
||||
/* Find the next input */
|
||||
adc_input++;
|
||||
if ( adc_input == 4 )
|
||||
adc_input = 6; // ADC 4 and 5 for i2c
|
||||
if ( adc_input >= 8 ) {
|
||||
adc_input = 0;
|
||||
#ifdef CTL_BRD_V1_2
|
||||
adc_input = 1; // WARNING ADC0 is for rservo driver reset on v1.2.0
|
||||
#endif /* CTL_BRD_V1_2 */
|
||||
}
|
||||
/* Select it */
|
||||
ADMUX = adc_input | ANALOG_VREF;
|
||||
/* Restart the conversion */
|
||||
sbi( ADCSR, ADSC );
|
||||
}
|
||||
|
||||
#endif /* CTL_BRD_V1_2 || CTL_BRD_V1_2_1 */
|
||||
@ -0,0 +1,62 @@
|
||||
/*
|
||||
Paparazzi fly by wire adc functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef _ADC_H_
|
||||
#define _ADC_H_
|
||||
|
||||
#include "airframe.h"
|
||||
|
||||
#if defined CTL_BRD_V1_2 || defined CTL_BRD_V1_2_1
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
|
||||
#define NB_ADC 8
|
||||
|
||||
/* Array containing the last measured value */
|
||||
extern uint16_t adc_samples[ NB_ADC ];
|
||||
|
||||
void adc_init( void );
|
||||
|
||||
#define AV_NB_SAMPLE 0x20
|
||||
|
||||
struct adc_buf {
|
||||
uint16_t sum;
|
||||
uint16_t values[ AV_NB_SAMPLE ];
|
||||
uint8_t head;
|
||||
};
|
||||
|
||||
/* Facility to store last values in a circular buffer for a specific
|
||||
channel: allocate a (struct adc_buf) and register it with the following
|
||||
function */
|
||||
void adc_buf_channel( uint8_t adc_channel, struct adc_buf *s );
|
||||
void fbw_adc_init( void );
|
||||
void fbw_adc_buf_channel( uint8_t adc_channel, struct adc_buf *s );
|
||||
|
||||
#endif /* CTL_BRD_V1_2 || CTL_BRD_V1_2 */
|
||||
|
||||
#endif /* _ADC_H_ */
|
||||
@ -0,0 +1,69 @@
|
||||
/* $Id: link_autopilot.h,v 1.1 2011-01-18 12:42:42 moellmer Exp $
|
||||
|
||||
(c) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef LINK_AUTOPILOT_H
|
||||
#define LINK_AUTOPILOT_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#include "std.h"
|
||||
#include "radio.h"
|
||||
#include "airframe.h"
|
||||
|
||||
/*
|
||||
System clock in MHz.
|
||||
*/
|
||||
#define CLOCK 16
|
||||
|
||||
typedef int16_t pprz_t; // type of commands
|
||||
|
||||
/* !!!!!!!!!!!!!!!!!!! Value used in gen_airframe.ml !!!!!!!!!!!!!!!!! */
|
||||
#define MAX_PPRZ (600 * CLOCK)
|
||||
#define MIN_PPRZ -MAX_PPRZ
|
||||
|
||||
struct inter_mcu_msg {
|
||||
int16_t channels[ RADIO_CTL_NB ];
|
||||
uint8_t ppm_cpt;
|
||||
uint8_t status;
|
||||
uint8_t nb_err;
|
||||
uint8_t vsupply; /* 1e-1 V */
|
||||
};
|
||||
|
||||
// Status bits from FBW to AUTOPILOT
|
||||
#define STATUS_RADIO_OK 0
|
||||
#define RADIO_REALLY_LOST 1
|
||||
#define AVERAGED_CHANNELS_SENT 2
|
||||
#define MASK_FBW_CHANGED 0x3
|
||||
|
||||
// Statut bits from AUTOPILOT to FBW
|
||||
#define STATUS_AUTO_OK 0
|
||||
|
||||
#define FRAME_LENGTH (sizeof(struct inter_mcu_msg)+1)
|
||||
|
||||
#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
|
||||
|
||||
void test_ppm_task( void );
|
||||
void check_mega128_values_task( void );
|
||||
void send_data_to_autopilot_task( void );
|
||||
void check_failsafe_task( void );
|
||||
#endif // LINK_AUTOPILOT_H
|
||||
@ -0,0 +1,219 @@
|
||||
/*
|
||||
Paparazzi $Id: main.c,v 1.4 2011-01-25 10:42:14 plazar Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <inttypes.h>
|
||||
#include <io.h>
|
||||
#include <signal.h>
|
||||
#include <interrupt.h>
|
||||
|
||||
#include "timer.h"
|
||||
#include "servo.h"
|
||||
#include "ppm.h"
|
||||
#include "spi.h"
|
||||
#include "link_autopilot.h"
|
||||
#include "radio.h"
|
||||
|
||||
|
||||
#include "uart.h"
|
||||
|
||||
|
||||
#ifndef CTL_BRD_V1_1
|
||||
#include "adc_fbw.h"
|
||||
struct adc_buf vsupply_adc_buf;
|
||||
struct adc_buf vservos_adc_buf;
|
||||
#endif
|
||||
|
||||
uint8_t mode;
|
||||
static uint8_t time_since_last_mega128;
|
||||
static uint16_t time_since_last_ppm;
|
||||
bool_t radio_ok, mega128_ok, radio_really_lost;
|
||||
|
||||
static const pprz_t failsafe[ ] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||
|
||||
static uint8_t ppm_cpt, last_ppm_cpt;
|
||||
|
||||
#define STALLED_TIME 30 // 500ms with a 60Hz timer
|
||||
#define REALLY_STALLED_TIME 300 // 5s with a 60Hz timer
|
||||
|
||||
|
||||
/* static inline void status_transmit( void ) { */
|
||||
/* uint8_t i; */
|
||||
/* uart_transmit(7); */
|
||||
/* uart_transmit(7); */
|
||||
/* for (i=0; i<sizeof(struct inter_mcu_msg); i++) */
|
||||
/* uart_transmit(((uint8_t*)&to_mega128)[ i ]); */
|
||||
/* uart_transmit('\n'); */
|
||||
|
||||
/* uart_transmit(7); */
|
||||
/* uart_transmit(7); */
|
||||
/* uint8_t i; */
|
||||
/* for(i = 0; i < RADIO_CTL_NB; i++) { */
|
||||
/* extern uint16_t ppm_pulses[ ]; */
|
||||
/* uart_transmit(ppm_pulses[ i ]>>8); */
|
||||
/* uart_transmit(ppm_pulses[ i ] & 0xff); */
|
||||
/* } */
|
||||
/* uart_transmit('\n'); */
|
||||
/* } */
|
||||
|
||||
|
||||
/* Prepare data to be sent to mcu0 */
|
||||
static void to_autopilot_from_last_radio ( void )
|
||||
{
|
||||
uint8_t i;
|
||||
_Pragma( "loopbound min 9 max 9" )
|
||||
for ( i = 0; i < RADIO_CTL_NB; i++ )
|
||||
to_mega128.channels[ i ] = last_radio[ i ];
|
||||
to_mega128.status = ( radio_ok ? _BV( STATUS_RADIO_OK ) : 0 );
|
||||
to_mega128.status |= ( radio_really_lost ? _BV( RADIO_REALLY_LOST ) : 0 );
|
||||
if ( last_radio_contains_avg_channels ) {
|
||||
to_mega128.status |= _BV( AVERAGED_CHANNELS_SENT );
|
||||
last_radio_contains_avg_channels = FALSE;
|
||||
}
|
||||
to_mega128.ppm_cpt = last_ppm_cpt;
|
||||
#ifndef CTL_BRD_V1_1
|
||||
to_mega128.vsupply = VoltageOfAdc( vsupply_adc_buf.sum / AV_NB_SAMPLE ) * 10;
|
||||
#else
|
||||
to_mega128.vsupply = 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
void _Pragma( "entrypoint" ) send_data_to_autopilot_task( void )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
if ( !SpiIsSelected() && spi_was_interrupted ) {
|
||||
spi_was_interrupted = FALSE;
|
||||
to_autopilot_from_last_radio();
|
||||
spi_reset();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef PAPABENCH_SINGLE
|
||||
extern uint8_t _1Hz;
|
||||
extern uint8_t _20Hz;
|
||||
#else
|
||||
static uint8_t _1Hz;
|
||||
static uint8_t _20Hz;
|
||||
#endif
|
||||
|
||||
void fbw_init( void )
|
||||
{
|
||||
uart_init_tx();
|
||||
uart_print_string( "FBW Booting $Id: main.c,v 1.4 2011-01-25 10:42:14 plazar Exp $\n" );
|
||||
|
||||
#ifndef CTL_BRD_V1_1
|
||||
fbw_adc_init();
|
||||
fbw_adc_buf_channel( 3, &vsupply_adc_buf );
|
||||
fbw_adc_buf_channel( 6, &vservos_adc_buf );
|
||||
#endif
|
||||
timer_init();
|
||||
servo_init();
|
||||
ppm_init();
|
||||
fbw_spi_init();
|
||||
//sei(); //FN
|
||||
}
|
||||
|
||||
void fbw_schedule( void )
|
||||
{
|
||||
if ( time_since_last_mega128 < STALLED_TIME )
|
||||
time_since_last_mega128++;
|
||||
if ( time_since_last_ppm < REALLY_STALLED_TIME )
|
||||
time_since_last_ppm++;
|
||||
if ( _1Hz == 0 ) {
|
||||
last_ppm_cpt = ppm_cpt;
|
||||
ppm_cpt = 0;
|
||||
}
|
||||
test_ppm_task();
|
||||
check_mega128_values_task();
|
||||
send_data_to_autopilot_task();
|
||||
check_failsafe_task();
|
||||
if ( _20Hz >= 3 )
|
||||
servo_transmit();
|
||||
}
|
||||
|
||||
#ifndef PAPABENCH_SINGLE
|
||||
int main( void )
|
||||
{
|
||||
fbw_init();
|
||||
#ifndef NO_MAINLOOP
|
||||
while ( 1 ) {
|
||||
#endif
|
||||
fbw_schedule();
|
||||
if ( timer_periodic() ) {
|
||||
_1Hz++;
|
||||
_20Hz++;
|
||||
if ( _1Hz >= 60 )
|
||||
_1Hz = 0;
|
||||
if ( _20Hz >= 3 )
|
||||
_20Hz = 0;
|
||||
}
|
||||
#ifndef NO_MAINLOOP
|
||||
}
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
void _Pragma( "entrypoint" ) test_ppm_task( void )
|
||||
{
|
||||
if ( ppm_valid ) {
|
||||
ppm_valid = FALSE;
|
||||
ppm_cpt++;
|
||||
radio_ok = TRUE;
|
||||
radio_really_lost = FALSE;
|
||||
time_since_last_ppm = 0;
|
||||
last_radio_from_ppm();
|
||||
if ( last_radio_contains_avg_channels )
|
||||
mode = MODE_OF_PPRZ( last_radio[ RADIO_MODE ] );
|
||||
if ( mode == MODE_MANUAL )
|
||||
servo_set( last_radio );
|
||||
} else
|
||||
if ( mode == MODE_MANUAL && radio_really_lost )
|
||||
mode = MODE_AUTO;
|
||||
if ( time_since_last_ppm >= STALLED_TIME )
|
||||
radio_ok = FALSE;
|
||||
if ( time_since_last_ppm >= REALLY_STALLED_TIME )
|
||||
radio_really_lost = TRUE;
|
||||
}
|
||||
void _Pragma( "entrypoint" ) check_failsafe_task( void )
|
||||
{
|
||||
if ( ( mode == MODE_MANUAL && !radio_ok ) ||
|
||||
( mode == MODE_AUTO && !mega128_ok ) )
|
||||
servo_set( failsafe );
|
||||
}
|
||||
void _Pragma( "entrypoint" ) check_mega128_values_task( void )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
if ( !SpiIsSelected() && spi_was_interrupted ) {
|
||||
if ( mega128_receive_valid ) {
|
||||
time_since_last_mega128 = 0;
|
||||
mega128_ok = TRUE;
|
||||
if ( mode == MODE_AUTO )
|
||||
servo_set( from_mega128.channels );
|
||||
}
|
||||
}
|
||||
if ( time_since_last_mega128 == STALLED_TIME )
|
||||
mega128_ok = FALSE;
|
||||
#endif
|
||||
}
|
||||
@ -0,0 +1,107 @@
|
||||
/* $Id: ppm.c,v 1.1 2011-01-18 12:42:42 moellmer Exp $
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
(c) 2003 Trammell Hudson <hudson@rotomotion.com>
|
||||
(c) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 "radio.h"
|
||||
#include "ppm.h"
|
||||
|
||||
#define AVERAGING_PERIOD (PPM_FREQ/4)
|
||||
|
||||
/*
|
||||
Pulse width is computed as the difference between now and the
|
||||
previous pulse. If no pulse has been received between then and
|
||||
now, the time of the last pulse will be equal to the last pulse
|
||||
we measured. Unfortunately, the Input Capture Flag (ICF1) will
|
||||
not be set since the interrupt routine disables it.
|
||||
|
||||
Sync pulses are timed with Timer2, which runs at Clk/1024. This
|
||||
is slow enough at both 4 and 8 Mhz to measure the lengthy (10ms
|
||||
or longer) pulse.
|
||||
|
||||
Otherwise, compute the pulse width with the 16-bit timer1,
|
||||
push the pulse width onto the stack and increment the
|
||||
pulse counter until we have received eight pulses.
|
||||
*/
|
||||
|
||||
uint16_t ppm_pulses[ PPM_NB_PULSES ];
|
||||
pprz_t last_radio[ PPM_NB_PULSES ];
|
||||
pprz_t avg_last_radio[ PPM_NB_PULSES ];
|
||||
bool_t last_radio_contains_avg_channels = FALSE;
|
||||
volatile bool_t ppm_valid;
|
||||
|
||||
/* MC3030, Trame PPM7: 25ms, 10.4 au neutre,
|
||||
sync pulse = 16.2ms with low value on every channels */
|
||||
|
||||
|
||||
|
||||
|
||||
#define RestartPpmCycle() { state = 0; sync_start = TCNT2; return; }
|
||||
|
||||
#ifdef PAPABENCH_SINGLE
|
||||
SIGNAL( SIG_SPM_READY )
|
||||
#else
|
||||
SIGNAL( SIG_INPUT_CAPTURE1 )
|
||||
#endif
|
||||
{
|
||||
static uint16_t last;
|
||||
uint16_t this;
|
||||
uint16_t width;
|
||||
static uint8_t state;
|
||||
static uint8_t sync_start;
|
||||
|
||||
this = ICR1;
|
||||
width = this - last;
|
||||
last = this;
|
||||
|
||||
if ( state == 0 ) {
|
||||
uint8_t end = TCNT2;
|
||||
uint8_t diff = ( end - sync_start );
|
||||
sync_start = end;
|
||||
|
||||
/* The frame period of the mc3030 seems to be 25ms.
|
||||
One pulse lasts from 1.05ms to 2.150ms.
|
||||
Sync pulse is at least 7ms : (7000*CLOCK)/1024 = 109
|
||||
*/
|
||||
if ( diff > ( uint8_t )( ( ( uint32_t )( 7000ul * CLOCK ) ) / 1024ul ) )
|
||||
state = 1;
|
||||
} else {
|
||||
/* Read a data pulses */
|
||||
if ( width < 700ul * CLOCK || width > 2300ul * CLOCK )
|
||||
RestartPpmCycle();
|
||||
ppm_pulses[ state - 1 ] = width;
|
||||
|
||||
if ( state >= PPM_NB_PULSES ) {
|
||||
ppm_valid = 1;
|
||||
RestartPpmCycle();
|
||||
} else
|
||||
state++;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
/* Copy from the ppm receiving buffer to the buffer sent to mcu0 */
|
||||
void last_radio_from_ppm()
|
||||
{
|
||||
LastRadioFromPpm()
|
||||
}
|
||||
@ -0,0 +1,102 @@
|
||||
/* $Id: ppm.h,v 1.2 2011-01-18 14:55:53 moellmer Exp $
|
||||
|
||||
Decoder for the trainer ports or hacked receivers for both
|
||||
Futaba and JR formats. The ppm_valid flag is set whenever
|
||||
a valid frame is received.
|
||||
|
||||
Pulse widths are stored as unscaled 16-bit values in ppm_pulses[ ].
|
||||
If you require actual microsecond values, divide by CLOCK.
|
||||
For an 8 Mhz clock and typical servo values, these will range
|
||||
from 0x1F00 to 0x4000.
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
(c) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
(c) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef PPM_H
|
||||
#define PPM_H
|
||||
|
||||
|
||||
/**
|
||||
Receiver types
|
||||
*/
|
||||
#define RXFUTABA 0
|
||||
#define RXJR 1
|
||||
|
||||
#define PPM_RX_TYPE RXFUTABA
|
||||
#define PPM_FREQ 40 // 25ms
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <signal.h>
|
||||
|
||||
#include "timer.h"
|
||||
#include "link_autopilot.h"
|
||||
|
||||
#define PpmOfUs(x) ((x)*CLOCK)
|
||||
|
||||
#define PPM_DDR DDRB
|
||||
#define PPM_PORT PORTB
|
||||
#define PPM_PIN PB0
|
||||
|
||||
/*
|
||||
PPM pulses are falling edge clocked on the ICP, which records
|
||||
the state of the global clock. We do not use any noise
|
||||
canceling features.
|
||||
|
||||
JR might be rising edge clocked; set that as an option
|
||||
*/
|
||||
static inline void
|
||||
ppm_init( void )
|
||||
{
|
||||
#if PPM_RX_TYPE == RXFUTABA
|
||||
cbi( TCCR1B, ICES1 );
|
||||
#elif PPM_RX_TYPE == RXJR
|
||||
sbi( TCCR1B, ICES1 );
|
||||
#else
|
||||
# error "ppm.h: Unknown receiver type in PPM_RX_TYPE"
|
||||
#endif
|
||||
|
||||
/* No noise cancelation */
|
||||
sbi( TCCR1B, ICNC1 );
|
||||
|
||||
/* Set ICP to input, no internal pull up */
|
||||
cbi( PPM_DDR, PPM_PIN );
|
||||
|
||||
/* Enable interrupt on input capture */
|
||||
sbi( TIMSK, TICIE1 );
|
||||
}
|
||||
|
||||
#define PPM_NB_PULSES RADIO_CTL_NB
|
||||
|
||||
extern volatile bool_t ppm_valid;
|
||||
extern pprz_t last_radio[ PPM_NB_PULSES ];
|
||||
extern bool_t last_radio_contains_avg_channels;
|
||||
|
||||
|
||||
#define MODE_MANUAL 0
|
||||
#define MODE_AUTO 1
|
||||
|
||||
#define MODE_OF_PPRZ(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ? MODE_MANUAL : MODE_AUTO)
|
||||
|
||||
extern void last_radio_from_ppm( void );
|
||||
#endif
|
||||
@ -0,0 +1,200 @@
|
||||
/* $Id: servo.c,v 1.4 2011-01-25 10:05:41 plazar Exp $
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
(c) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
(c) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include "servo.h"
|
||||
#include "link_autopilot.h"
|
||||
|
||||
#include "airframe.h"
|
||||
|
||||
#include "uart.h"
|
||||
|
||||
|
||||
/*
|
||||
Paparazzi boards have one 4017 servo driver.
|
||||
It is driven by OCR1A (PB1) with reset on PORTD5.
|
||||
*/
|
||||
#define _4017_NB_CHANNELS 10
|
||||
|
||||
#ifdef CTL_BRD_V1_1
|
||||
#define _4017_RESET_PORT PORTD
|
||||
#define _4017_RESET_DDR DDRD
|
||||
#define _4017_RESET_PIN 5
|
||||
#endif /* CTL_BRD_V1_1 */
|
||||
|
||||
#ifdef CTL_BRD_V1_2
|
||||
#define _4017_RESET_PORT PORTC
|
||||
#define _4017_RESET_DDR DDRC
|
||||
#define _4017_RESET_PIN 0
|
||||
#endif /* CTL_BRD_V1_2 */
|
||||
|
||||
#ifdef CTL_BRD_V1_2_1
|
||||
#define _4017_RESET_PORT PORTD
|
||||
#define _4017_RESET_DDR DDRD
|
||||
#define _4017_RESET_PIN 7
|
||||
#endif /* CTL_BRD_V1_2 */
|
||||
|
||||
#define _4017_CLOCK_PORT PORTB
|
||||
#define _4017_CLOCK_DDR DDRB
|
||||
#define _4017_CLOCK_PIN PB1
|
||||
|
||||
#define SERVO_OCR OCR1A
|
||||
#define SERVO_ENABLE OCIE1A
|
||||
#define SERVO_FLAG OCF1A
|
||||
#define SERVO_FORCE FOC1A
|
||||
#define SERVO_COM0 COM1A0
|
||||
#define SERVO_COM1 COM1A1
|
||||
|
||||
/* Following macro is required since the compiler does not solve at
|
||||
compile-time indexation in a known array with a known index */
|
||||
#define SERVO_NEUTRAL_(i) SERVOS_NEUTRALS_ ## i
|
||||
#define SERVO_NEUTRAL(i) (SERVO_NEUTRAL_(i)*CLOCK)
|
||||
|
||||
#define SERVO_NEUTRAL_I(i) (((int[ ])SERVOS_NEUTRALS[ i ])*CLOCK)
|
||||
#define SERVO_MIN_I(i) (((int[ ])SERVOS_MINS[ i ])*CLOCK)
|
||||
|
||||
#define SERVO_MIN (SERVO_MIN_US*CLOCK)
|
||||
#define SERVO_MAX (SERVO_MAX_US*CLOCK)
|
||||
#define ChopServo(x) ((x) < SERVO_MIN ? SERVO_MIN : ((x) > SERVO_MAX ? SERVO_MAX : (x)))
|
||||
|
||||
/* holds the servo pulses width in clock ticks */
|
||||
static uint16_t servo_widths[ _4017_NB_CHANNELS ];
|
||||
|
||||
/*
|
||||
We use the output compare registers to generate our servo pulses.
|
||||
These should be connected to a decade counter that routes the
|
||||
pulses to the appropriate servo.
|
||||
|
||||
Initialization involves:
|
||||
|
||||
- Reseting the decade counters
|
||||
- Writing the first pulse width to the counters
|
||||
- Setting output compare to set the clock line by calling servo_enable()
|
||||
- Bringing down the reset lines
|
||||
|
||||
Ideally, you can use two decade counters to drive 20 servos.
|
||||
*/
|
||||
void
|
||||
servo_init( void )
|
||||
{
|
||||
uint8_t i;
|
||||
|
||||
/* Configure the reset and clock lines */
|
||||
_4017_RESET_DDR |= _BV( _4017_RESET_PIN );
|
||||
_4017_CLOCK_DDR |= _BV( _4017_CLOCK_PIN );
|
||||
|
||||
/* Reset the decade counter */
|
||||
sbi( _4017_RESET_PORT, _4017_RESET_PIN );
|
||||
|
||||
/* Lower the regular servo line */
|
||||
cbi( _4017_CLOCK_PORT, _4017_CLOCK_PIN );
|
||||
|
||||
/* Set all servos at their midpoints */
|
||||
_Pragma( "loopbound min 10 max 10" )
|
||||
for ( i = 0 ; i < _4017_NB_CHANNELS ; i++ )
|
||||
// servo_widths[ i ] = SERVO_MIN;
|
||||
servo_widths[ i ] = ( SERVO_MIN + SERVO_MAX ) / 2;
|
||||
|
||||
/* Set servos to go off some long time from now */
|
||||
SERVO_OCR = 32768ul;
|
||||
|
||||
/*
|
||||
Configure output compare to toggle the output bits.
|
||||
*/
|
||||
TCCR1A |= _BV( SERVO_COM0 );
|
||||
|
||||
/* Clear the interrupt flags in case they are set */
|
||||
TIFR = _BV( SERVO_FLAG );
|
||||
|
||||
/* Unassert the decade counter reset to start it running */
|
||||
cbi( _4017_RESET_PORT, _4017_RESET_PIN );
|
||||
|
||||
/* Enable our output compare interrupts */
|
||||
TIMSK |= _BV( SERVO_ENABLE );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
Interrupt routine
|
||||
|
||||
write the next pulse width to OCR register and
|
||||
assert the servo signal. It will be cleared by
|
||||
the following compare match.
|
||||
*/
|
||||
SIGNAL( SIG_OUTPUT_COMPARE1A )
|
||||
{
|
||||
static uint8_t servo = 0;
|
||||
uint16_t width;
|
||||
|
||||
if ( servo >= _4017_NB_CHANNELS ) {
|
||||
sbi( _4017_RESET_PORT, _4017_RESET_PIN );
|
||||
servo = 0;
|
||||
// FIXME: 500 ns required by 4017 reset ???? why does it work without!
|
||||
// asm( "nop; nop; nop; nop;nop; nop; nop; nop;nop; nop; nop; nop;nop; nop; nop; nop;" );
|
||||
cbi( _4017_RESET_PORT, _4017_RESET_PIN );
|
||||
}
|
||||
|
||||
width = servo_widths[ servo ];
|
||||
|
||||
SERVO_OCR += width;
|
||||
|
||||
TCCR1A |= _BV( SERVO_FORCE );
|
||||
|
||||
servo++;
|
||||
}
|
||||
|
||||
void servo_set_one( uint8_t servo, uint16_t value_us )
|
||||
{
|
||||
servo_widths[ servo ] = ChopServo( CLOCK * value_us );
|
||||
}
|
||||
|
||||
void
|
||||
_Pragma( "entrypoint" ) servo_transmit( void )
|
||||
{
|
||||
uint8_t servo;
|
||||
uart_transmit( ( uint8_t )0 );
|
||||
uart_transmit( ( uint8_t )0 );
|
||||
|
||||
_Pragma( "loopbound min 10 max 10" )
|
||||
for ( servo = 0; servo < _4017_NB_CHANNELS; servo++ ) {
|
||||
uart_transmit( ( uint8_t )( servo_widths[ servo ] >> 8 ) );
|
||||
uart_transmit( ( uint8_t )( servo_widths[ servo ] & 0xff ) );
|
||||
}
|
||||
uart_transmit( ( uint8_t )'\n' );
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
|
||||
defines how servos react to radio control or autopilot channels
|
||||
|
||||
*/
|
||||
|
||||
void servo_set( const pprz_t values[ ] )
|
||||
{
|
||||
ServoSet( values ); /*Generated from airframe.xml */
|
||||
}
|
||||
@ -0,0 +1,61 @@
|
||||
/* $Id: servo.h,v 1.1 2011-01-18 12:48:40 moellmer Exp $
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
(c) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
(c) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
This is the new decade counter based servo driving code. It uses
|
||||
one 16-bit output compare registers to determine when the regular
|
||||
servo clock line should be toggled, causing the output to move to the
|
||||
next servo. The other 16-bit output compare is used to drive a
|
||||
JR or Futaba compatible high-speed digital servo.
|
||||
|
||||
User visibile routines:
|
||||
|
||||
- servo_init();
|
||||
|
||||
Call once at the start of the program to bring the servos online
|
||||
and start the external decade counters. This will also start the
|
||||
high speed servo.
|
||||
|
||||
- servo_make_pulse_width( length );
|
||||
|
||||
Converts a position value between 0 and 65536 to actual pulsewidth. 0 is
|
||||
all the way left (1.0 ms pulse) and 65536 is all the way right (2.0 ms
|
||||
pulse). Use it like this:
|
||||
|
||||
servo_widths[ i ] = servo_make_pulse_width( val )
|
||||
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef SERVO_H
|
||||
#define SERVO_H
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "timer.h"
|
||||
|
||||
extern void servo_init( void );
|
||||
extern void servo_set( const pprz_t values[ ] );
|
||||
extern void servo_set_one( uint8_t servo, uint16_t value_us );
|
||||
extern void servo_transmit( void );
|
||||
|
||||
|
||||
#endif /* SERVO_H */
|
||||
@ -0,0 +1,115 @@
|
||||
/*
|
||||
$Id: spi.c,v 1.2 2011-01-25 10:05:41 plazar Exp $
|
||||
|
||||
Paparazzi mcu1 spi functions
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <inttypes.h>
|
||||
#include <arch/io.h>
|
||||
#include <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
|
||||
#include "spi.h"
|
||||
|
||||
#define IT_PORT PORTD
|
||||
#define IT_DDR DDRD
|
||||
#define IT_PIN 7
|
||||
|
||||
#define SPI_DDR DDRB
|
||||
#define SPI_MOSI_PIN 3
|
||||
#define SPI_MISO_PIN 4
|
||||
#define SPI_SCK_PIN 5
|
||||
|
||||
struct inter_mcu_msg from_mega128;
|
||||
struct inter_mcu_msg to_mega128;
|
||||
volatile bool_t mega128_receive_valid = FALSE;
|
||||
volatile bool_t spi_was_interrupted = FALSE;
|
||||
|
||||
static volatile uint8_t idx_buf = 0;
|
||||
static volatile uint8_t xor_in, xor_out;
|
||||
|
||||
void spi_reset( void )
|
||||
{
|
||||
idx_buf = 0;
|
||||
xor_in = 0;
|
||||
xor_out = ( ( uint8_t * )&to_mega128 )[ idx_buf ];
|
||||
SPDR = xor_out;
|
||||
mega128_receive_valid = FALSE;
|
||||
}
|
||||
|
||||
void fbw_spi_init( void )
|
||||
{
|
||||
to_mega128.status = 0;
|
||||
to_mega128.nb_err = 0;
|
||||
|
||||
/* set it pin output */
|
||||
// IT_DDR |= _BV(IT_PIN);
|
||||
|
||||
/* set MISO pin output */
|
||||
SPI_DDR |= _BV( SPI_MISO_PIN );
|
||||
/* enable SPI, slave, MSB first, sck idle low */
|
||||
SPCR = _BV( SPE );
|
||||
/* enable interrupt */
|
||||
SPCR |= _BV( SPIE );
|
||||
}
|
||||
|
||||
SIGNAL( SIG_SPI )
|
||||
{
|
||||
static uint8_t tmp;
|
||||
|
||||
idx_buf++;
|
||||
|
||||
spi_was_interrupted = TRUE;
|
||||
|
||||
if ( idx_buf > FRAME_LENGTH )
|
||||
return;
|
||||
/* we have sent/received a complete frame */
|
||||
if ( idx_buf == FRAME_LENGTH ) {
|
||||
/* read checksum from receive register */
|
||||
tmp = SPDR;
|
||||
/* notify valid frame */
|
||||
if ( tmp == xor_in )
|
||||
mega128_receive_valid = TRUE;
|
||||
else
|
||||
to_mega128.nb_err++;
|
||||
return;
|
||||
}
|
||||
|
||||
/* we are sending/receiving payload */
|
||||
if ( idx_buf < FRAME_LENGTH - 1 ) {
|
||||
/* place new payload byte in send register */
|
||||
tmp = ( ( uint8_t * )&to_mega128 )[ idx_buf ];
|
||||
SPDR = tmp;
|
||||
xor_out = xor_out ^ tmp;
|
||||
}
|
||||
/* we are done sending the payload */
|
||||
else { // idx_buf == FRAME_LENGTH - 1
|
||||
/* place checksum in send register */
|
||||
SPDR = xor_out;
|
||||
}
|
||||
|
||||
/* read the byte from receive register */
|
||||
tmp = SPDR;
|
||||
( ( uint8_t * )&from_mega128 )[ idx_buf - 1 ] = tmp;
|
||||
xor_in = xor_in ^ tmp;
|
||||
}
|
||||
@ -0,0 +1,48 @@
|
||||
/* $Id: spi.h,v 1.1 2011-01-18 12:42:42 moellmer Exp $
|
||||
|
||||
Paparazzi fbw spi functions
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef SPI_H
|
||||
#define SPI_H
|
||||
|
||||
#include "link_autopilot.h"
|
||||
|
||||
|
||||
#define SPI_PORT PORTB
|
||||
#define SPI_PIN PINB
|
||||
#define SPI_SS_PIN 2
|
||||
|
||||
#define SpiIsSelected() (bit_is_clear(SPI_PIN, SPI_SS_PIN))
|
||||
|
||||
extern struct inter_mcu_msg from_mega128;
|
||||
extern struct inter_mcu_msg to_mega128;
|
||||
extern volatile bool_t mega128_receive_valid;
|
||||
extern volatile bool_t spi_was_interrupted;
|
||||
|
||||
|
||||
void spi_init( void );
|
||||
void spi_reset( void );
|
||||
|
||||
|
||||
#endif /* SPI_H */
|
||||
@ -0,0 +1,92 @@
|
||||
/* $Id: timer.h,v 1.2 2011-01-18 14:55:53 moellmer Exp $
|
||||
|
||||
Paparazzi fbw timer functions
|
||||
|
||||
Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
|
||||
Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef TIMER_H
|
||||
#define TIMER_H
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <signal.h>
|
||||
#include <io.h>
|
||||
#include "link_autopilot.h"
|
||||
|
||||
|
||||
/*
|
||||
Enable Timer1 (16-bit) running at Clk/1 for the global system
|
||||
clock. This will be used for computing the servo pulse widths,
|
||||
PPM decoding, etc.
|
||||
|
||||
Low frequency periodic tasks will be signaled by timer 2
|
||||
running at Clk/1024. For 16 Mhz clock, this will be every
|
||||
16384 microseconds, or 61 Hz.
|
||||
*/
|
||||
static inline void
|
||||
timer_init( void )
|
||||
{
|
||||
/* Timer1 @ Clk/1: System clock, ppm and servos pulses */
|
||||
TCCR1A = 0x00;
|
||||
TCCR1B = 0x01;
|
||||
|
||||
/* Timer2 @ Clk/1024: Periodic clock */
|
||||
TCCR2 = 0x07;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
Retrieve the current time from the global clock in Timer1,
|
||||
disabling interrupts to avoid stomping on the TEMP register.
|
||||
If interrupts are already off, the non_atomic form can be used.
|
||||
*/
|
||||
static inline uint16_t
|
||||
timer_now( void )
|
||||
{
|
||||
return TCNT1;
|
||||
}
|
||||
|
||||
static inline uint16_t
|
||||
timer_now_non_atomic( void )
|
||||
{
|
||||
return TCNT1L;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
Periodic tasks occur when Timer2 overflows. Check and unset
|
||||
the overflow bit. We cycle through four possible periodic states,
|
||||
so each state occurs every 60 Hz.
|
||||
*/
|
||||
static inline bool_t
|
||||
timer_periodic( void )
|
||||
{
|
||||
if ( !bit_is_set( TIFR, TOV2 ) )
|
||||
return FALSE;
|
||||
|
||||
TIFR = 1 << TOV2;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,133 @@
|
||||
/*
|
||||
Paparazzi $Id: uart.c,v 1.4 2011-01-25 10:17:46 plazar Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 <arch/signal.h>
|
||||
#include <arch/interrupt.h>
|
||||
#include <arch/io.h>
|
||||
|
||||
|
||||
#include "std.h"
|
||||
#include "uart.h"
|
||||
|
||||
#define TX_BUF_SIZE 256
|
||||
static uint8_t tx_head; /* next free in buf */
|
||||
static volatile uint8_t tx_tail; /* next char to send */
|
||||
static uint8_t tx_buf[ TX_BUF_SIZE ];
|
||||
|
||||
/*
|
||||
UART Baud rate generation settings:
|
||||
|
||||
With 16.0 MHz clock,UBRR=25 => 38400 baud
|
||||
|
||||
*/
|
||||
void uart_init_tx( void )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
/* Baudrate is 38.4k */
|
||||
UBRRH = 0;
|
||||
UBRRL = 25;
|
||||
/* single speed */
|
||||
UCSRA = 0;
|
||||
/* Enable transmitter */
|
||||
UCSRB = _BV( TXEN );
|
||||
/* Set frame format: 8data, 1stop bit */
|
||||
UCSRC = _BV( URSEL ) | _BV( UCSZ1 ) | _BV( UCSZ0 );
|
||||
#endif
|
||||
}
|
||||
|
||||
void uart_init_rx()
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
/* Enable receiver */
|
||||
UCSRB |= _BV( RXEN );
|
||||
/* Enable uart receive interrupt */
|
||||
sbi( UCSRB, RXCIE );
|
||||
#endif
|
||||
}
|
||||
|
||||
void uart_transmit( unsigned char data )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
if ( UCSRB & _BV( TXCIE ) ) {
|
||||
/* we are waiting for the last char to be sent : buffering */
|
||||
if ( tx_tail == tx_head + 1 ) { /* BUF_SIZE = 256 */
|
||||
/* Buffer is full (almost, but tx_head = tx_tail means "empty" */
|
||||
return;
|
||||
}
|
||||
tx_buf[ tx_head ] = data;
|
||||
tx_head++; /* BUF_SIZE = 256 */
|
||||
} else { /* Channel is free: just send */
|
||||
UDR = data;
|
||||
sbi( UCSRB, TXCIE );
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void uart_print_hex ( uint8_t c )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
const uint8_t hex[ 16 ] = { '0', '1', '2', '3', '4', '5', '6', '7',
|
||||
'8', '9', 'A', 'B', 'C', 'D', 'E', 'F'
|
||||
};
|
||||
uint8_t high = ( c & 0xF0 ) >> 4;
|
||||
uint8_t low = c & 0x0F;
|
||||
uart_transmit( hex[ high ] );
|
||||
uart_transmit( hex[ low ] );
|
||||
#endif
|
||||
}
|
||||
|
||||
void uart_print_hex16 ( uint16_t c )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
uint8_t high = ( uint8_t )( c >> 8 );
|
||||
uint8_t low = ( uint8_t )( c );
|
||||
uart_print_hex( high );
|
||||
uart_print_hex( low );
|
||||
#endif
|
||||
}
|
||||
|
||||
void uart_print_string( const uint8_t *s )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
uint8_t i = 0;
|
||||
_Pragma( "loopbound min 100 max 100" )
|
||||
while ( s[ i ] ) {
|
||||
uart_transmit( s[ i ] );
|
||||
i++;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
SIGNAL( SIG_UART_TRANS )
|
||||
{
|
||||
#ifndef WCET_ANALYSIS
|
||||
if ( tx_head == tx_tail ) {
|
||||
/* Nothing more to send */
|
||||
cbi( UCSRB, TXCIE ); /* disable interrupt */
|
||||
} else {
|
||||
UDR = tx_buf[ tx_tail ];
|
||||
tx_tail++; /* warning tx_buf_len is 256 */
|
||||
}
|
||||
#endif
|
||||
}
|
||||
@ -0,0 +1,39 @@
|
||||
/*
|
||||
Paparazzi $Id: uart.h,v 1.1 2011-01-18 12:42:42 moellmer Exp $
|
||||
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef _UART_H_
|
||||
#define _UART_H_
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
void uart_init_tx( void );
|
||||
void uart_init_rx( void );
|
||||
void uart_transmit( unsigned char data );
|
||||
|
||||
void uart_print_hex ( uint8_t c );
|
||||
void uart_print_hex16 ( uint16_t c );
|
||||
void uart_print_string( const uint8_t *s );
|
||||
void uart_print_float( const float *f );
|
||||
|
||||
#endif
|
||||
@ -0,0 +1,49 @@
|
||||
/*
|
||||
downlink.h
|
||||
Copyright (C) 2003 Antoine Drouin
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
/*
|
||||
decodes downlink protocol
|
||||
*/
|
||||
|
||||
#ifndef DOWNLINK_H
|
||||
#define DOWNLINK_H
|
||||
|
||||
#include <glib.h>
|
||||
#include "protocol.h"
|
||||
#include "transport.h"
|
||||
#include "geometry.h"
|
||||
|
||||
#define DL_STX 0x05
|
||||
#define DL_ETX 0x06
|
||||
#define DL_HEAD_LEN 1
|
||||
#define DL_PAYLOAD_OFFSET 1
|
||||
#define DL_TAIL_LEN 2
|
||||
|
||||
|
||||
struct Transport *downlink_new( struct PprzProtocol *protocol,
|
||||
void( *err_callback )( gpointer callback_data, GError *error ),
|
||||
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg ),
|
||||
gpointer callback_data );
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
185
targets/wasm-tacle/parallel/PapaBench/sw/include/c/geometry.h
Normal file
185
targets/wasm-tacle/parallel/PapaBench/sw/include/c/geometry.h
Normal file
@ -0,0 +1,185 @@
|
||||
/*
|
||||
geometry.h
|
||||
Copyright (C) 2003 Antoine Drouin
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
/*
|
||||
geometry :) originated from Pascal Brisset pascal(dot)brisset(at)free(dot)fr
|
||||
ocaml code.
|
||||
*/
|
||||
|
||||
#ifndef GEOMETRY_H
|
||||
#define GEOMETRY_H
|
||||
|
||||
#include <math.h>
|
||||
#include "angles.h"
|
||||
/*
|
||||
please use
|
||||
x, y, z, h in meters
|
||||
lat, lon in radians
|
||||
*/
|
||||
struct double_c2d {
|
||||
double x, y;
|
||||
};
|
||||
|
||||
struct double_p2d {
|
||||
double r, teta;
|
||||
};
|
||||
|
||||
struct double_e2d {
|
||||
double lat, lon;
|
||||
};
|
||||
|
||||
struct double_c3d {
|
||||
double x, y, z;
|
||||
};
|
||||
|
||||
struct double_e3d {
|
||||
double lat, lon, h;
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct double_c2d vector_c2d( const struct double_c2d a,
|
||||
const struct double_c2d b );
|
||||
struct double_c2d unit_vector_c2d( const struct double_c2d a,
|
||||
const struct double_c2d b );
|
||||
struct double_c2d normal_vector_c2d( const struct double_c2d u );
|
||||
|
||||
double vect_prod_c2d( const struct double_c2d a, const struct double_c2d b );
|
||||
double scal_prod_c2d( const struct double_c2d a, const struct double_c2d b );
|
||||
double module_c2d( const struct double_c2d a );
|
||||
double norme2_c2d( const struct double_c2d a );
|
||||
double distance_c2d( const struct double_c2d a, const struct double_c2d b );
|
||||
void rotate_c2d( struct double_c2d *a, double alpha );
|
||||
void translate_c2d( struct double_c2d *a, const struct double_c2d v );
|
||||
void add_c2d_s( struct double_c2d *a, const struct double_c2d b );
|
||||
struct double_c2d add_c2d( struct double_c2d a, struct double_c2d b );
|
||||
void minus_c2d_s( struct double_c2d *a, const struct double_c2d v );
|
||||
struct double_c2d minus_c2d( struct double_c2d a, const struct double_c2d v );
|
||||
void normalise_c2d_s( struct double_c2d *a );
|
||||
struct double_c2d normalise_c2d( struct double_c2d a );
|
||||
void scale_c2d_s( struct double_c2d *v, double factor );
|
||||
struct double_c2d scale_c2d( struct double_c2d v, double factor );
|
||||
double angle_c2d( struct double_c2d u, struct double_c2d v );
|
||||
|
||||
struct double_p2d p2d_of_c2d( const struct double_c2d c );
|
||||
struct double_c2d c2d_of_p2d( const struct double_p2d p );
|
||||
struct double_c2d c2d_of_polar( double r, double teta );
|
||||
struct double_p2d add_p2d( const struct double_p2d p1,
|
||||
const struct double_p2d p2 );
|
||||
struct double_p2d minus_p2d( const struct double_p2d p1,
|
||||
const struct double_p2d p2 );
|
||||
|
||||
void translate_c3d( struct double_c3d *a, const struct double_c3d v );
|
||||
void scale_c3d( struct double_c3d *v, double factor );
|
||||
|
||||
struct double_c2d lin_interp_c2d( struct double_c2d a, double ta,
|
||||
struct double_c2d b, double tb, double t );
|
||||
|
||||
|
||||
/* rad is an angle in EAST NORTH frame,
|
||||
returns the wind direction in degrees
|
||||
*/
|
||||
double wind_dir_from_angle_rad( double rad );
|
||||
|
||||
/* converts between angle in EAST NORTH frame
|
||||
and headings (0->N 90->E) */
|
||||
double heading_of_to_angle_deg( double angle );
|
||||
double heading_of_to_angle_rad( double angle );
|
||||
|
||||
/*
|
||||
compute bank angle
|
||||
*/
|
||||
double compute_phi( struct double_c2d curspeed, struct double_c2d last_speed,
|
||||
double delta_t );
|
||||
|
||||
#define SEC_OF_TIMEVAL(tv) (tv.tv_sec + 1e-6 * tv.tv_usec)
|
||||
#define DELAY_SEC_OF_TIMEVAL(tv1, tv2) (SEC_OF_TIMEVAL(tv1) - SEC_OF_TIMEVAL(tv2))
|
||||
|
||||
double mpi_pi( double val );
|
||||
|
||||
struct ellipsoid {
|
||||
double dx, dy, dz;
|
||||
double a;
|
||||
double df;
|
||||
double e;
|
||||
};
|
||||
|
||||
enum type_ellipsoid {
|
||||
NTF,
|
||||
WGS84,
|
||||
ED50,
|
||||
NAD27,
|
||||
ELLIPSOID_NB
|
||||
};
|
||||
|
||||
extern const struct ellipsoid ellipsoids[ ELLIPSOID_NB ];
|
||||
|
||||
struct lambert {
|
||||
const struct ellipsoid *ellipsoid;
|
||||
double phi0; /* radians */
|
||||
double lphi0;
|
||||
double r0;
|
||||
double lambda0;
|
||||
long x0;
|
||||
long y0;
|
||||
long ys;
|
||||
long k0; /* scale factor */
|
||||
};
|
||||
|
||||
|
||||
enum type_lambert {
|
||||
LAMBERTI,
|
||||
LAMBERTII,
|
||||
LAMBERTIIE,
|
||||
LAMBERTIII,
|
||||
LAMBERTIV,
|
||||
LAMBERT_NB
|
||||
};
|
||||
|
||||
extern const struct lambert lamberts[ LAMBERT_NB ];
|
||||
|
||||
|
||||
#define DECIMAL(D,M,S) (D + M / 60. + S / 3600.)
|
||||
#define RAD_OF_DEG(D) (D*M_PI/180.)
|
||||
#define DEG_OF_RAD(R) (R/M_PI*180.)
|
||||
#define LATITUDE_ISOMETRIQUE(PHI,E) (log(tan(M_PI_4 + PHI/2.0)) - E/2.0 * log((1.0+E*sin(PHI)) / (1.0-E*sin(PHI))))
|
||||
#define E_SQUARE(D) (2.0*D - D * D)
|
||||
#define E_PRIME_SQUARE(D) (1.0 / (1.0 - D) / (1.0 - D) - 1.0)
|
||||
|
||||
|
||||
|
||||
|
||||
/* projections */
|
||||
void lambert_of_e2d ( const enum type_lambert lmb,
|
||||
const enum type_ellipsoid elps,
|
||||
struct double_e2d world_pos, struct double_c2d *lmb_pos );
|
||||
void e2d_of_lambert ( const enum type_lambert lmb,
|
||||
const enum type_ellipsoid elps,
|
||||
struct double_c2d c_pos, struct double_e2d *e_pos );
|
||||
/* coordinates */
|
||||
void e3d_of_c3d( const enum type_ellipsoid elps, struct double_c3d c_pos,
|
||||
struct double_e3d *e_pos );
|
||||
void e2d_of_e2d( const enum type_ellipsoid dest, const enum type_ellipsoid src,
|
||||
struct double_e2d *e2d_pos );
|
||||
|
||||
|
||||
#endif /* GEOMETRY_H */
|
||||
@ -0,0 +1,66 @@
|
||||
/*
|
||||
glade_support.h
|
||||
Copyright (C) 2003 Antoine Drouin
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
/*
|
||||
|
||||
glade hack.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef GLADE_SUPPORT_H
|
||||
#define GLADE_SUPPORT_H
|
||||
|
||||
#define DEFAULT_LEN 256
|
||||
|
||||
#define UPDATE_LABEL(object, label_name, fmt, args...) \
|
||||
{ \
|
||||
GtkLabel* label = GTK_LABEL(gtk_object_get_data(GTK_OBJECT(object), label_name)); \
|
||||
GString* str = g_string_sized_new(DEFAULT_LEN); \
|
||||
if (label) { \
|
||||
g_string_printf(str, fmt, ##args); \
|
||||
gtk_label_set_text(label, str->str); \
|
||||
} \
|
||||
else \
|
||||
g_message("##### unknown label [ %s ]", label_name); \
|
||||
g_string_free(str, TRUE); \
|
||||
} \
|
||||
|
||||
#define UPDATE_LABEL_COLOR(object, label_name, test, fmt, args...) \
|
||||
{ \
|
||||
GtkStateType state; \
|
||||
GtkLabel* label = GTK_LABEL(gtk_object_get_data(GTK_OBJECT(object), label_name)); \
|
||||
GString* str = g_string_sized_new(DEFAULT_LEN); \
|
||||
if (label) { \
|
||||
g_string_printf(str, fmt, ##args); \
|
||||
gtk_label_set_text(label, str->str); \
|
||||
} \
|
||||
else \
|
||||
g_message("##### unknown label [ %s ]", label_name); \
|
||||
g_string_free(str, TRUE); \
|
||||
for (state = GTK_STATE_NORMAL; state <= GTK_STATE_INSENSITIVE; state++) \
|
||||
gtk_widget_modify_fg(GTK_WIDGET(label), state, (test)?&my_red_color:>k_widget_get_default_style()->fg[ state ]); \
|
||||
} \
|
||||
|
||||
|
||||
|
||||
|
||||
#endif /* GLADE_SUPPORT_H */
|
||||
51
targets/wasm-tacle/parallel/PapaBench/sw/include/c/logger.h
Normal file
51
targets/wasm-tacle/parallel/PapaBench/sw/include/c/logger.h
Normal file
@ -0,0 +1,51 @@
|
||||
/*
|
||||
logger.h
|
||||
Copyright (C) 2003 Antoine Drouin
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
/*
|
||||
write and read logs for the paparazzi
|
||||
*/
|
||||
|
||||
|
||||
#ifndef LOGGER_H
|
||||
#define LOGGER_H
|
||||
|
||||
#include <glib.h>
|
||||
#include <libxml/parser.h>
|
||||
|
||||
#include "downlink.h"
|
||||
#include "protocol.h"
|
||||
|
||||
struct Logger {
|
||||
GIOChannel *channel;
|
||||
};
|
||||
|
||||
|
||||
struct Logger *logger_new( const gchar *out_file, xmlDocPtr doc );
|
||||
void logger_log( struct Logger *this, struct PprzMsg *msg );
|
||||
void logger_free( struct Logger *this );
|
||||
|
||||
GList *logger_parse_log( const gchar *filename,
|
||||
struct PprzProtocol **protocol );
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
70
targets/wasm-tacle/parallel/PapaBench/sw/include/c/network.h
Normal file
70
targets/wasm-tacle/parallel/PapaBench/sw/include/c/network.h
Normal file
@ -0,0 +1,70 @@
|
||||
/*
|
||||
$id:$
|
||||
Copyright (C) 2003 Antoine Drouin
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
/*
|
||||
network functions
|
||||
*/
|
||||
|
||||
#ifndef NETWORK_H
|
||||
#define NETWORK_H
|
||||
|
||||
#include <glib.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
|
||||
#include "protocol.h"
|
||||
|
||||
struct NetworkClient {
|
||||
int socket;
|
||||
struct sockaddr_in bcast_addr;
|
||||
struct sockaddr_in server_addr;
|
||||
GFunc rcv_callback;
|
||||
GFunc err_callback;
|
||||
gpointer user_data;
|
||||
struct PprzProtocol *protocol;
|
||||
};
|
||||
|
||||
struct NetworkServer {
|
||||
int bcast_socket;
|
||||
struct sockaddr_in bcast_addr;
|
||||
int listening_socket;
|
||||
GFunc rcv_callback;
|
||||
GFunc err_callback;
|
||||
gpointer user_data;
|
||||
struct PprzProtocol *protocol;
|
||||
};
|
||||
|
||||
|
||||
struct NetworkServer *network_server_new( struct PprzProtocol *protocol,
|
||||
const char *bcast_addr, const int bcast_port,
|
||||
GFunc rcv_callback, GFunc err_callback, gpointer user_data );
|
||||
void network_server_dispatch( struct NetworkServer *this, struct PprzMsg *msg );
|
||||
|
||||
|
||||
struct NetworkClient *network_client_new( struct PprzProtocol *protocol,
|
||||
const char *bcast_addr, const int bcast_port,
|
||||
GFunc rcv_callback, GFunc err_callback, gpointer user_data );
|
||||
void network_client_send_to_server( struct NetworkClient *this,
|
||||
struct PprzMsg *msg );
|
||||
|
||||
|
||||
#endif /* NETWORK_H */
|
||||
120
targets/wasm-tacle/parallel/PapaBench/sw/include/c/protocol.h
Normal file
120
targets/wasm-tacle/parallel/PapaBench/sw/include/c/protocol.h
Normal file
@ -0,0 +1,120 @@
|
||||
/*
|
||||
$Id: protocol.h,v 1.1 2011-01-18 12:48:44 moellmer Exp $
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef PROTOCOL_H
|
||||
#define PROTOCOL_H
|
||||
|
||||
#include <glib.h>
|
||||
#include <libxml/xmlmemory.h>
|
||||
#include <libxml/parser.h>
|
||||
|
||||
enum PprzFieldType {
|
||||
TYP_UINT_8,
|
||||
TYP_UINT_16,
|
||||
TYP_UINT_32,
|
||||
TYP_INT_8,
|
||||
TYP_INT_16,
|
||||
TYP_INT_32,
|
||||
TYP_FLOAT,
|
||||
TYP_ARRAY_UINT_8,
|
||||
TYP_NB
|
||||
};
|
||||
|
||||
struct PprzFieldClass {
|
||||
gchar *name;
|
||||
enum PprzFieldType type;
|
||||
guint offset;
|
||||
gchar *format;
|
||||
gchar *unit;
|
||||
gchar *description;
|
||||
};
|
||||
|
||||
struct PprzMsgClass {
|
||||
guint id;
|
||||
gchar *name;
|
||||
GList *fields_classes;
|
||||
GHashTable *fields_classes_by_name;
|
||||
struct PprzFieldClass **fields_classes_by_id;
|
||||
guint nb_fields_classes;
|
||||
guint size; /* size in byte off all fields + 1 for id */
|
||||
};
|
||||
|
||||
struct PprzMsg {
|
||||
GTimeVal date;
|
||||
struct PprzMsgClass *class;
|
||||
guchar *bytes;
|
||||
};
|
||||
|
||||
struct PprzProtocol {
|
||||
GList *msgs_classes;
|
||||
GHashTable *msgs_classes_by_name;
|
||||
struct PprzMsgClass **msgs_classes_by_id;
|
||||
guint nb_msgs_classes;
|
||||
};
|
||||
|
||||
struct PprzProtocol *pprz_protocol_new_from_xml( xmlDocPtr doc,
|
||||
xmlNodePtr cur );
|
||||
void pprz_protocol_free( struct PprzProtocol *this );
|
||||
|
||||
struct PprzMsg *pprz_msg_new( struct PprzMsgClass *msg_class, GTimeVal date );
|
||||
struct PprzMsg *pprz_protocol_msg_new_by_id( struct PprzProtocol *this,
|
||||
guchar id, GTimeVal date );
|
||||
struct PprzMsg *pprz_protocol_msg_new_by_name( struct PprzProtocol *this,
|
||||
const gchar *name, GTimeVal date );
|
||||
struct PprzMsg *pprz_protocol_msg_new_of_bin( struct PprzProtocol *this,
|
||||
const guchar *buf, GTimeVal date );
|
||||
struct PprzMsg *pprz_protocol_msg_new_of_ascii( struct PprzProtocol *this,
|
||||
const gchar *line );
|
||||
void pprz_msg_free( struct PprzMsg *msg );
|
||||
|
||||
void pprz_protocol_ascii_of_msg( struct PprzMsg *msg, GString **buf );
|
||||
|
||||
const gchar *pprz_protocol_str_of_field_type( enum PprzFieldType type );
|
||||
const guint pprz_protocol_size_of_field_type( enum PprzFieldType type );
|
||||
|
||||
const struct PprzMsgClass *protocol_msg_class_of_id( struct PprzProtocol *this,
|
||||
guint id );
|
||||
|
||||
void pprz_msg_ascii_of_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class, GString *buf );
|
||||
|
||||
gpointer pprz_msg_get_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class );
|
||||
gpointer pprz_msg_get_field_by_name( struct PprzMsg *msg, const gchar *name );
|
||||
gpointer pprz_msg_get_field_by_place( struct PprzMsg *msg, guint place );
|
||||
|
||||
gboolean pprz_msg_set_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class, gconstpointer value );
|
||||
gboolean pprz_msg_set_field_by_name( struct PprzMsg *msg, const gchar *name,
|
||||
gconstpointer value );
|
||||
gboolean pprz_msg_set_field_by_id( struct PprzMsg *msg, guint id,
|
||||
gconstpointer value );
|
||||
|
||||
gboolean pprz_msg_set_field_ascii( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class, const gchar *str_val );
|
||||
gboolean pprz_msg_set_field_ascii_by_name( struct PprzMsg *msg,
|
||||
const gchar *name, const gchar *str_val );
|
||||
gboolean pprz_msg_set_field_ascii_by_id( struct PprzMsg *msg, guint id,
|
||||
const gchar *str_val );
|
||||
|
||||
|
||||
#endif /* PROTOCOL_H */
|
||||
60
targets/wasm-tacle/parallel/PapaBench/sw/include/c/traces.h
Normal file
60
targets/wasm-tacle/parallel/PapaBench/sw/include/c/traces.h
Normal file
@ -0,0 +1,60 @@
|
||||
/*
|
||||
traces.h
|
||||
Copyright (C) 2003 Antoine Drouin
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef TRACES_H
|
||||
#define TRACES_H
|
||||
|
||||
#define TRACE_ON
|
||||
#define TIME_STAMP_TRACE
|
||||
|
||||
#ifdef TRACE_ON
|
||||
#include <stdio.h>
|
||||
|
||||
#define TRACE_ERROR 0x00000001L
|
||||
#define TRACE_DATA_READ 0x00000002L
|
||||
#define TRACE_MSG_RAW 0x00000004L
|
||||
#define TRACE_MSG 0x00000008L
|
||||
#define TRACE_CHECKSUM 0x00000010L
|
||||
#define TRACE_DOWNLINK 0x00000020L
|
||||
#define TRACE_MODEM 0x00000040L
|
||||
#define TRACE_MODEM_VERB 0x00000080L
|
||||
#define TRACE_MODEM_READ 0x00000100L
|
||||
#define TRACE_PARSER 0x00000200L
|
||||
#define TRACE_TRANSPORT 0x00000400L
|
||||
#define TRACE_TRANSPORT_VERB 0x00000800L
|
||||
|
||||
extern unsigned long TraceLevel;
|
||||
|
||||
#define TRACE(type,fmt,args...) \
|
||||
{ \
|
||||
if (TraceLevel&type) { \
|
||||
fprintf(stderr, "[ "#type" ] " fmt, ##args); \
|
||||
} \
|
||||
}
|
||||
const char *print_hex( const unsigned char *buf, unsigned int len );
|
||||
|
||||
#else
|
||||
#define TRACE(type,fmt,args...)
|
||||
#endif /* TRACE_ON */
|
||||
|
||||
#endif /* TRACES_H */
|
||||
|
||||
@ -0,0 +1,75 @@
|
||||
#ifndef TRANSPORT_H
|
||||
#define TRANSPORT_H
|
||||
|
||||
#include <glib.h>
|
||||
|
||||
#include <string.h>
|
||||
|
||||
|
||||
#define TRANSPORT_BUF_LEN 1024
|
||||
#define TRANSPORT_HEAD_LEN 1
|
||||
#define TRANSPORT_PAYLOAD_OFFSET TRANSPORT_HEAD_LEN
|
||||
#define TRANSPORT_TAIL_LEN 2
|
||||
|
||||
enum TransportError {
|
||||
TRANSPORT_NO_ERROR,
|
||||
TRANSPORT_CHECKSUM_ERROR,
|
||||
TRANSPORT_NO_ETX,
|
||||
TRANSPORT_INVALID_MSG_ID,
|
||||
TRANSPORT_INVALID_MSG_LEN,
|
||||
TRANSPORT_BUF_OVFW,
|
||||
TRANSPORT_ERROR_NB
|
||||
};
|
||||
|
||||
struct TransportMsg {
|
||||
GTimeVal date;
|
||||
guint len;
|
||||
guchar *data;
|
||||
};
|
||||
|
||||
struct TransportStatus {
|
||||
guint32 run_time;
|
||||
guint32 nb_byte;
|
||||
guint32 nb_msg;
|
||||
guint32 nb_err;
|
||||
gfloat byte_rate;
|
||||
gfloat msg_rate;
|
||||
};
|
||||
|
||||
struct Transport {
|
||||
void( *err_callback )( gpointer callback_data, GError *error );
|
||||
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg );
|
||||
gpointer callback_data;
|
||||
GQuark quark;
|
||||
gchar stx;
|
||||
gchar etx;
|
||||
gboolean fixed_size;
|
||||
gboolean two_bytes_checksum;
|
||||
guint nb_msg_type;
|
||||
guint *size_msg;
|
||||
guint max_msg_size;
|
||||
struct TransportStatus status;
|
||||
gulong nb_msg_last_status;
|
||||
gulong nb_byte_last_status;
|
||||
guchar buf[ TRANSPORT_BUF_LEN ];
|
||||
guint buf_len;
|
||||
GTimeVal start_date, last_status_date;
|
||||
|
||||
};
|
||||
|
||||
struct Transport *transport_new( gboolean fixed_size,
|
||||
gboolean two_bytes_checksum, guint nb_msg, guint *size_msg, guint max_msg_size,
|
||||
guchar stx, guchar etx,
|
||||
void( *err_callback )( gpointer callback_data, GError *error ),
|
||||
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg ),
|
||||
gpointer callback_data );
|
||||
|
||||
struct TransportStatus *transport_get_status( struct Transport *this );
|
||||
|
||||
void transport_feed_data( struct Transport *this, const guchar *buf,
|
||||
guint len );
|
||||
|
||||
void transport_free( struct Transport *this );
|
||||
|
||||
|
||||
#endif /* TRANSPORT_H */
|
||||
12
targets/wasm-tacle/parallel/PapaBench/sw/include/inttypes.h
Normal file
12
targets/wasm-tacle/parallel/PapaBench/sw/include/inttypes.h
Normal file
@ -0,0 +1,12 @@
|
||||
#ifndef INTTYPES_H_
|
||||
#define INTTYPES_H_
|
||||
|
||||
typedef signed char int8_t;
|
||||
typedef unsigned char uint8_t;
|
||||
typedef signed short int16_t;
|
||||
typedef unsigned short uint16_t;
|
||||
typedef signed long int32_t;
|
||||
typedef unsigned long uint32_t;
|
||||
|
||||
#endif // INTTYPES_H_
|
||||
|
||||
21
targets/wasm-tacle/parallel/PapaBench/sw/include/math.h
Normal file
21
targets/wasm-tacle/parallel/PapaBench/sw/include/math.h
Normal file
@ -0,0 +1,21 @@
|
||||
#ifndef MATH_H_
|
||||
#define MATH_H_
|
||||
|
||||
#define M_PI 3.141592653589793238462643
|
||||
#define HALFPI 1.57079632679489661923
|
||||
#define sin(x) (pp_sin(x))
|
||||
#define atan2(x,y) (pp_atan2(x,y))
|
||||
#define sqrt(x) (pp_sqrt(x))
|
||||
#define cos(x) (pp_sin(x+HALFPI)) /*cosinus [ radians ]*/
|
||||
#define fabs(x) (pp_fabs(x))
|
||||
#define pp_fabs(x) ((x)< 0. ? -(x) : (x)) /*floating absolute value*/
|
||||
#define Max(x,y) (pp_Max(x,y))
|
||||
#define pp_Max(x,y) ((x) >= (y) ? (x) : (y))
|
||||
#define Min(x,y) (pp_Min(x,y))
|
||||
#define pp_Min(x,y) ((x) <= (y) ? (x) : (y))
|
||||
|
||||
extern double pp_sin( double x );
|
||||
extern double pp_sqrt( double x );
|
||||
extern double pp_atan2( double x, double y );
|
||||
|
||||
#endif /*MATH_H_*/
|
||||
22
targets/wasm-tacle/parallel/PapaBench/sw/include/std.h
Normal file
22
targets/wasm-tacle/parallel/PapaBench/sw/include/std.h
Normal file
@ -0,0 +1,22 @@
|
||||
#ifndef STD_H
|
||||
#define STD_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#ifndef FALSE
|
||||
#define FALSE 0
|
||||
#endif
|
||||
#ifndef TRUE
|
||||
#define TRUE (!FALSE)
|
||||
#endif
|
||||
|
||||
typedef uint8_t bool_t; // Boolean values
|
||||
|
||||
#ifndef cbi
|
||||
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
|
||||
#endif
|
||||
#ifndef sbi
|
||||
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
|
||||
#endif
|
||||
|
||||
#endif /* STD_H */
|
||||
65
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/downlink.c
Normal file
65
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/downlink.c
Normal file
@ -0,0 +1,65 @@
|
||||
/*
|
||||
downlink.c
|
||||
Copyright (C) 2003 Antoine Drouin
|
||||
|
||||
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 <string.h>
|
||||
|
||||
#include "downlink.h"
|
||||
|
||||
#define GDOUBLE_FROM_DIF_TV(a,b) ((gdouble)(a.tv_sec-b.tv_sec) + 1e-6 * (gdouble)(a.tv_usec-b.tv_usec))
|
||||
|
||||
|
||||
#define READ_GFLOAT_FROM_BUF(dest, buf) dest = *((gfloat*)(buf)); buf += 4;
|
||||
#define READ_GUINT32_FROM_BUF(dest, buf) dest = GUINT32_FROM_LE(*((guint32*)(buf))); buf += 4;
|
||||
#define READ_GUINT16_FROM_BUF(dest, buf) dest = GUINT16_FROM_LE(*((guint16*)(buf))); buf += 2;
|
||||
#define READ_GINT32_FROM_BUF(dest, buf) dest = GINT32_FROM_LE(*((gint32*)(buf))); buf += 4;
|
||||
#define READ_GINT16_FROM_BUF(dest, buf) dest = GINT16_FROM_LE(*((gint16*)(buf))); buf += 2;
|
||||
#define READ_GUINT8_FROM_BUF(dest, buf) dest = (guint8)(*(buf++));
|
||||
|
||||
struct Transport *downlink_new( struct PprzProtocol *protocol,
|
||||
void( *err_callback )( gpointer callback_data, GError *error ),
|
||||
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg ),
|
||||
gpointer callback_data )
|
||||
{
|
||||
struct Transport *transport;
|
||||
guint *fixed_payload_len = g_new( guint,
|
||||
g_list_length( protocol->msgs_classes ) );
|
||||
guint i = 0;
|
||||
GList *msgs_classes = protocol->msgs_classes;
|
||||
while ( msgs_classes ) {
|
||||
fixed_payload_len[ i ] = ( ( struct PprzMsgClass * )msgs_classes->data )->size;
|
||||
// printf("fixed_payload_len : %d %d\n", i, fixed_payload_len[ i ]);
|
||||
i++;
|
||||
msgs_classes = msgs_classes->next;
|
||||
}
|
||||
|
||||
transport = transport_new( TRUE, /* fixed_size */
|
||||
TRUE, /* two bytes checksum */
|
||||
i, /* nb_msg */
|
||||
fixed_payload_len, /* size_msg */
|
||||
100, /* max_msg_size */
|
||||
DL_STX, DL_ETX, /* STX, ETX */
|
||||
err_callback, /* err callback */
|
||||
msg_callback, /* msg callback */
|
||||
callback_data ); /* callback data */
|
||||
return transport;
|
||||
}
|
||||
451
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/geometry.c
Normal file
451
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/geometry.c
Normal file
@ -0,0 +1,451 @@
|
||||
/*
|
||||
geometry.c
|
||||
Copyright (C) 2003 Antoine Drouin
|
||||
|
||||
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 "geometry.h"
|
||||
|
||||
//#include <math.h>
|
||||
#include "math_papabench.h"
|
||||
|
||||
const struct ellipsoid ellipsoids[ ELLIPSOID_NB ] = {
|
||||
{-168., -60., 320., 6378249.2, 0.0034075495234250643, 0.08248325676}, /* NTF */
|
||||
{ 0., 0., 0., 6378137.0, 0.0033528106647474805, 0.08181919106}, /* WGS84 */
|
||||
{-87.0, -98., -121.0, 6378388.0, 0.003367003367003367, 0.08199188998}, /* ED50 */
|
||||
{ 0.0, 125.0, 194.0, 6378067.6, 0.0033155460257, 0.08181919106} /* NAD27 */
|
||||
};
|
||||
/* From http://www.tandt.be/wis/WiS/eqntf.html and http://www.ign.fr/MP/GEOD/geodesie/coordonnees.html */
|
||||
const struct lambert lamberts[ LAMBERT_NB ] = {
|
||||
{ /* LAMBERTI */
|
||||
&ellipsoids[ NTF ],
|
||||
RAD_OF_DEG( DECIMAL( 49., 30., 0. ) ), /* phi0 */
|
||||
0.991996665, /* lphi0 */
|
||||
5457616.674, /* r0 */
|
||||
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
||||
600000, /* x0 */
|
||||
200000, /* y0 */
|
||||
5657617, /* ys */
|
||||
0.99987734 /* k0 */
|
||||
},
|
||||
{ /* LAMBERTII */
|
||||
&ellipsoids[ NTF ],
|
||||
RAD_OF_DEG( DECIMAL( 46., 48., 0. ) ), /* phi0 */
|
||||
0.921557361, /* lphi0 */
|
||||
5999695.77, /* r0 */
|
||||
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
||||
600000, /* x0 */
|
||||
2200000, /* y0 */
|
||||
6199696, /* ys */
|
||||
0.99987742 /* k0 */
|
||||
},
|
||||
{ /* LAMBERTIIE */
|
||||
&ellipsoids[ NTF ],
|
||||
RAD_OF_DEG( DECIMAL( 46., 48., 0. ) ), /* phi0 */
|
||||
0.921557361, /* lphi0 */
|
||||
5999695.77, /* r0 */
|
||||
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
||||
600000, /* x0 */
|
||||
2200000, /* y0 */
|
||||
8199696, /* ys */
|
||||
0.99987742 /* k0 */
|
||||
},
|
||||
{ /* LAMBERTIII */
|
||||
&ellipsoids[ NTF ],
|
||||
RAD_OF_DEG( DECIMAL( 44., 6., 0. ) ), /* phi0 */
|
||||
0.921557361, /* lphi0 */
|
||||
6591905.08, /* r0 */
|
||||
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
||||
600000, /* x0 */
|
||||
3200000, /* y0 */
|
||||
6791905, /* ys */
|
||||
0.99987750 /* k0 */
|
||||
},
|
||||
{ /* LAMBERTIV */
|
||||
&ellipsoids[ NTF ],
|
||||
RAD_OF_DEG( DECIMAL( 42., 9., 54. ) ), /* phi0 */
|
||||
0.921557361, /* lphi0 */
|
||||
7053300.18, /* r0 */
|
||||
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
||||
234, /* x0 */ /* FIXME mmmmmmm suspicious */
|
||||
4185861, /* y0 */
|
||||
7239162, /* ys */
|
||||
0.99994471 /* k0 */
|
||||
},
|
||||
};
|
||||
|
||||
double distance_c2d( const struct double_c2d a, const struct double_c2d b )
|
||||
{
|
||||
return sqrt( ( a.x - b.x ) * ( a.x - b.x ) + ( a.y - b.y ) * ( a.y - b.y ) );
|
||||
}
|
||||
|
||||
struct double_c2d vector_c2d( const struct double_c2d a,
|
||||
const struct double_c2d b )
|
||||
{
|
||||
struct double_c2d v;
|
||||
v.x = b.x - a.x;
|
||||
v.y = b.y - a.y;
|
||||
return v;
|
||||
}
|
||||
|
||||
struct double_c2d unit_vector_c2d( const struct double_c2d a,
|
||||
const struct double_c2d b )
|
||||
{
|
||||
struct double_c2d v = vector_c2d( a, b );
|
||||
scale_c2d_s( &v, 1 / module_c2d( v ) );
|
||||
return v;
|
||||
}
|
||||
|
||||
struct double_c2d normal_vector_c2d( const struct double_c2d u )
|
||||
{
|
||||
struct double_c2d v;
|
||||
v.x = -u.y;
|
||||
v.y = u.x;
|
||||
return v;
|
||||
}
|
||||
|
||||
double vect_prod_c2d( const struct double_c2d a, const struct double_c2d b )
|
||||
{
|
||||
return a.x * b.y - a.y * b.x;
|
||||
}
|
||||
|
||||
double scal_prod_c2d( const struct double_c2d a, const struct double_c2d b )
|
||||
{
|
||||
return a.x * b.x + a.y * b.y;
|
||||
}
|
||||
|
||||
void rotate_c2d( struct double_c2d *a, double alpha )
|
||||
{
|
||||
double tmp;
|
||||
tmp = a->x * cos( alpha ) - a->y * sin( alpha );
|
||||
a->y = a->x * sin( alpha ) + a->y * cos( alpha );
|
||||
a->x = tmp;
|
||||
}
|
||||
|
||||
double norme2_c2d( const struct double_c2d a )
|
||||
{
|
||||
return a.x * a.x + a.y * a.y;
|
||||
}
|
||||
|
||||
double module_c2d( const struct double_c2d a )
|
||||
{
|
||||
return sqrt( norme2_c2d( a ) );
|
||||
}
|
||||
|
||||
void translate_c2d( struct double_c2d *a, const struct double_c2d v )
|
||||
{
|
||||
add_c2d_s( a, v );
|
||||
}
|
||||
|
||||
void add_c2d_s( struct double_c2d *a, const struct double_c2d b )
|
||||
{
|
||||
a->x = a->x + b.x;
|
||||
a->y = a->y + b.y;
|
||||
}
|
||||
|
||||
struct double_c2d add_c2d( struct double_c2d a, const struct double_c2d b )
|
||||
{
|
||||
add_c2d_s( &a, b );
|
||||
return a;
|
||||
}
|
||||
|
||||
void normalise_c2d_s( struct double_c2d *a )
|
||||
{
|
||||
scale_c2d_s( a, 1 / module_c2d( *a ) );
|
||||
}
|
||||
|
||||
struct double_c2d normalise_c2d( struct double_c2d a )
|
||||
{
|
||||
normalise_c2d_s( &a );
|
||||
return a;
|
||||
}
|
||||
|
||||
void minus_c2d_s( struct double_c2d *a, const struct double_c2d b )
|
||||
{
|
||||
a->x = a->x - b.x;
|
||||
a->y = a->y - b.y;
|
||||
}
|
||||
|
||||
struct double_c2d minus_c2d( struct double_c2d a, const struct double_c2d b )
|
||||
{
|
||||
minus_c2d_s( &a, b );
|
||||
return a;
|
||||
}
|
||||
|
||||
void scale_c2d_s( struct double_c2d *v, double factor )
|
||||
{
|
||||
v->x = v->x * factor;
|
||||
v->y = v->y * factor;
|
||||
}
|
||||
|
||||
struct double_c2d scale_c2d( struct double_c2d v, double factor )
|
||||
{
|
||||
scale_c2d_s( &v, factor );
|
||||
return v;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#define MPI_PI(val)\
|
||||
{\
|
||||
while (val> M_PI) val -= 2. * M_PI;\
|
||||
while (val<-M_PI) val += 2. * M_PI;\
|
||||
}
|
||||
|
||||
double mpi_pi( double val )
|
||||
{
|
||||
MPI_PI( val );
|
||||
return val;
|
||||
};
|
||||
|
||||
struct double_c2d lin_interp_c2d( struct double_c2d a, double ta,
|
||||
struct double_c2d b, double tb, double t )
|
||||
{
|
||||
double delta_t = tb - ta;
|
||||
struct double_c2d res;
|
||||
res.x = a.x + ( t - ta ) / delta_t *( b.x - a.x );
|
||||
res.y = a.y + ( t - ta ) / delta_t *( b.y - a.y );
|
||||
return res;
|
||||
}
|
||||
|
||||
double wind_dir_from_angle_deg( double deg )
|
||||
{
|
||||
double w = 180. + 90. - deg;
|
||||
while ( w > 360 ) w -= 360;
|
||||
return w;
|
||||
}
|
||||
|
||||
double wind_dir_from_angle_rad( double rad )
|
||||
{
|
||||
double w = M_PI + M_PI_2 - rad;
|
||||
while ( w > 2 * M_PI ) w -= ( 2 * M_PI );
|
||||
return w;
|
||||
}
|
||||
|
||||
double heading_of_to_angle_deg( double angle )
|
||||
{
|
||||
double a = 90. - angle + 360.;
|
||||
while ( a >= 180. ) a -= 360.;
|
||||
return a;
|
||||
}
|
||||
|
||||
double heading_of_to_angle_rad( double angle )
|
||||
{
|
||||
double two_pi = 2 * M_PI;
|
||||
double a = M_PI_2 - angle + two_pi;
|
||||
while ( a >= two_pi ) a -= two_pi;
|
||||
return a;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#define G 9.81
|
||||
#define ACCEL_DOWN 0
|
||||
double compute_phi( struct double_c2d curspeed, struct double_c2d last_speed,
|
||||
double delta_t )
|
||||
{
|
||||
double accel_norm, accel_tgt, phi;
|
||||
struct double_c2d accel = minus_c2d( curspeed, last_speed );
|
||||
// printf("%lf %lf %lf %lf %lf\n", curspeed.x, curspeed.y, last_speed.x, last_speed.y, delta_t);
|
||||
scale_c2d_s( &accel, 1 / delta_t );
|
||||
accel_norm = vect_prod_c2d( accel, curspeed ) / module_c2d( curspeed );
|
||||
accel_tgt = scal_prod_c2d( accel, curspeed ) / module_c2d( curspeed );
|
||||
phi = -atan2( accel_norm, G - ACCEL_DOWN );
|
||||
return phi;
|
||||
}
|
||||
|
||||
double angle_c2d( struct double_c2d u, struct double_c2d v )
|
||||
{
|
||||
double alpha1 = atan2( u.y, u.x );
|
||||
double alpha2 = atan2( v.y, v.x );
|
||||
double alpha = alpha2 - alpha1;
|
||||
MPI_PI( alpha );
|
||||
return alpha;
|
||||
}
|
||||
|
||||
struct double_p2d p2d_of_c2d( const struct double_c2d c )
|
||||
{
|
||||
struct double_p2d p;
|
||||
p.r = module_c2d( c );
|
||||
p.teta = atan2( c.y, c.x );
|
||||
return p;
|
||||
}
|
||||
|
||||
struct double_c2d c2d_of_p2d( const struct double_p2d p )
|
||||
{
|
||||
struct double_c2d c;
|
||||
c.x = p.r * cos( p.teta );
|
||||
c.y = p.r * sin( p.teta );
|
||||
return c;
|
||||
}
|
||||
|
||||
struct double_p2d add_p2d( const struct double_p2d p1,
|
||||
const struct double_p2d p2 )
|
||||
{
|
||||
/* p.r = sqrt(p1.r*p1.r + p2.r*p2.r + 2 * p1.r * p2.r * cos ( p1.teta - p2.teta )); */
|
||||
/* p.teta = 0; */
|
||||
struct double_c2d c1 = c2d_of_p2d( p1 );
|
||||
struct double_c2d c2 = c2d_of_p2d( p2 );
|
||||
struct double_c2d c3 = add_c2d( c1, c2 );
|
||||
struct double_p2d p = p2d_of_c2d( c3 );
|
||||
return p;
|
||||
}
|
||||
|
||||
|
||||
struct double_p2d minus_p2d( const struct double_p2d p1,
|
||||
const struct double_p2d p2 )
|
||||
{
|
||||
struct double_c2d c1 = c2d_of_p2d( p1 );
|
||||
struct double_c2d c2 = c2d_of_p2d( p2 );
|
||||
struct double_c2d c3 = minus_c2d( c1, c2 );
|
||||
struct double_p2d p = p2d_of_c2d( c3 );
|
||||
return p;
|
||||
}
|
||||
|
||||
|
||||
struct double_c2d c2d_of_polar( double r, double teta )
|
||||
{
|
||||
struct double_c2d res = {r * cos( teta ), r * sin( teta )};
|
||||
return res;
|
||||
}
|
||||
|
||||
void translate_c3d( struct double_c3d *a, const struct double_c3d v )
|
||||
{
|
||||
a->x = a->x + v.x;
|
||||
a->y = a->y + v.y;
|
||||
a->z = a->z + v.z;
|
||||
}
|
||||
|
||||
void scale_c3d( struct double_c3d *v, double factor )
|
||||
{
|
||||
v->x = v->x * factor;
|
||||
v->y = v->y * factor;
|
||||
v->z = v->z * factor;
|
||||
}
|
||||
|
||||
static double inverse_latitude_isometrique( double lat, double e,
|
||||
double epsilon )
|
||||
{
|
||||
double exp_l = exp( lat );
|
||||
double phi0 = 2. * atan( exp_l - M_PI_2 );
|
||||
double loop( double phi ) {
|
||||
double sin_phi = e * sin( phi );
|
||||
double new_phi = 2. * atan ( pow( ( 1. + sin_phi ) / ( 1. - sin_phi ),
|
||||
e / 2 ) * exp_l ) - M_PI_2;
|
||||
if ( fabs( phi - new_phi ) < epsilon ) return new_phi;
|
||||
return loop( new_phi );
|
||||
}
|
||||
return loop( phi0 );
|
||||
}
|
||||
|
||||
static double lambert_n ( const enum type_lambert lmb )
|
||||
{
|
||||
return sin( lamberts[ lmb ].phi0 );
|
||||
}
|
||||
static double lambert_c ( const enum type_lambert lmb )
|
||||
{
|
||||
return lamberts[ lmb ].r0 * exp( lamberts[ lmb ].phi0 * lambert_n( lmb ) );
|
||||
}
|
||||
|
||||
void lambert_of_e2d ( const enum type_lambert lmb,
|
||||
const enum type_ellipsoid elps,
|
||||
struct double_e2d e2d_pos, struct double_c2d *lmb_pos )
|
||||
{
|
||||
double n, ll, r, gamma;
|
||||
e2d_of_e2d( NTF, elps, &e2d_pos );
|
||||
n = lambert_n( lmb );
|
||||
ll = LATITUDE_ISOMETRIQUE( e2d_pos.lat, lamberts[ lmb ].ellipsoid->e );
|
||||
r = lamberts[ lmb ].r0 * exp( lamberts[ lmb ].lphi0 * sin( lamberts[ lmb ].phi0 ) ) *
|
||||
exp( -ll * n );
|
||||
gamma = ( e2d_pos.lon - lamberts[ lmb ].lambda0 ) * n;
|
||||
lmb_pos->x = ( double )lamberts[ lmb ].x0 + r * sin( gamma );
|
||||
lmb_pos->y = ( double )lamberts[ lmb ].ys - r * cos( gamma );
|
||||
}
|
||||
|
||||
void e2d_of_lambert ( const enum type_lambert lmb,
|
||||
const enum type_ellipsoid elps,
|
||||
struct double_c2d c_pos, struct double_e2d *e_pos )
|
||||
{
|
||||
double dx = c_pos.x - lamberts[ lmb ].x0;
|
||||
double dy = c_pos.y - lamberts[ lmb ].ys;
|
||||
double r = sqrt( dx * dx + dy * dy );
|
||||
double gamma = atan2( dx, -dy );
|
||||
double lambda = lamberts[ lmb ].lambda0 + gamma / lambert_n( lmb );
|
||||
double ll = -1. / lambert_n( lmb ) * log( fabs( r / lambert_c( lmb ) ) );
|
||||
double phi = inverse_latitude_isometrique( ll, lamberts[ lmb ].ellipsoid->e,
|
||||
1e-11 );
|
||||
e_pos->lon = lambda;
|
||||
e_pos->lat = phi;
|
||||
e2d_of_e2d( elps, NTF, e_pos );
|
||||
}
|
||||
|
||||
void e2d_of_e2d( const enum type_ellipsoid dest, const enum type_ellipsoid src,
|
||||
struct double_e2d *e2d_pos )
|
||||
{
|
||||
if ( dest != src ) {
|
||||
double sin_lat = sin( e2d_pos->lat );
|
||||
double cos_lat = cos( e2d_pos->lat );
|
||||
double sin_lon = sin( e2d_pos->lon );
|
||||
double cos_lon = cos( e2d_pos->lon );
|
||||
double d16 = E_SQUARE( ellipsoids[ src ].df );
|
||||
double d17 = E_SQUARE( ellipsoids[ dest ].df );
|
||||
double d18 = ellipsoids[ src ].a / sqrt ( 1.0 - d16 * sin_lat * sin_lat );
|
||||
double d20 = d18 * cos_lat * cos_lon;
|
||||
double d21 = d18 * cos_lat * sin_lon;
|
||||
double d22 = d18 * ( 1.0 - d16 ) * sin_lat;
|
||||
double d23 = d20 - ellipsoids[ dest ].dx + ellipsoids[ src ].dx;
|
||||
double d24 = d21 - ellipsoids[ dest ].dy + ellipsoids[ src ].dy;
|
||||
double d25 = d22 - ellipsoids[ dest ].dz + ellipsoids[ src ].dz;
|
||||
double d26 = sqrt ( d23 * d23 + d24 * d24 );
|
||||
double d27 = atan2( d25, d26 * ( 1.0 - ellipsoids[ dest ].df ) );
|
||||
double d28 = ellipsoids[ dest ].a * ( 1.0 - ellipsoids[ dest ].df );
|
||||
double d29 = E_PRIME_SQUARE( ellipsoids[ dest ].df );
|
||||
double d3 = atan2( ( d25 + d29 * d28 * sin( d27 ) * sin( d27 ) * sin( d27 ) ),
|
||||
( d26 - d17 * ellipsoids[ dest ].a * cos( d27 ) * cos( d27 ) * cos( d27 ) ) );
|
||||
double d4 = atan2( d24, d23 );
|
||||
e2d_pos->lon = d4;
|
||||
e2d_pos->lat = d3;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
converts 3d cartesian coordinate to 3d ellipsoidal
|
||||
*/
|
||||
void e3d_of_c3d( const enum type_ellipsoid elps, struct double_c3d c_pos,
|
||||
struct double_e3d *e_pos )
|
||||
{
|
||||
#define WGS_84_SMA 6378137.00
|
||||
#define WGS_84_SMB 6356752.31
|
||||
#define E2 ((WGS_84_SMA*WGS_84_SMA-WGS_84_SMB*WGS_84_SMB)/(WGS_84_SMA*WGS_84_SMA))
|
||||
#define PREC 0.000000000001
|
||||
double dist = sqrt( c_pos.x * c_pos.x + c_pos.y * c_pos.y );
|
||||
double v, old_phi, new_phi = atan2( ( double )c_pos.z, dist * ( 1 - E2 ) );
|
||||
do {
|
||||
old_phi = new_phi;
|
||||
v = WGS_84_SMA / sqrt( 1 - ( E2 * sin( old_phi ) * sin( old_phi ) ) );
|
||||
new_phi = atan2( ( ( double )c_pos.z + E2 * v * sin ( old_phi ) ), dist );
|
||||
} while ( fabs( new_phi - old_phi ) > PREC );
|
||||
e_pos->lat = new_phi;
|
||||
e_pos->lon = atan2( c_pos.y, c_pos.x );
|
||||
e_pos->h = ( dist / cos ( new_phi ) ) - v;
|
||||
}
|
||||
|
||||
|
||||
|
||||
193
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/logger.c
Normal file
193
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/logger.c
Normal file
@ -0,0 +1,193 @@
|
||||
/*
|
||||
logger.c
|
||||
Copyright (C) 2003 Antoine Drouin
|
||||
|
||||
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 <sys/types.h>
|
||||
#include <dirent.h>
|
||||
#include <libgen.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <errno.h>
|
||||
#include <sys/stat.h>
|
||||
#include <glib.h>
|
||||
|
||||
#include "logger.h"
|
||||
|
||||
//static GList* logger_parse_messages(struct PprzProtocol* protocol, xmlDocPtr doc, xmlNodePtr cur);
|
||||
static GList *logger_parse_message( struct PprzProtocol *protocol, GList *msgs,
|
||||
const guchar *buf );
|
||||
|
||||
struct Logger *logger_new( const gchar *out_file, xmlDocPtr doc )
|
||||
{
|
||||
const gchar *opening = "<data>\n";
|
||||
struct Logger *this = g_new( struct Logger, 1 );
|
||||
if ( this ) {
|
||||
GError *err = NULL;
|
||||
|
||||
gchar *copy = strdup( out_file );
|
||||
gchar *out_dir = dirname( copy );
|
||||
DIR *d;
|
||||
if ( !( d = opendir( out_dir ) ) ) {
|
||||
if ( errno == ENOENT ) {
|
||||
if ( mkdir( out_dir, S_IRWXU | S_IRWXG ) ) {
|
||||
g_message( "log directory %s doesn't exist and could not create it", out_dir );
|
||||
/* FIXME : FREE */
|
||||
return NULL;
|
||||
}
|
||||
} else {
|
||||
g_message( "could not acces log directory %s %s", out_dir,
|
||||
g_strerror( errno ) );
|
||||
/* FIXME : FREE */
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
closedir( d );
|
||||
g_free( copy );
|
||||
|
||||
xmlSaveFormatFile ( out_file, doc, 1 );
|
||||
/* FIXME : check result */
|
||||
|
||||
this->channel = g_io_channel_new_file( out_file, "a", &err );
|
||||
if ( err ) {
|
||||
g_message( "could not open log file %s : %s", out_file, err->message );
|
||||
g_error_free( err );
|
||||
g_free( this );
|
||||
return NULL;
|
||||
}
|
||||
g_io_channel_write_chars( this->channel, opening, strlen( opening ), NULL,
|
||||
NULL );
|
||||
}
|
||||
return this;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void logger_free( struct Logger *this )
|
||||
{
|
||||
const gchar *closing = "</data>\n</log>\n";
|
||||
g_io_channel_write_chars( this->channel, closing, strlen( closing ), NULL,
|
||||
NULL );
|
||||
g_io_channel_flush( this->channel, NULL );
|
||||
// g_io_channel_free(this->channel);
|
||||
g_free( this );
|
||||
}
|
||||
|
||||
void logger_log( struct Logger *this, struct PprzMsg *msg )
|
||||
{
|
||||
GString *str;
|
||||
pprz_protocol_ascii_of_msg( msg, &str );
|
||||
if ( str ) {
|
||||
GError *err = NULL;
|
||||
guint len;
|
||||
g_io_channel_write_chars ( this->channel, str->str, str->len, &len, &err );
|
||||
if ( err ) {
|
||||
g_warning( "log write failed %s", err->message );
|
||||
g_error_free( err );
|
||||
g_string_free( str, TRUE );
|
||||
return;
|
||||
}
|
||||
g_string_free( str, TRUE );
|
||||
g_io_channel_flush( this->channel, &err );
|
||||
if ( err ) {
|
||||
g_warning( "log flush failed %s", err->message );
|
||||
g_error_free( err );
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
GList *logger_parse_log( const gchar *filename,
|
||||
struct PprzProtocol **protocol )
|
||||
{
|
||||
xmlDocPtr doc;
|
||||
xmlNodePtr cur;
|
||||
|
||||
GList *msgs = NULL;
|
||||
const gchar *protocol_closure = "</protocol>";
|
||||
gchar *contents, *data_start;
|
||||
gsize len;
|
||||
GError *err;
|
||||
gboolean res = g_file_get_contents( filename, &contents, &len, &err );
|
||||
|
||||
if ( !res ) {
|
||||
g_message( "could not read file %s : %s", filename, err->message );
|
||||
g_error_free( err );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ( !( data_start = g_strstr_len( contents, len, protocol_closure ) ) ) {
|
||||
g_free( contents );
|
||||
g_message( "protocol closing not found" );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
data_start += strlen( protocol_closure );
|
||||
|
||||
if ( !( doc = xmlParseMemory( contents, data_start - contents ) ) ) {
|
||||
g_free( contents );
|
||||
g_message( "error parsing protocol xml" );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ( !( cur = xmlDocGetRootElement( doc ) ) ) {
|
||||
g_free( contents );
|
||||
g_message( "empty protocol" );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if ( !( *protocol = pprz_protocol_new_from_xml( doc, cur ) ) ) {
|
||||
g_free( contents );
|
||||
g_message( "error interpreting protocol" );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
msgs = logger_parse_message( *protocol, msgs, data_start );
|
||||
g_free( contents );
|
||||
|
||||
return msgs;
|
||||
}
|
||||
|
||||
/* static GList* logger_parse_messages(struct PprzProtocol* protocol, xmlDocPtr doc, xmlNodePtr cur) { */
|
||||
|
||||
/* if (!cur) return NULL; */
|
||||
/* if (xmlStrcmp(cur->name, (const xmlChar *) "data")) */
|
||||
/* return cur->next?logger_parse_messages(protocol, doc, cur->next):NULL; */
|
||||
/* cur = cur->xmlChildrenNode; */
|
||||
|
||||
/* if (xmlStrcmp(cur->name, (const xmlChar *) "text")) */
|
||||
/* return NULL; */
|
||||
|
||||
/* return logger_parse_message(protocol, NULL, cur->content); */
|
||||
/* } */
|
||||
|
||||
static GList *logger_parse_message( struct PprzProtocol *protocol, GList *msgs,
|
||||
const guchar *buf )
|
||||
{
|
||||
guchar *next;
|
||||
struct PprzMsg *msg = pprz_protocol_msg_new_of_ascii( protocol, buf );
|
||||
if ( msg )
|
||||
msgs = g_list_append( msgs, msg );
|
||||
if ( ( next = index( buf, '\n' ) ) )
|
||||
return logger_parse_message( protocol, msgs, next + 1 );
|
||||
return msgs;
|
||||
}
|
||||
93
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/math.c
Normal file
93
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/math.c
Normal file
@ -0,0 +1,93 @@
|
||||
/*
|
||||
Math library emulation functions
|
||||
|
||||
Copyright (C) 2007 IRIT - UPS <casse@irit.fr>
|
||||
|
||||
This file is part of papabench.
|
||||
|
||||
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 papabench; see the file COPYING. If not, write to
|
||||
the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
Boston, MA 02111-1307, USA.
|
||||
|
||||
*/
|
||||
#include <math.h>
|
||||
|
||||
double pp_atan2( double x, double y )
|
||||
{
|
||||
double coeff_1 = M_PI / 4;
|
||||
double coeff_2 = 3 * coeff_1;
|
||||
double abs_y = fabs( y ) + 1e-10;
|
||||
double angle, r;
|
||||
if ( x > 0 ) {
|
||||
r = ( x - abs_y ) / ( x + abs_y );
|
||||
angle = coeff_1 - coeff_1 * r;
|
||||
} else {
|
||||
r = ( x + abs_y ) / ( abs_y - x );
|
||||
angle = coeff_2 - coeff_1 * r;
|
||||
}
|
||||
if ( y < 0 )
|
||||
return ( -angle );
|
||||
else
|
||||
return angle;
|
||||
}
|
||||
|
||||
|
||||
/* Calculates sin(x), angle x must be in rad.
|
||||
Range: -pi/2 <= x <= pi/2
|
||||
Precision: +/- .000,000,005
|
||||
*/
|
||||
|
||||
double pp_sin( double x )
|
||||
{
|
||||
double xi, y, q, q2;
|
||||
int sign;
|
||||
|
||||
xi = x;
|
||||
sign = 1;
|
||||
_Pragma( "loopbound min 0 max 0" )
|
||||
while ( xi < -1.57079632679489661923 ) xi += 6.28318530717958647692;
|
||||
_Pragma( "loopbound min 0 max 0" )
|
||||
while ( xi > 4.71238898038468985769 ) xi -= 6.28318530717958647692;
|
||||
if ( xi > 1.57079632679489661923 ) {
|
||||
xi -= 3.141592653589793238462643;
|
||||
sign = -1;
|
||||
}
|
||||
q = xi / 1.57079632679;
|
||||
q2 = q * q;
|
||||
y = ( ( ( ( .00015148419 * q2
|
||||
- .00467376557 ) * q2
|
||||
+ .07968967928 ) * q2
|
||||
- .64596371106 ) * q2
|
||||
+ 1.57079631847 ) * q;
|
||||
return ( sign < 0 ? -y : y );
|
||||
}
|
||||
|
||||
#define SQRT_PRECISION 5
|
||||
double pp_sqrt( double n )
|
||||
{
|
||||
#if 0
|
||||
float x, m;
|
||||
int i, e;
|
||||
|
||||
/* compute the approximation */
|
||||
m = frexpf( n, &e );
|
||||
x = ldexp( m, e / 2 );
|
||||
|
||||
/* perform the computation */
|
||||
_Pragma( "loopbound min 5 max 5 )
|
||||
for ( i = 0; i < SQRT_PRECISION; i++ )
|
||||
x = ( x + n / x ) / 2;
|
||||
return x;
|
||||
#endif
|
||||
}
|
||||
192
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/network.c
Normal file
192
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/network.c
Normal file
@ -0,0 +1,192 @@
|
||||
/*
|
||||
$id:$
|
||||
Copyright (C) 2003 Antoine Drouin
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
/*
|
||||
network functions
|
||||
*/
|
||||
|
||||
#include "network.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <netdb.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#include <sys/socket.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
static gboolean server_on_data_received( GIOChannel *source,
|
||||
GIOCondition condition, gpointer data );
|
||||
static gboolean client_on_data_received( GIOChannel *source,
|
||||
GIOCondition condition, gpointer data );
|
||||
|
||||
struct NetworkServer *network_server_new( struct PprzProtocol *protocol,
|
||||
const char *bcast_addr, const int bcast_port,
|
||||
GFunc rcv_callback, GFunc err_callback, gpointer user_data )
|
||||
{
|
||||
struct NetworkServer *this = g_new( struct NetworkServer, 1 );
|
||||
if ( this ) {
|
||||
GIOChannel *channel;
|
||||
struct protoent *pte = getprotobyname( "UDP" );
|
||||
static int so_broadcast = TRUE;
|
||||
|
||||
this->bcast_socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto );
|
||||
setsockopt( this->bcast_socket, SOL_SOCKET, SO_BROADCAST, &so_broadcast,
|
||||
sizeof( so_broadcast ) );
|
||||
|
||||
this->bcast_addr.sin_family = PF_INET;
|
||||
this->bcast_addr.sin_port = htons( bcast_port );
|
||||
this->bcast_addr.sin_addr.s_addr = inet_addr( bcast_addr );
|
||||
|
||||
channel = g_io_channel_unix_new( this->bcast_socket );
|
||||
g_io_add_watch ( channel, G_IO_IN | G_IO_PRI, server_on_data_received, this );
|
||||
|
||||
this->protocol = protocol;
|
||||
|
||||
this->rcv_callback = rcv_callback;
|
||||
this->err_callback = err_callback;
|
||||
this->user_data = user_data;
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
#define FIXME_LEN 2048
|
||||
|
||||
void network_server_dispatch( struct NetworkServer *this, struct PprzMsg *msg )
|
||||
{
|
||||
guchar buf[ FIXME_LEN ];
|
||||
GTimeVal *date = ( GTimeVal * )buf;
|
||||
guchar *type = buf + sizeof( GTimeVal );
|
||||
guchar *data = type + 1;
|
||||
guint len = sizeof( GTimeVal ) + msg->class->size;
|
||||
*date = msg->date;
|
||||
*type = msg->class->id;
|
||||
memcpy( data, msg->bytes, msg->class->size - 1 );
|
||||
if ( sendto( this->bcast_socket, buf, len, 0,
|
||||
( struct sockaddr * )&this->bcast_addr, sizeof( this->bcast_addr ) ) == -1 )
|
||||
g_message( "error broadcasting" );
|
||||
}
|
||||
|
||||
struct NetworkClient *network_client_new( struct PprzProtocol *protocol,
|
||||
const char *bcast_addr, const int bcast_port,
|
||||
GFunc rcv_callback, GFunc err_callback, gpointer user_data )
|
||||
{
|
||||
struct NetworkClient *this = g_new( struct NetworkClient, 1 );
|
||||
if ( this ) {
|
||||
int so_reuseaddr = TRUE;
|
||||
GIOChannel *channel;
|
||||
struct protoent *pte = getprotobyname( "UDP" );
|
||||
|
||||
this->socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto );
|
||||
setsockopt( this->socket, SOL_SOCKET, SO_REUSEADDR, &so_reuseaddr,
|
||||
sizeof( so_reuseaddr ) );
|
||||
this->bcast_addr.sin_family = PF_INET;
|
||||
this->bcast_addr.sin_port = htons( bcast_port );
|
||||
this->bcast_addr.sin_addr.s_addr = inet_addr( bcast_addr );
|
||||
bind( this->socket, ( struct sockaddr * )&this->bcast_addr,
|
||||
sizeof( this->bcast_addr ) );
|
||||
|
||||
channel = g_io_channel_unix_new( this->socket );
|
||||
g_io_add_watch ( channel, G_IO_IN | G_IO_PRI, client_on_data_received, this );
|
||||
|
||||
this->protocol = protocol;
|
||||
|
||||
this->rcv_callback = rcv_callback;
|
||||
this->err_callback = err_callback;
|
||||
this->user_data = user_data;
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
void network_client_send_to_server( struct NetworkClient *this,
|
||||
struct PprzMsg *msg )
|
||||
{
|
||||
guchar buf[ FIXME_LEN ];
|
||||
GTimeVal *date = ( GTimeVal * )buf;
|
||||
guchar *type = buf + sizeof( GTimeVal );
|
||||
guchar *data = type + 1;
|
||||
guint len = sizeof( GTimeVal ) + msg->class->size;
|
||||
*date = msg->date;
|
||||
*type = msg->class->id;
|
||||
memcpy( data, msg->bytes, msg->class->size - 1 );
|
||||
if ( sendto( this->socket, buf, len, 0,
|
||||
( struct sockaddr * )&this->bcast_addr, sizeof( this->bcast_addr ) ) == -1 )
|
||||
g_message( "error sending to server" );
|
||||
}
|
||||
|
||||
static gboolean server_on_data_received( GIOChannel *source,
|
||||
GIOCondition condition, gpointer data )
|
||||
{
|
||||
guchar buf[ FIXME_LEN ];
|
||||
struct sockaddr_in from;
|
||||
int from_len;
|
||||
gint len;
|
||||
struct NetworkServer *this = ( struct NetworkServer * )data;
|
||||
if ( ( len = recvfrom( this->bcast_socket, buf, FIXME_LEN, 0,
|
||||
( struct sockaddr * )&from, &from_len ) ) == -1 ) {
|
||||
this->err_callback( NULL, this->user_data );
|
||||
return FALSE;
|
||||
} else {
|
||||
struct PprzMsg *msg;
|
||||
GTimeVal *date = ( GTimeVal * )buf;
|
||||
guchar *ptr_type = buf + sizeof( GTimeVal );
|
||||
if ( !( msg = pprz_protocol_msg_new_of_bin( this->protocol, ptr_type,
|
||||
*date ) ) ) {
|
||||
this->err_callback( NULL, this->user_data );
|
||||
return TRUE;
|
||||
}
|
||||
this->rcv_callback( msg, this->user_data );
|
||||
// g_message("network data received %s %d", inet_ntoa(from.sin_addr), ntohs(from.sin_port));
|
||||
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static gboolean client_on_data_received( GIOChannel *source,
|
||||
GIOCondition condition, gpointer data )
|
||||
{
|
||||
guchar buf[ FIXME_LEN ];
|
||||
struct sockaddr_in from;
|
||||
int from_len;
|
||||
gint len;
|
||||
struct NetworkClient *this = ( struct NetworkClient * )data;
|
||||
if ( ( len = recvfrom( this->socket, buf, FIXME_LEN, 0,
|
||||
( struct sockaddr * )&from, &from_len ) ) == -1 ) {
|
||||
g_message( "client_on_data_received [ %s ]", strerror( errno ) );
|
||||
this->err_callback( NULL, this->user_data );
|
||||
return TRUE;
|
||||
} else {
|
||||
struct PprzMsg *msg;
|
||||
GTimeVal *date = ( GTimeVal * )buf;
|
||||
guchar *ptr_type = buf + sizeof( GTimeVal );
|
||||
if ( !( msg = pprz_protocol_msg_new_of_bin( this->protocol, ptr_type,
|
||||
*date ) ) ) {
|
||||
this->err_callback( NULL, this->user_data );
|
||||
return TRUE;
|
||||
}
|
||||
this->rcv_callback( msg, this->user_data );
|
||||
// g_message("network data received %s %d", inet_ntoa(from.sin_addr), ntohs(from.sin_port));
|
||||
}
|
||||
this->server_addr = from;
|
||||
return TRUE;
|
||||
}
|
||||
551
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/protocol.c
Normal file
551
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/protocol.c
Normal file
@ -0,0 +1,551 @@
|
||||
/*
|
||||
$Id: protocol.c,v 1.1 2011-01-18 12:48:45 moellmer Exp $
|
||||
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
|
||||
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 "protocol.h"
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
struct PprzTypInfo {
|
||||
gchar *xml_name;
|
||||
guint size;
|
||||
gchar *default_format;
|
||||
};
|
||||
|
||||
const struct PprzTypInfo type_info[ TYP_NB ] = {
|
||||
{"uint8", 1, "%hhu"},
|
||||
{"uint16", 2, "%hu" },
|
||||
{"uint32", 4, "%u" },
|
||||
{"int8", 1, "%hhd"},
|
||||
{"int16", 2, "%hd" },
|
||||
{"int32", 4, "%d" },
|
||||
{"float", 4, "%f" },
|
||||
{"uint8_array", 1, "%0x"}
|
||||
};
|
||||
|
||||
static gboolean parse_xml_msg( struct PprzProtocol *this, xmlNodePtr ptr );
|
||||
static gboolean parse_xml_field( struct PprzMsgClass *msg_class,
|
||||
xmlNodePtr ptr );
|
||||
static void ascii_of_fields( struct PprzMsg *msg, GList *fields_class,
|
||||
GString *buf );
|
||||
static gboolean field_of_ascii( GList *fields_class, gchar *line,
|
||||
struct PprzMsg *msg );
|
||||
|
||||
static enum PprzFieldType type_of_string( const gchar *type_str );
|
||||
|
||||
static struct PprzMsgClass *msg_class_of_name( struct PprzProtocol *this,
|
||||
const gchar *name );
|
||||
static struct PprzMsgClass *msg_class_of_id( struct PprzProtocol *this,
|
||||
guchar id );
|
||||
|
||||
static struct PprzFieldClass *field_class_of_name( struct PprzMsg *msg,
|
||||
const gchar *name );
|
||||
/* static struct PprzFieldClass* field_class_of_id(struct PprzMsg* msg, guchar id); */
|
||||
|
||||
struct PprzProtocol *pprz_protocol_new_from_xml( xmlDocPtr doc,
|
||||
xmlNodePtr cur )
|
||||
{
|
||||
struct PprzProtocol *this;
|
||||
if ( !cur ) return NULL; /* empty document */
|
||||
if ( xmlStrcmp( cur->name,
|
||||
( const xmlChar * ) "protocol" ) ) /* not protocol */
|
||||
return cur->next ? pprz_protocol_new_from_xml( doc, cur->next ) : NULL;
|
||||
this = g_new( struct PprzProtocol, 1 );
|
||||
this->msgs_classes = NULL;
|
||||
this->msgs_classes_by_name = g_hash_table_new( g_str_hash, g_str_equal );
|
||||
if ( !parse_xml_msg( this, cur->xmlChildrenNode ) ) {
|
||||
g_free( this );
|
||||
return NULL;
|
||||
}
|
||||
this->nb_msgs_classes = g_list_length( this->msgs_classes );
|
||||
this->msgs_classes_by_id = g_new( struct PprzMsgClass *,
|
||||
this->nb_msgs_classes );
|
||||
{
|
||||
GList *cur = this->msgs_classes;
|
||||
guint i = 0;
|
||||
while ( cur ) {
|
||||
this->msgs_classes_by_id[ i ] = cur->data;
|
||||
i++;
|
||||
cur = cur->next;
|
||||
}
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
const gchar *pprz_protocol_str_of_field_type( enum PprzFieldType type )
|
||||
{
|
||||
return type_info[ type ].xml_name;
|
||||
}
|
||||
|
||||
const guint pprz_protocol_size_of_field_type( enum PprzFieldType type )
|
||||
{
|
||||
return type_info[ type ].size;
|
||||
}
|
||||
|
||||
static enum PprzFieldType type_of_string( const gchar *type_str )
|
||||
{
|
||||
guint ret = 0;
|
||||
while ( ret < TYP_NB && strcmp( type_info[ ret ].xml_name, type_str ) )
|
||||
ret++;
|
||||
return ret;
|
||||
}
|
||||
|
||||
struct PprzMsg *pprz_message_new( struct PprzMsgClass *msg_class,
|
||||
GTimeVal date )
|
||||
{
|
||||
struct PprzMsg *this = g_new( struct PprzMsg, 1 );
|
||||
this->date = date;
|
||||
this->class = msg_class;
|
||||
this->bytes = g_new( guchar, this->class->size - 1 );
|
||||
return this;
|
||||
}
|
||||
|
||||
struct PprzMsg *pprz_protocol_msg_new_by_id( struct PprzProtocol *this,
|
||||
guchar id, GTimeVal date )
|
||||
{
|
||||
struct PprzMsgClass *msg_class = msg_class_of_id( this, id );
|
||||
if ( msg_class ) return pprz_message_new( msg_class, date );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
struct PprzMsg *pprz_protocol_msg_new_by_name( struct PprzProtocol *this,
|
||||
const gchar *name, GTimeVal date )
|
||||
{
|
||||
struct PprzMsgClass *msg_class = msg_class_of_name( this, name );
|
||||
if ( msg_class ) return pprz_message_new( msg_class, date );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
struct PprzMsg *pprz_protocol_msg_new_of_bin( struct PprzProtocol *this,
|
||||
const guchar *buf, GTimeVal date )
|
||||
{
|
||||
struct PprzMsg *msg = pprz_protocol_msg_new_by_id( this, buf[ 0 ], date );
|
||||
if ( msg )
|
||||
memcpy( msg->bytes, buf + 1, msg->class->size - 1 );
|
||||
return msg;
|
||||
}
|
||||
|
||||
|
||||
struct PprzMsg *pprz_protocol_msg_new_of_ascii( struct PprzProtocol *this,
|
||||
const gchar *line )
|
||||
{
|
||||
gchar *kw;
|
||||
GTimeVal date;
|
||||
struct PprzMsg *msg;
|
||||
if ( sscanf( line, "%ld.%06ld %as", &date.tv_sec, &date.tv_usec, &kw ) != 3 )
|
||||
return NULL;
|
||||
if ( !( msg = pprz_protocol_msg_new_by_name( this, kw, date ) ) )
|
||||
return NULL;
|
||||
if ( msg->class->fields_classes ) { // message has arguments
|
||||
gchar *space = index( line, ' ' );
|
||||
g_assert( space );
|
||||
g_assert( space + 1 ); // checked before by sscanf
|
||||
space = index( space + 1, ' ' );
|
||||
g_assert( space ); // checked before by sscanf
|
||||
if ( !field_of_ascii( msg->class->fields_classes, space, msg ) ) {
|
||||
pprz_msg_free( msg );
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
void pprz_msg_free( struct PprzMsg *this )
|
||||
{
|
||||
g_free( this->bytes );
|
||||
g_free( this );
|
||||
}
|
||||
|
||||
gboolean pprz_msg_set_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class, gconstpointer value )
|
||||
{
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch (field_class->type) {
|
||||
case TYP_UINT_8:
|
||||
(guint8*)(msg->bytes+field_class->offset) = *(guint8*)value;
|
||||
break;
|
||||
case TYP_UINT_16:
|
||||
(guint16*)(msg->bytes+field_class->offset) = *(guint16*)value;
|
||||
break;
|
||||
case TYP_UINT_32:
|
||||
(guint32*)(msg->bytes+field_class->offset) = *(guint32*)value;
|
||||
break;
|
||||
case TYP_INT_8:
|
||||
(gint8*)(msg->bytes+field_class->offset) = *(gint8*)value;
|
||||
break;
|
||||
case TYP_INT_16:
|
||||
(gint16*)(msg->bytes+field_class->offset) = *(gint16*)value;
|
||||
break;
|
||||
case TYP_INT_32:
|
||||
(gint32*)(msg->bytes+field_class->offset) = *(gint32*)value;
|
||||
break;
|
||||
case TYP_FLOAT:
|
||||
(gfloat*)(msg->bytes+field_class->offset) = *(gfloat*)value;
|
||||
break;
|
||||
case TYP_ARRAY_UINT_8:
|
||||
// g_string_append_printf(buf, format, *((gfloat*)data));
|
||||
break;
|
||||
default:
|
||||
g_warning("in ascii of field : unknown type");
|
||||
}
|
||||
#else*/
|
||||
if ( field_class->type == TYP_UINT_8 ) {
|
||||
*( guint8 * )( msg->bytes + field_class->offset ) = *( guint8 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_16 ) {
|
||||
*( guint16 * )( msg->bytes + field_class->offset ) = *( guint16 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_32 ) {
|
||||
*( guint32 * )( msg->bytes + field_class->offset ) = *( guint32 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_8 ) {
|
||||
*( gint8 * )( msg->bytes + field_class->offset ) = *( gint8 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_16 ) {
|
||||
*( gint16 * )( msg->bytes + field_class->offset ) = *( gint16 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_32 ) {
|
||||
*( gint32 * )( msg->bytes + field_class->offset ) = *( gint32 * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_FLOAT ) {
|
||||
*( gfloat * )( msg->bytes + field_class->offset ) = *( gfloat * )value;
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_ARRAY_UINT_8 ) {
|
||||
// g_string_append_printf(buf, format, *((gfloat*)data));
|
||||
break;
|
||||
} else
|
||||
g_warning( "in ascii of field : unknown type" );
|
||||
//#endif
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
gboolean pprz_msg_set_field_by_name( struct PprzMsg *msg, const gchar *name,
|
||||
gconstpointer value )
|
||||
{
|
||||
struct PprzFieldClass *field_class = field_class_of_name( msg, name );
|
||||
if ( field_class )
|
||||
return pprz_msg_set_field( msg, field_class, value );
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
gboolean pprz_msg_set_field_by_id( struct PprzMsg *msg, guint id,
|
||||
gconstpointer value )
|
||||
{
|
||||
GList *cell = g_list_nth( msg->class->fields_classes, id );
|
||||
if ( cell )
|
||||
return pprz_msg_set_field( msg, cell->data, value );
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
|
||||
gpointer pprz_msg_get_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class )
|
||||
{
|
||||
return msg->bytes + field_class->offset;
|
||||
}
|
||||
|
||||
gpointer pprz_msg_get_field_by_name( struct PprzMsg *msg, const gchar *name )
|
||||
{
|
||||
struct PprzFieldClass *field_class = field_class_of_name( msg, name );
|
||||
if ( field_class )
|
||||
return pprz_msg_get_field( msg, field_class );
|
||||
g_message( "pprz_msg_get_field_by_name : unknown field [ %s ] in msg [ %s ]", name,
|
||||
msg->class->name );
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#define DEFAULT_LINE_LEN 256
|
||||
void pprz_protocol_ascii_of_msg( struct PprzMsg *msg, GString **buf )
|
||||
{
|
||||
*buf = g_string_sized_new( DEFAULT_LINE_LEN );
|
||||
g_string_printf( *buf, "%ld.%06ld %s ", msg->date.tv_sec, msg->date.tv_usec,
|
||||
msg->class->name );
|
||||
if ( msg->class->fields_classes ) ascii_of_fields( msg,
|
||||
msg->class->fields_classes, *buf );
|
||||
g_string_append_printf( *buf, "\n" );
|
||||
}
|
||||
|
||||
void pprz_msg_ascii_of_field( struct PprzMsg *msg,
|
||||
struct PprzFieldClass *field_class, GString *buf )
|
||||
{
|
||||
guchar *format;
|
||||
guchar *data;
|
||||
format = field_class->format;
|
||||
if ( !format ) format = type_info[ field_class->type ].default_format;
|
||||
data = msg->bytes + field_class->offset;
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch (field_class->type) {
|
||||
case TYP_UINT_8:
|
||||
g_string_append_printf(buf, format, *((guint8*)data));
|
||||
break;
|
||||
case TYP_UINT_16:
|
||||
g_string_append_printf(buf, format, *((guint16*)data));
|
||||
break;
|
||||
case TYP_UINT_32:
|
||||
g_string_append_printf(buf, format, *((guint32*)data));
|
||||
break;
|
||||
case TYP_INT_8:
|
||||
g_string_append_printf(buf, format, *((gint8*)data));
|
||||
break;
|
||||
case TYP_INT_16:
|
||||
g_string_append_printf(buf, format, *((gint16*)data));
|
||||
break;
|
||||
case TYP_INT_32:
|
||||
g_string_append_printf(buf, format, *((gint32*)data));
|
||||
break;
|
||||
case TYP_FLOAT:
|
||||
g_string_append_printf(buf, format, *((gfloat*)data));
|
||||
break;
|
||||
case TYP_ARRAY_UINT_8:
|
||||
// g_string_append_printf(buf, format, *((gfloat*)data));
|
||||
break;
|
||||
default:
|
||||
g_warning("in ascii of field : unknown type");
|
||||
}
|
||||
#else*/
|
||||
if ( field_class->type == TYP_UINT_8 ) {
|
||||
g_string_append_printf( buf, format, *( ( guint8 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_16 ) {
|
||||
g_string_append_printf( buf, format, *( ( guint16 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_32 ) {
|
||||
g_string_append_printf( buf, format, *( ( guint32 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_8 ) {
|
||||
g_string_append_printf( buf, format, *( ( gint8 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_16 ) {
|
||||
g_string_append_printf( buf, format, *( ( gint16 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_32 ) {
|
||||
g_string_append_printf( buf, format, *( ( gint32 * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_FLOAT ) {
|
||||
g_string_append_printf( buf, format, *( ( gfloat * )data ) );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_ARRAY_UINT_8 ) {
|
||||
// g_string_append_printf(buf, format, *((gfloat*)data));
|
||||
break;
|
||||
} else
|
||||
g_warning( "in ascii of field : unknown type" );
|
||||
//#endif
|
||||
}
|
||||
|
||||
|
||||
static gboolean field_of_ascii( GList *fields_classes, gchar *line,
|
||||
struct PprzMsg *msg )
|
||||
{
|
||||
struct PprzFieldClass *field_class = ( struct PprzFieldClass * )
|
||||
fields_classes->data;
|
||||
gchar *end_ptr;
|
||||
gchar *ptr_field = msg->bytes + field_class->offset;
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch (field_class->type) {
|
||||
case TYP_UINT_8:
|
||||
((guint8*)ptr_field) = (guint8)strtoul(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_UINT_16:
|
||||
((guint16*)ptr_field) = (guint16)strtoul(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_UINT_32:
|
||||
((guint32*)ptr_field) = (guint32)strtoul(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_INT_8:
|
||||
((gint8*)ptr_field) = (gint8)strtol(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_INT_16:
|
||||
((gint16*)ptr_field) = (gint16)strtol(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_INT_32:
|
||||
((gint32*)ptr_field) = (gint32)strtol(line, &end_ptr, 10);
|
||||
break;
|
||||
case TYP_FLOAT:
|
||||
((gfloat*)ptr_field) = (gfloat)strtod(line, &end_ptr);
|
||||
break;
|
||||
case TYP_ARRAY_UINT_8:
|
||||
break;
|
||||
default:
|
||||
printf("in field of ascii: unknown type\n");
|
||||
}
|
||||
#else*/
|
||||
if ( field_class->type == TYP_UINT_8 ) {
|
||||
*( ( guint8 * )ptr_field ) = ( guint8 )strtoul( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_16 ) {
|
||||
*( ( guint16 * )ptr_field ) = ( guint16 )strtoul( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_UINT_32 ) {
|
||||
*( ( guint32 * )ptr_field ) = ( guint32 )strtoul( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_8 ) {
|
||||
*( ( gint8 * )ptr_field ) = ( gint8 )strtol( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_16 ) {
|
||||
*( ( gint16 * )ptr_field ) = ( gint16 )strtol( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_INT_32 ) {
|
||||
*( ( gint32 * )ptr_field ) = ( gint32 )strtol( line, &end_ptr, 10 );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_FLOAT ) {
|
||||
*( ( gfloat * )ptr_field ) = ( gfloat )strtod( line, &end_ptr );
|
||||
break;
|
||||
} else
|
||||
if ( field_class->type == TYP_ARRAY_UINT_8 )
|
||||
break;
|
||||
else
|
||||
printf( "in field of ascii: unknown type\n" );
|
||||
//#endif
|
||||
if ( fields_classes->next )
|
||||
return field_of_ascii( fields_classes->next, end_ptr, msg );
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static void ascii_of_fields( struct PprzMsg *msg, GList *fields_class,
|
||||
GString *buf )
|
||||
{
|
||||
struct PprzFieldClass *field_class = ( struct PprzFieldClass * )
|
||||
fields_class->data;
|
||||
pprz_msg_ascii_of_field( msg, field_class, buf );
|
||||
if ( fields_class->next ) {
|
||||
g_string_append_printf( buf, " " );
|
||||
ascii_of_fields( msg, fields_class->next, buf );
|
||||
}
|
||||
}
|
||||
|
||||
static gboolean parse_xml_msg( struct PprzProtocol *this, xmlNodePtr ptr )
|
||||
{
|
||||
struct PprzMsgClass *new_msg_class;
|
||||
if ( !xmlNodeIsText ( ptr ) ) {
|
||||
if ( xmlStrcmp( ptr->name, ( const xmlChar * )"message" ) ) /* not a message */
|
||||
return FALSE;
|
||||
new_msg_class = g_new( struct PprzMsgClass, 1 );
|
||||
new_msg_class->id = g_list_length( this->msgs_classes );
|
||||
new_msg_class->name = xmlGetProp( ptr, "id" );
|
||||
new_msg_class->fields_classes = NULL;
|
||||
new_msg_class->fields_classes_by_name = g_hash_table_new( g_str_hash,
|
||||
g_str_equal );
|
||||
new_msg_class->size = 1;
|
||||
this->msgs_classes = g_list_append( this->msgs_classes, new_msg_class );
|
||||
g_hash_table_insert( this->msgs_classes_by_name, new_msg_class->name,
|
||||
new_msg_class );
|
||||
if ( ptr->xmlChildrenNode &&
|
||||
!parse_xml_field( new_msg_class, ptr->xmlChildrenNode ) )
|
||||
/* fixme : free space */
|
||||
return FALSE;
|
||||
new_msg_class->nb_fields_classes = g_list_length(
|
||||
new_msg_class->fields_classes );
|
||||
new_msg_class->fields_classes_by_id = g_new( struct PprzFieldClass *,
|
||||
new_msg_class->nb_fields_classes );
|
||||
{
|
||||
GList *cur = new_msg_class->fields_classes;
|
||||
guint i = 0;
|
||||
while ( cur ) {
|
||||
new_msg_class->fields_classes_by_id[ i ] = cur->data;
|
||||
i++;
|
||||
cur = cur->next;
|
||||
}
|
||||
}
|
||||
}
|
||||
if ( ptr->next )
|
||||
return parse_xml_msg( this, ptr->next );
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static gboolean parse_xml_field( struct PprzMsgClass *msg_class,
|
||||
xmlNodePtr ptr )
|
||||
{
|
||||
struct PprzFieldClass *new_field_class;
|
||||
xmlChar *type_str;
|
||||
if ( !xmlNodeIsText ( ptr ) ) {
|
||||
if ( xmlStrcmp( ptr->name, ( const xmlChar * )"field" ) ) /* not a field */
|
||||
return FALSE;
|
||||
new_field_class = g_new( struct PprzFieldClass, 1 );
|
||||
new_field_class->name = xmlGetProp( ptr, "id" );
|
||||
new_field_class->format = xmlGetProp( ptr, "format" );
|
||||
new_field_class->unit = xmlGetProp( ptr, "unit" );
|
||||
new_field_class->description = xmlGetProp( ptr, "description" );
|
||||
type_str = xmlGetProp( ptr, "type" );
|
||||
if ( ( new_field_class->type = type_of_string( type_str ) ) >= TYP_NB ) {
|
||||
/* fixme : free space */
|
||||
return FALSE;
|
||||
}
|
||||
xmlFree( type_str );
|
||||
msg_class->fields_classes = g_list_append( msg_class->fields_classes,
|
||||
new_field_class );
|
||||
g_hash_table_insert( msg_class->fields_classes_by_name, new_field_class->name,
|
||||
new_field_class );
|
||||
new_field_class->offset = msg_class->size - 1;
|
||||
msg_class->size += type_info[ new_field_class->type ].size;
|
||||
}
|
||||
if ( ptr->next )
|
||||
/* check return and free space */
|
||||
return parse_xml_field( msg_class, ptr->next );
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
static struct PprzMsgClass *msg_class_of_name( struct PprzProtocol *this,
|
||||
const gchar *name )
|
||||
{
|
||||
return g_hash_table_lookup( this->msgs_classes_by_name, name );
|
||||
}
|
||||
|
||||
static struct PprzMsgClass *msg_class_of_id( struct PprzProtocol *this,
|
||||
guchar id )
|
||||
{
|
||||
if ( id >= this->nb_msgs_classes )
|
||||
return NULL;
|
||||
return this->msgs_classes_by_id[ id ];
|
||||
}
|
||||
|
||||
static struct PprzFieldClass *field_class_of_name( struct PprzMsg *msg,
|
||||
const gchar *name )
|
||||
{
|
||||
return g_hash_table_lookup( msg->class->fields_classes_by_name, name );
|
||||
}
|
||||
|
||||
/* static struct PprzFieldClass* field_class_of_id(struct PprzMsg* msg, guchar id) { */
|
||||
/* GList* cell = g_list_nth(msg->class->fields_classes, id); */
|
||||
/* return cell?cell->data:NULL; */
|
||||
/* } */
|
||||
52
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/traces.c
Normal file
52
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/traces.c
Normal file
@ -0,0 +1,52 @@
|
||||
/*
|
||||
traces.c
|
||||
Copyright (C) 2003 Antoine Drouin
|
||||
|
||||
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 "traces.h"
|
||||
|
||||
#ifdef TRACE_ON
|
||||
unsigned long TraceLevel = TRACE_ERROR | TRACE_TRANSPORT
|
||||
; // | TRACE_MODEM_READ ;// | TRACE_DATA_READ | TRACE_DOWNLINK; //| TRACE_MODEM_READ;// ;//| TRACE_MSG// | TRACE_DOWNLINK;
|
||||
|
||||
// unsigned long TraceLevel = TRACE_DOWNLINK | TRACE_MSG | TRACE_ERROR ;
|
||||
|
||||
const char *print_hex( const unsigned char *buf, unsigned int len )
|
||||
{
|
||||
const char d2h[ ] = "0123456789ABCDEF";
|
||||
int i = 0;
|
||||
#define HEX_BUF_LEN 1024
|
||||
#define FMT_LEN 3
|
||||
#define FMT "%c%c "
|
||||
static unsigned char priv_buf[ HEX_BUF_LEN ];
|
||||
/* printf("[ %d ] ", len); */
|
||||
for ( i = 0; i < len && i < HEX_BUF_LEN / FMT_LEN; i++ ) {
|
||||
// printf(FMT, d2h[ (int)buf[ i ]/16 ], d2h[ (int)buf[ i ]%16 ]);
|
||||
sprintf( priv_buf + FMT_LEN * i, FMT, d2h[ ( int )buf[ i ] / 16 ],
|
||||
d2h[ ( int )buf[ i ] % 16 ] );
|
||||
}
|
||||
if ( len == 0 )
|
||||
priv_buf[ 0 ] = '\0';
|
||||
return priv_buf;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
244
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/transport.c
Normal file
244
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/transport.c
Normal file
@ -0,0 +1,244 @@
|
||||
#include "transport.h"
|
||||
|
||||
|
||||
#include "geometry.h"
|
||||
#include "traces.h"
|
||||
|
||||
static void parse_buf( struct Transport *this );
|
||||
static gboolean check_checksum( struct Transport *this, guint msg_len );
|
||||
static void remove_n_from_buf( struct Transport *this, guint len );
|
||||
static void remove_until_stx( struct Transport *this );
|
||||
|
||||
struct Transport *transport_new( gboolean fixed_size,
|
||||
gboolean two_bytes_checksum, guint nb_msg,
|
||||
guint *size_msg, guint max_msg_size,
|
||||
guchar stx, guchar etx,
|
||||
void( *err_callback )( gpointer callback_data, GError *error ),
|
||||
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg ),
|
||||
gpointer callback_data )
|
||||
{
|
||||
struct Transport *this = g_new( struct Transport, 1 );
|
||||
if ( this ) {
|
||||
this->stx = stx;
|
||||
this->etx = etx;
|
||||
this->err_callback = err_callback;
|
||||
this->msg_callback = msg_callback;
|
||||
this->callback_data = callback_data;
|
||||
this->buf_len = 0;
|
||||
this->fixed_size = fixed_size;
|
||||
this->two_bytes_checksum = two_bytes_checksum;
|
||||
this->quark = g_quark_from_string( "Transport" );
|
||||
g_get_current_time( &this->start_date );
|
||||
if ( fixed_size ) {
|
||||
this->nb_msg_type = nb_msg;
|
||||
this->size_msg = size_msg;
|
||||
//this->max_msg_size = FIXME find max size in array;;
|
||||
} else
|
||||
this->max_msg_size = max_msg_size;
|
||||
this->nb_byte_last_status = 0;
|
||||
this->nb_msg_last_status = 0;
|
||||
this->status.nb_byte = 0;
|
||||
this->status.nb_msg = 0;
|
||||
this->status.nb_err = 0;
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
#define GDOUBLE_FROM_DIF_TV(a,b) ((gdouble)(a.tv_sec-b.tv_sec) + 1e-6 * (gdouble)(a.tv_usec-b.tv_usec))
|
||||
|
||||
struct TransportStatus *transport_get_status( struct Transport *this )
|
||||
{
|
||||
GTimeVal now;
|
||||
gdouble duration;
|
||||
|
||||
g_get_current_time( &now );
|
||||
this->status.run_time = DELAY_SEC_OF_TIMEVAL( now, this->start_date );
|
||||
|
||||
duration = GDOUBLE_FROM_DIF_TV( now, this->last_status_date );
|
||||
this->status.byte_rate = this->nb_byte_last_status / duration;
|
||||
this->status.msg_rate = this->nb_msg_last_status / duration;
|
||||
|
||||
this->nb_byte_last_status = 0;
|
||||
this->nb_msg_last_status = 0;
|
||||
this->last_status_date = now;
|
||||
|
||||
return &this->status;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void transport_free( struct Transport *this )
|
||||
{
|
||||
g_free( this );
|
||||
}
|
||||
|
||||
void transport_feed_data( struct Transport *this, const guchar *buf,
|
||||
guint len )
|
||||
{
|
||||
if ( len >= TRANSPORT_BUF_LEN - this->buf_len ) {
|
||||
GError *err = g_error_new( this->quark,
|
||||
TRANSPORT_BUF_OVFW, /* guint code */
|
||||
"buffer overflow" /* const gchar *format */
|
||||
);
|
||||
this->err_callback( this->callback_data, err );
|
||||
g_error_free( err );
|
||||
} else {
|
||||
this->status.nb_byte += len;
|
||||
this->nb_byte_last_status += len;
|
||||
memcpy( this->buf + this->buf_len, buf, len );
|
||||
this->buf_len += len;
|
||||
parse_buf( this );
|
||||
}
|
||||
}
|
||||
|
||||
static gboolean check_checksum( struct Transport *this, guint msg_len )
|
||||
{
|
||||
guint checksum_idx = msg_len - TRANSPORT_TAIL_LEN;
|
||||
guint i;
|
||||
if ( this->two_bytes_checksum ) {
|
||||
guchar cka = 0, ckb = 0;
|
||||
for ( i = TRANSPORT_PAYLOAD_OFFSET; i < checksum_idx; i++ ) {
|
||||
cka += this->buf[ i ];
|
||||
ckb += cka;
|
||||
}
|
||||
return ( cka == this->buf[ checksum_idx ] &&
|
||||
ckb == this->buf[ checksum_idx + 1 ] );
|
||||
} else {
|
||||
guchar checksum = 0;
|
||||
for ( i = TRANSPORT_PAYLOAD_OFFSET; i < checksum_idx; i++ )
|
||||
checksum ^= this->buf[ i ];
|
||||
if ( checksum != this->buf[ checksum_idx ] )
|
||||
TRACE( TRACE_ERROR, "transport checksum error (found : 0x%02X read : 0x%02X)\n",
|
||||
checksum, this->buf[ checksum_idx ] );
|
||||
return checksum == this->buf[ checksum_idx ];
|
||||
}
|
||||
}
|
||||
|
||||
static void remove_n_from_buf( struct Transport *this, guint len )
|
||||
{
|
||||
memmove( this->buf, this->buf + len, this->buf_len - len );
|
||||
this->buf_len -= len;
|
||||
}
|
||||
|
||||
static void remove_until_stx( struct Transport *this )
|
||||
{
|
||||
guchar *stx = memchr( this->buf, this->stx, this->buf_len );
|
||||
if ( stx ) {
|
||||
if ( stx != this->buf ) {
|
||||
memmove( this->buf, stx, this->buf_len - ( stx - this->buf ) );
|
||||
this->buf_len -= ( stx - this->buf );
|
||||
}
|
||||
} else
|
||||
this->buf_len = 0;
|
||||
}
|
||||
|
||||
static gboolean get_msg_len( struct Transport *this, guint *len )
|
||||
{
|
||||
if ( this->fixed_size ) {
|
||||
guint type = this->buf[ TRANSPORT_PAYLOAD_OFFSET ];
|
||||
if ( type >= this->nb_msg_type ) { /* BAP ID */
|
||||
TRACE( TRACE_TRANSPORT, "%d bad type\n", this->buf[ TRANSPORT_PAYLOAD_OFFSET ] );
|
||||
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
|
||||
this->buf_len ) );
|
||||
this->status.nb_err++;
|
||||
{
|
||||
GError *err = g_error_new( this->quark,
|
||||
TRANSPORT_INVALID_MSG_ID, /* guint code */
|
||||
"invalid msg id" /* const gchar *format */
|
||||
);
|
||||
this->err_callback( this->callback_data, err );
|
||||
g_error_free( err );
|
||||
}
|
||||
return FALSE;
|
||||
} else *len = this->size_msg[ type ] + TRANSPORT_HEAD_LEN + TRANSPORT_TAIL_LEN;
|
||||
} else {
|
||||
*len = this->buf[ TRANSPORT_PAYLOAD_OFFSET ] + TRANSPORT_HEAD_LEN +
|
||||
TRANSPORT_TAIL_LEN;
|
||||
if ( *len >= this->max_msg_size ) { /* BAD LEN */
|
||||
TRACE( TRACE_TRANSPORT, "%d bad len\n", this->buf[ TRANSPORT_PAYLOAD_OFFSET ] );
|
||||
this->status.nb_err++;
|
||||
{
|
||||
GError *err = g_error_new( this->quark,
|
||||
TRANSPORT_INVALID_MSG_LEN, /* guint code */
|
||||
"invalid message len" /* const gchar *format */
|
||||
);
|
||||
this->err_callback( this->callback_data, err );
|
||||
g_error_free( err );
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
static void parse_buf( struct Transport *this )
|
||||
{
|
||||
/* make sure first byte in buffer is STX */
|
||||
remove_until_stx( this );
|
||||
// if (this->fixed_size)
|
||||
// TRACE(TRACE_TRANSPORT, "parse_buf %s\n", print_hex(this->buf, this->buf_len));
|
||||
if ( this->buf_len < TRANSPORT_PAYLOAD_OFFSET ) /* NO PAYLOAD */
|
||||
TRACE( TRACE_TRANSPORT_VERB, "no payload\n" );
|
||||
else {
|
||||
guint msg_len;
|
||||
if ( !get_msg_len( this,
|
||||
&msg_len ) ) { /* BAD LEN ( or TYPE) */
|
||||
remove_n_from_buf( this, 1 );
|
||||
parse_buf( this );
|
||||
} else {
|
||||
if ( this->buf_len < msg_len ) /* NOT ENOUGTH DATA */
|
||||
TRACE( TRACE_TRANSPORT_VERB, "not enough data\n" );
|
||||
else {
|
||||
if ( !check_checksum( this, msg_len ) ) {
|
||||
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
|
||||
this->buf_len ) );
|
||||
this->status.nb_err++;
|
||||
{
|
||||
GError *err = g_error_new( this->quark,
|
||||
TRANSPORT_CHECKSUM_ERROR, /* guint code */
|
||||
"checksum error" /* const gchar *format */
|
||||
);
|
||||
this->err_callback( this->callback_data, err );
|
||||
g_error_free( err );
|
||||
}
|
||||
|
||||
remove_n_from_buf( this, 1 );
|
||||
parse_buf( this );
|
||||
} else {
|
||||
if ( !this->two_bytes_checksum && this->buf[ msg_len - 1 ] != this->etx ) {
|
||||
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
|
||||
this->buf_len ) );
|
||||
TRACE( TRACE_TRANSPORT_VERB, "%d is not ETX\n", this->buf[ msg_len - 1 ] );
|
||||
this->status.nb_err++;
|
||||
{
|
||||
GError *err = g_error_new( this->quark,
|
||||
TRANSPORT_NO_ETX, /* guint code */
|
||||
"NO ETX" /* const gchar *format */
|
||||
);
|
||||
this->err_callback( this->callback_data, err );
|
||||
g_error_free( err );
|
||||
}
|
||||
remove_n_from_buf( this, 1 );
|
||||
parse_buf( this );
|
||||
} else {
|
||||
struct TransportMsg msg;
|
||||
guint payload_len = msg_len - TRANSPORT_HEAD_LEN - TRANSPORT_TAIL_LEN;
|
||||
g_get_current_time( &msg.date );
|
||||
msg.len = payload_len;
|
||||
msg.data = this->buf + TRANSPORT_PAYLOAD_OFFSET;
|
||||
this->status.nb_msg++;
|
||||
this->nb_msg_last_status++;
|
||||
|
||||
TRACE( TRACE_TRANSPORT_VERB,
|
||||
"transport msg date : %ld%06ld len : %d data : %s\n", msg.date.tv_sec,
|
||||
msg.date.tv_usec, msg.len, print_hex( msg.data, msg.len ) );
|
||||
this->msg_callback( this->callback_data, &msg );
|
||||
remove_n_from_buf( this, msg_len );
|
||||
parse_buf( this );
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,12 @@
|
||||
int main( int argc, char **argv, char **envp );
|
||||
|
||||
static char *argv[ ] = { "", 0 };
|
||||
static char *envp[ ] = { 0 };
|
||||
|
||||
void _start( void )
|
||||
{
|
||||
__asm( "lis 1," STACK ">>16" );
|
||||
__asm( "addis 1,1," STACK "&0xffff" );
|
||||
main( 1, argv, envp );
|
||||
while ( 1 );
|
||||
}
|
||||
116
targets/wasm-tacle/parallel/PapaBench/sw/var/include/airframe.h
Normal file
116
targets/wasm-tacle/parallel/PapaBench/sw/var/include/airframe.h
Normal file
@ -0,0 +1,116 @@
|
||||
/* This file has been generated from conf/airframes/twinstar3.xml */
|
||||
/* Please DO NOT EDIT */
|
||||
|
||||
#ifndef AIRFRAME_H
|
||||
#define AIRFRAME_H
|
||||
|
||||
#define AIRFRAME_NAME "Twinstar trois"
|
||||
|
||||
#define CTL_BRD_V1_2_1 1
|
||||
|
||||
#define ADC_CHANNEL_IR1 1
|
||||
#define ADC_CHANNEL_IR2 2
|
||||
|
||||
#define NB_SERVO 6
|
||||
|
||||
#define SERVO_MOTOR_LEFT 3
|
||||
#define SERVO_MOTOR_LEFT_TRAVEL 0.833333333333
|
||||
#define SERVOS_NEUTRALS_3 1000
|
||||
|
||||
#define SERVO_MOTOR_RIGHT 9
|
||||
#define SERVO_MOTOR_RIGHT_TRAVEL 0.833333333333
|
||||
#define SERVOS_NEUTRALS_9 1000
|
||||
|
||||
#define SERVO_AILERON_LEFT 0
|
||||
#define SERVO_AILERON_LEFT_TRAVEL -0.55
|
||||
#define SERVOS_NEUTRALS_0 1600
|
||||
|
||||
#define SERVO_AILERON_RIGHT 2
|
||||
#define SERVO_AILERON_RIGHT_TRAVEL -0.520833333333
|
||||
#define SERVOS_NEUTRALS_2 1650
|
||||
|
||||
#define SERVO_ELEVATOR 6
|
||||
#define SERVO_ELEVATOR_TRAVEL 0.496666666667
|
||||
#define SERVOS_NEUTRALS_6 1530
|
||||
|
||||
#define SERVO_RUDDER 7
|
||||
#define SERVO_RUDDER_TRAVEL -0.483333333333
|
||||
#define SERVOS_NEUTRALS_7 1450
|
||||
|
||||
|
||||
#define SERVOS_MINS {1850,1000,2000,1000,1000,1000,1220,1850,1000,1000}
|
||||
#define SERVOS_NEUTRALS {1600,1500,1650,1000,1500,1500,1530,1450,1500,1000}
|
||||
#define SERVOS_MAXS {1190,2000,1375,2000,2000,2000,1816,1270,2000,2000}
|
||||
|
||||
#define SERVO_MIN_US 1000ul
|
||||
#define SERVO_MAX_US 2000ul
|
||||
|
||||
#define ServoSet(values) { \
|
||||
uint16_t servo_value;\
|
||||
int16_t _var_roll;\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_MOTOR_LEFT) + (int16_t)((2 * values[ RADIO_GAIN1 ])*SERVO_MOTOR_LEFT_TRAVEL);\
|
||||
servo_widths[ SERVO_MOTOR_LEFT ] = ChopServo(servo_value);\
|
||||
\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_MOTOR_RIGHT) + (int16_t)((2 * values[ RADIO_THROTTLE ])*SERVO_MOTOR_RIGHT_TRAVEL);\
|
||||
servo_widths[ SERVO_MOTOR_RIGHT ] = ChopServo(servo_value);\
|
||||
\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_ELEVATOR) + (int16_t)((values[ RADIO_PITCH ])*SERVO_ELEVATOR_TRAVEL);\
|
||||
servo_widths[ SERVO_ELEVATOR ] = ChopServo(servo_value);\
|
||||
\
|
||||
_var_roll = values[ RADIO_ROLL ];\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_AILERON_LEFT) + (int16_t)(((_var_roll > 0 ? 1 : AILERON_DIFF) * _var_roll)*SERVO_AILERON_LEFT_TRAVEL);\
|
||||
servo_widths[ SERVO_AILERON_LEFT ] = ChopServo(servo_value);\
|
||||
\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_AILERON_RIGHT) + (int16_t)(((_var_roll > 0 ? AILERON_DIFF : 1) * _var_roll)*SERVO_AILERON_RIGHT_TRAVEL);\
|
||||
servo_widths[ SERVO_AILERON_RIGHT ] = ChopServo(servo_value);\
|
||||
\
|
||||
servo_value = SERVO_NEUTRAL(SERVO_RUDDER) + (int16_t)((values[ RADIO_YAW ] + values[ RADIO_ROLL ]*COMBI_SWITCH)*SERVO_RUDDER_TRAVEL);\
|
||||
servo_widths[ SERVO_RUDDER ] = ChopServo(servo_value);\
|
||||
\
|
||||
}
|
||||
#define AILERON_DIFF 0.66
|
||||
#define COMBI_SWITCH 1.0
|
||||
|
||||
#define IR_ROLL_NEUTRAL_DEFAULT -915
|
||||
#define IR_PITCH_NEUTRAL_DEFAULT 110
|
||||
#define IR_DEFAULT_CONTRAST 200
|
||||
#define IR_RAD_OF_IR_CONTRAST 0.75
|
||||
#define IR_RollOfIrs(x1,x2) (-1*(x1)+ -1*(x2))
|
||||
#define IR_PitchOfIrs(x1,x2) (-1*(x1)+ 1*(x2))
|
||||
#define IR_RAD_OF_IR_MAX_VAL 0.0045
|
||||
#define IR_RAD_OF_IR_MIN_VAL 0.00075
|
||||
|
||||
#define ROLL_PGAIN 10000.
|
||||
#define PITCH_OF_ROLL 0.0
|
||||
#define PITCH_PGAIN 15000.
|
||||
#define MAX_ROLL 0.35
|
||||
#define MAX_PITCH 0.35
|
||||
#define MIN_PITCH -0.35
|
||||
|
||||
#define CLIMB_PITCH_PGAIN -0.1
|
||||
#define CLIMB_PITCH_IGAIN 0.025
|
||||
#define CLIMB_PGAIN -0.03
|
||||
#define CLIMB_IGAIN 0.1
|
||||
#define CLIMB_MAX 1.
|
||||
#define CLIMB_LEVEL_GAZ 0.31
|
||||
#define CLIMB_PITCH_OF_VZ_PGAIN 0.05
|
||||
#define CLIMB_GAZ_OF_CLIMB 0.2
|
||||
|
||||
#define COURSE_PGAIN -0.2
|
||||
#define ALTITUDE_PGAIN -0.025
|
||||
#define NAV_PITCH 0.
|
||||
|
||||
#define VOLTAGE_ADC_A 0.0175
|
||||
#define VOLTAGE_ADC_B 0.088
|
||||
#define VoltageOfAdc(adc) (VOLTAGE_ADC_A * adc + VOLTAGE_ADC_B)
|
||||
#define LOW_BATTERY 93
|
||||
|
||||
#define NOMINAL_AIRSPEED 10.
|
||||
#define CARROT 5.
|
||||
|
||||
#define ROLL_RESPONSE_FACTOR 4.
|
||||
#define YAW_RESPONSE_FACTOR 4.
|
||||
#define WEIGHT 1.3
|
||||
|
||||
|
||||
#endif // AIRFRAME_H
|
||||
@ -0,0 +1,546 @@
|
||||
/* This file has been generated from conf/flight_plans/braunschweig.xml */
|
||||
/* Please DO NOT EDIT */
|
||||
|
||||
#ifndef FLIGHT_PLAN_H
|
||||
#define FLIGHT_PLAN_H
|
||||
|
||||
#define FLIGHT_PLAN_NAME "EMAV 2004, 8 shape"
|
||||
#define NAV_UTM_EAST0 605530
|
||||
#define NAV_UTM_NORTH0 5797350
|
||||
#define QFU 270.0
|
||||
#define WP_HOME 0
|
||||
#define WAYPOINTS { \
|
||||
{0.0, 0.0, 200},\
|
||||
{0.0, 0.0, 200},\
|
||||
{115.0, -75.0, 200},\
|
||||
{156.7, -41.7, 200},\
|
||||
{115.0, 0.0, 200},\
|
||||
{0.0, -75.0, 200},\
|
||||
{-51.7, -36.7, 200},\
|
||||
};
|
||||
#define NB_WAYPOINT 7
|
||||
#define GROUND_ALT 125.
|
||||
#define SECURITY_ALT 150.
|
||||
#define MAX_DIST_FROM_HOME 500.
|
||||
#ifdef NAV_C
|
||||
|
||||
static inline void auto_nav( void )
|
||||
{
|
||||
/*#ifdef WITH_SWITCH
|
||||
switch (nav_block) {
|
||||
Block(0) // init
|
||||
switch(nav_stage) {
|
||||
Label(while_1)
|
||||
Stage(0)
|
||||
if (! (!(estimator_flight_time))) Goto(endwhile_2) else NextStage();
|
||||
Stage(1)
|
||||
Goto(while_1)
|
||||
Label(endwhile_2)
|
||||
Stage(2)
|
||||
if ((estimator_flight_time>8)) NextStage() else {
|
||||
desired_course = RadOfDeg(QFU);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.150000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
|
||||
nav_desired_gaz = TRIM_UPPRZ(0.800000*MAX_PPRZ);
|
||||
}
|
||||
return;
|
||||
Stage(3)
|
||||
if ((estimator_z>SECURITY_ALT)) NextStage() else {
|
||||
desired_course = RadOfDeg(QFU);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_CLIMB;
|
||||
desired_climb = 8.000000;
|
||||
}
|
||||
return;
|
||||
Stage(4)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
Block(1) // two
|
||||
if RcEvent1() { GotoBlock(2) }
|
||||
switch(nav_stage) {
|
||||
Label(while_3)
|
||||
Stage(0)
|
||||
if (! (TRUE)) Goto(endwhile_4) else NextStage();
|
||||
Stage(1)
|
||||
if (approaching(1)) NextStageFrom(1) else {
|
||||
fly_to(1);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(2)
|
||||
if (approaching(4)) NextStageFrom(4) else {
|
||||
fly_to(4);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(3)
|
||||
Goto(while_3)
|
||||
Label(endwhile_4)
|
||||
Stage(4)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
Block(2) // height
|
||||
if RcEvent1() { GotoBlock(3) }
|
||||
switch(nav_stage) {
|
||||
Label(while_5)
|
||||
Stage(0)
|
||||
if (! (TRUE)) Goto(endwhile_6) else NextStage();
|
||||
Stage(1)
|
||||
if (approaching(6)) NextStageFrom(6) else {
|
||||
fly_to(6);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 6 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(2)
|
||||
if (approaching(1)) NextStageFrom(1) else {
|
||||
fly_to(1);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(3)
|
||||
if (approaching(2)) NextStageFrom(2) else {
|
||||
route_to(last_wp, 2);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 2 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(4)
|
||||
if (approaching(3)) NextStageFrom(3) else {
|
||||
fly_to(3);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 3 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(5)
|
||||
if (approaching(4)) NextStageFrom(4) else {
|
||||
fly_to(4);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(6)
|
||||
if (approaching(5)) NextStageFrom(5) else {
|
||||
route_to(last_wp, 5);
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 5 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
Stage(7)
|
||||
Goto(while_5)
|
||||
Label(endwhile_6)
|
||||
Stage(8)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
Block(3) // xyz
|
||||
if RcEvent1() { GotoBlock(4) }
|
||||
switch(nav_stage) {
|
||||
Stage(0)
|
||||
|
||||
|
||||
//NormCourse(x) { while (x < 0) x += 360; while (x >= 360) x -= 360;}
|
||||
|
||||
|
||||
//CircleXY(x,y,radius) { float alpha = atan2(estimator_y - y, estimator_x - x);float alpha_carrot = alpha + CARROT / (-radius * estimator_hspeed_mod);
|
||||
//fly_to_xy(x+cos(alpha_carrot)*fabs(radius),y+sin(alpha_carrot)*fabs(radius)); qdr = DegOfRad(M_PI/2 - alpha_carrot); NormCourse(qdr);}
|
||||
|
||||
|
||||
//Goto3D(50)
|
||||
{static float carrot_x, carrot_y; int16_t pitch; int16_t roll; if (pprz_mode == PPRZ_MODE_AUTO2) { int16_t yaw = from_fbw.channels[ RADIO_YAW ]; if (yaw >
|
||||
MIN_DX || yaw < -MIN_DX) { carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.);carrot_x = Min(carrot_x, MAX_DIST_CARROT); carrot_x = Max(carrot_x, -MAX_DIST_CARROT); }
|
||||
pitch = from_fbw.channels[ RADIO_PITCH ]; if (pitch > MIN_DX || pitch < -MIN_DX) { carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); carrot_y = Min(carrot_y,
|
||||
MAX_DIST_CARROT); carrot_y = Max(carrot_y, -MAX_DIST_CARROT);} vertical_mode = VERTICAL_MODE_AUTO_ALT; roll = from_fbw.channels[ RADIO_ROLL ]; if (roll >
|
||||
MIN_DX || roll < -MIN_DX) { desired_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); desired_altitude = Max(desired_altitude, MIN_HEIGHT_CARROT+GROUND_ALT);
|
||||
desired_altitude = Min(desired_altitude, MAX_HEIGHT_CARROT+GROUND_ALT); } } { float alpha = atan2(estimator_y - carrot_y, estimator_x - carrot_x);float alpha_carrot = alpha + CARROT / (-50 * estimator_hspeed_mod);
|
||||
fly_to_xy(carrot_x+cos(alpha_carrot)*fabs(50),carrot_y+sin(alpha_carrot)*fabs(50)); qdr = DegOfRad(M_PI/2 - alpha_carrot); { while (qdr < 0) qdr += 360;
|
||||
while (qdr >= 360) qdr -= 360;}} }
|
||||
|
||||
|
||||
return;
|
||||
Stage(1)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
Block(4) // circle
|
||||
if RcEvent1() { GotoBlock(5) }
|
||||
switch(nav_stage) {
|
||||
Stage(0)
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 0 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle(0, 150);
|
||||
return;
|
||||
Stage(1)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
Block(5) // hippo
|
||||
if RcEvent1() { GotoBlock(1) }
|
||||
switch(nav_stage) {
|
||||
Label(while_7)
|
||||
Stage(0)
|
||||
if (! (TRUE)) Goto(endwhile_8) else NextStage();
|
||||
Stage(1)
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle(1, 100);
|
||||
if (Qdr(0)) NextStage();
|
||||
return;
|
||||
Stage(2)
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle(4, 100);
|
||||
if (Qdr(180)) NextStage();
|
||||
return;
|
||||
Stage(3)
|
||||
Goto(while_7)
|
||||
Label(endwhile_8)
|
||||
Stage(4)
|
||||
NextBlock()
|
||||
}
|
||||
|
||||
}
|
||||
#else*/
|
||||
if ( nav_block == 0 ) {
|
||||
// init
|
||||
|
||||
if ( nav_stage == 0 ) {
|
||||
Label( while_1 )
|
||||
nav_stage = 0;
|
||||
if ( ! ( !( estimator_flight_time ) ) ) Goto( endwhile_2 )
|
||||
else NextStage();
|
||||
} else
|
||||
if ( nav_stage == 1 ) {
|
||||
Goto( while_1 )
|
||||
} else
|
||||
if ( nav_stage == 2 ) {
|
||||
Label( endwhile_2 )
|
||||
nav_stage = 2;
|
||||
if ( ( estimator_flight_time > 8 ) ) NextStage() else {
|
||||
desired_course = RadOfDeg( QFU );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.150000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_GAZ;
|
||||
nav_desired_gaz = TRIM_UPPRZ( 0.800000 * MAX_PPRZ );
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 3 ) {
|
||||
nav_stage = 3;
|
||||
if ( ( estimator_z > SECURITY_ALT ) ) NextStage() else {
|
||||
desired_course = RadOfDeg( QFU );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_CLIMB;
|
||||
desired_climb = 8.000000;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 4 )
|
||||
NextBlock()
|
||||
else { }
|
||||
} else
|
||||
if ( nav_block == 1 ) { // two
|
||||
nav_block = 1;
|
||||
if RcEvent1() {
|
||||
GotoBlock( 2 )
|
||||
}
|
||||
if ( nav_stage == 0 ) {
|
||||
Label( while_3 )
|
||||
if ( ! ( TRUE ) ) Goto( endwhile_4 ) else NextStage();
|
||||
} else
|
||||
if ( nav_stage == 1 ) {
|
||||
nav_stage = 1;
|
||||
if ( approaching( 1 ) ) NextStageFrom( 1 ) else {
|
||||
fly_to( 1 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 2 ) {
|
||||
nav_stage = 2;
|
||||
if ( approaching( 4 ) ) NextStageFrom( 4 ) else {
|
||||
fly_to( 4 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 3 ) {
|
||||
nav_stage = 3;
|
||||
Goto( while_3 )
|
||||
} else
|
||||
if ( nav_stage == 4 ) {
|
||||
Label( endwhile_4 )
|
||||
nav_stage = 4;
|
||||
NextBlock()
|
||||
} else { }
|
||||
} else
|
||||
if ( nav_block == 2 ) { // height
|
||||
nav_block = 2;
|
||||
if RcEvent1() {
|
||||
GotoBlock( 3 )
|
||||
}
|
||||
|
||||
if ( nav_stage == 0 ) {
|
||||
Label( while_5 )
|
||||
nav_stage = 0;
|
||||
if ( ! ( TRUE ) ) Goto( endwhile_6 ) else NextStage();
|
||||
} else
|
||||
if ( nav_stage == 1 ) {
|
||||
nav_stage = 1;
|
||||
if ( approaching( 6 ) ) NextStageFrom( 6 ) else {
|
||||
fly_to( 6 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 6 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 2 ) {
|
||||
nav_stage = 2;
|
||||
if ( approaching( 1 ) ) NextStageFrom( 1 ) else {
|
||||
fly_to( 1 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 3 ) {
|
||||
nav_stage = 3;
|
||||
if ( approaching( 2 ) ) NextStageFrom( 2 ) else {
|
||||
route_to( last_wp, 2 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 2 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 4 ) {
|
||||
nav_stage = 4;
|
||||
if ( approaching( 3 ) ) NextStageFrom( 3 ) else {
|
||||
fly_to( 3 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 3 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 5 ) {
|
||||
nav_stage = 5;
|
||||
if ( approaching( 4 ) ) NextStageFrom( 4 ) else {
|
||||
fly_to( 4 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 6 ) {
|
||||
nav_stage = 6;
|
||||
if ( approaching( 5 ) ) NextStageFrom( 5 ) else {
|
||||
route_to( last_wp, 5 );
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 5 ].a;
|
||||
pre_climb = 0.;
|
||||
}
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 7 ) {
|
||||
nav_stage = 7;
|
||||
Goto( while_5 )
|
||||
}
|
||||
|
||||
else
|
||||
if ( nav_stage == 8 ) {
|
||||
Label( endwhile_6 )
|
||||
nav_stage = 8;
|
||||
NextBlock();
|
||||
} else { }
|
||||
} else
|
||||
if ( nav_block == 3 ) { // xyz
|
||||
nav_block = 3;
|
||||
if RcEvent1() {
|
||||
GotoBlock( 4 )
|
||||
}
|
||||
if ( nav_stage == 0 ) {
|
||||
nav_stage = 0;
|
||||
//Goto3D(50)
|
||||
{
|
||||
static float carrot_x, carrot_y;
|
||||
int16_t pitch;
|
||||
int16_t roll;
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 ) {
|
||||
int16_t yaw = from_fbw.channels[ RADIO_YAW ];
|
||||
if ( yaw > MIN_DX || yaw < -MIN_DX ) {
|
||||
carrot_x += FLOAT_OF_PPRZ( yaw, 0, -20. );
|
||||
carrot_x = Min( carrot_x, MAX_DIST_CARROT );
|
||||
carrot_x = Max( carrot_x, -MAX_DIST_CARROT );
|
||||
}
|
||||
pitch = from_fbw.channels[ RADIO_PITCH ];
|
||||
if ( pitch > MIN_DX || pitch < -MIN_DX ) {
|
||||
carrot_y += FLOAT_OF_PPRZ( pitch, 0, -20. );
|
||||
carrot_y = Min( carrot_y,
|
||||
MAX_DIST_CARROT );
|
||||
carrot_y = Max( carrot_y, -MAX_DIST_CARROT );
|
||||
}
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
roll = from_fbw.channels[ RADIO_ROLL ];
|
||||
if ( roll > MIN_DX || roll < -MIN_DX ) {
|
||||
desired_altitude += FLOAT_OF_PPRZ( roll, 0, -1.0 );
|
||||
desired_altitude = Max( desired_altitude, MIN_HEIGHT_CARROT + GROUND_ALT );
|
||||
desired_altitude = Min( desired_altitude, MAX_HEIGHT_CARROT + GROUND_ALT );
|
||||
}
|
||||
}
|
||||
{
|
||||
float alpha = atan2( estimator_y - carrot_y, estimator_x - carrot_x );
|
||||
float alpha_carrot = alpha + CARROT / ( -50 * estimator_hspeed_mod );
|
||||
fly_to_xy( carrot_x + cos( alpha_carrot )*fabs( 50 ),
|
||||
carrot_y + sin( alpha_carrot )*fabs( 50 ) );
|
||||
qdr = DegOfRad( M_PI / 2 - alpha_carrot );
|
||||
{
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( qdr < 0 ) qdr += 360;
|
||||
_Pragma( "loopbounds min 0 max 1" )
|
||||
while ( qdr >= 360 ) qdr -= 360;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 1 ) {
|
||||
nav_stage = 1;
|
||||
NextBlock()
|
||||
} else { }
|
||||
} else
|
||||
if ( nav_block == 4 ) {
|
||||
nav_block = 4;
|
||||
if RcEvent1() {
|
||||
GotoBlock( 5 )
|
||||
}
|
||||
if ( nav_stage == 0 ) {
|
||||
nav_stage = 0;
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 0 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle( 0, 150 );
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 1 ) {
|
||||
nav_stage = 1;
|
||||
NextBlock()
|
||||
} else {}
|
||||
} else
|
||||
if ( nav_block == 5 ) {
|
||||
nav_block = 5;
|
||||
if RcEvent1() {
|
||||
GotoBlock( 1 )
|
||||
}
|
||||
if ( nav_stage == 0 ) {
|
||||
Label( while_7 )
|
||||
nav_stage = 0;
|
||||
if ( ! ( TRUE ) ) Goto( endwhile_8 ) else NextStage();
|
||||
}
|
||||
|
||||
else
|
||||
if ( nav_stage == 1 ) {
|
||||
nav_stage = 1;
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 1 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle( 1, 100 );
|
||||
if ( Qdr( 0 ) ) NextStage();
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 2 ) {
|
||||
nav_stage = 2;
|
||||
auto_pitch = FALSE;
|
||||
nav_pitch = 0.000000;
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT;
|
||||
desired_altitude = waypoints[ 4 ].a;
|
||||
pre_climb = 0.;
|
||||
Circle( 4, 100 );
|
||||
if ( Qdr( 180 ) ) NextStage();
|
||||
return;
|
||||
} else
|
||||
if ( nav_stage == 3 ) {
|
||||
nav_stage = 3;
|
||||
Goto( while_7 )
|
||||
} else
|
||||
if ( nav_stage == 4 ) {
|
||||
Label( endwhile_8 )
|
||||
nav_stage = 4;
|
||||
NextBlock()
|
||||
} else { }
|
||||
} else { }
|
||||
|
||||
//#endif
|
||||
}
|
||||
#endif // NAV_C
|
||||
|
||||
#endif // FLIGHT_PLAN_H
|
||||
@ -0,0 +1,55 @@
|
||||
/* This file has been generated from conf/flight_plans/braunschweig.xml */
|
||||
/* Please DO NOT EDIT */
|
||||
|
||||
#ifndef INFLIGHT_CALIB_H
|
||||
#define INFLIGHT_CALIB_H
|
||||
|
||||
void inflight_calib( bool_t mode_changed )
|
||||
{
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO1 ) {
|
||||
if ( inflight_calib_mode == IF_CALIB_MODE_UP ) {
|
||||
static int16_t ir_pitch_neutral_init;
|
||||
if ( mode_changed ) {
|
||||
ir_pitch_neutral_init = ir_pitch_neutral;
|
||||
slider1_init = from_fbw.channels[ RADIO_GAIN1 ];
|
||||
}
|
||||
ir_pitch_neutral = ParamValInt16( ir_pitch_neutral_init, 60.000000,
|
||||
from_fbw.channels[ RADIO_GAIN1 ], slider1_init );
|
||||
slider_1_val = ( float )ir_pitch_neutral;
|
||||
}
|
||||
if ( inflight_calib_mode == IF_CALIB_MODE_UP ) {
|
||||
static int16_t ir_roll_neutral_init;
|
||||
if ( mode_changed ) {
|
||||
ir_roll_neutral_init = ir_roll_neutral;
|
||||
slider2_init = from_fbw.channels[ RADIO_GAIN2 ];
|
||||
}
|
||||
ir_roll_neutral = ParamValInt16( ir_roll_neutral_init, -60.000000,
|
||||
from_fbw.channels[ RADIO_GAIN2 ], slider2_init );
|
||||
slider_2_val = ( float )ir_roll_neutral;
|
||||
}
|
||||
}
|
||||
if ( pprz_mode == PPRZ_MODE_AUTO2 ) {
|
||||
if ( inflight_calib_mode == IF_CALIB_MODE_UP ) {
|
||||
static float course_pgain_init;
|
||||
if ( mode_changed ) {
|
||||
course_pgain_init = course_pgain;
|
||||
slider1_init = from_fbw.channels[ RADIO_GAIN1 ];
|
||||
}
|
||||
course_pgain = ParamValFloat( course_pgain_init, 0.100000,
|
||||
from_fbw.channels[ RADIO_GAIN1 ], slider1_init );
|
||||
slider_1_val = ( float )course_pgain;
|
||||
}
|
||||
if ( inflight_calib_mode == IF_CALIB_MODE_UP ) {
|
||||
static float max_roll_init;
|
||||
if ( mode_changed ) {
|
||||
max_roll_init = max_roll;
|
||||
slider2_init = from_fbw.channels[ RADIO_GAIN2 ];
|
||||
}
|
||||
max_roll = ParamValFloat( max_roll_init, -0.200000,
|
||||
from_fbw.channels[ RADIO_GAIN2 ], slider2_init );
|
||||
slider_2_val = ( float )max_roll;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif // INFLIGHT_CALIB_H
|
||||
348
targets/wasm-tacle/parallel/PapaBench/sw/var/include/messages.h
Normal file
348
targets/wasm-tacle/parallel/PapaBench/sw/var/include/messages.h
Normal file
@ -0,0 +1,348 @@
|
||||
/* Automatically generated from conf/messages.xml */
|
||||
/* Please DO NOT EDIT */
|
||||
#define DL_ID 0
|
||||
#define DL_BOOT 1
|
||||
#define DL_CALIB_START 2
|
||||
#define DL_CALIB_CONTRAST 3
|
||||
#define DL_TAKEOFF 4
|
||||
#define DL_RAD_OF_IR 5
|
||||
#define DL_ATTITUDE 6
|
||||
#define DL_ADC 7
|
||||
#define DL_GPS 8
|
||||
#define DL_NAVIGATION_REF 9
|
||||
#define DL_NAVIGATION 10
|
||||
#define DL_PPRZ_MODE 11
|
||||
#define DL_BAT 12
|
||||
#define DL_DEBUG 13
|
||||
#define DL_CLIMB_PID 14
|
||||
#define DL_DOWNLINK_STATUS 15
|
||||
#define DL_MODEM_STATUS 16
|
||||
#define DL_SETTINGS 17
|
||||
#define DL_DESIRED 18
|
||||
#define DL_WIND 19
|
||||
#define DL_IMU 20
|
||||
#define DL_RAW_IMU 21
|
||||
#define DL_KALMAN 22
|
||||
#define DL_MSG_NB 23
|
||||
|
||||
#define DOWNLINK_SEND_ID(md5sum){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(20)) {\
|
||||
ModemStartMessage(DL_ID) \
|
||||
{\
|
||||
int i;\
|
||||
for(i = 0; i < 16; i++) {\
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(&md5sum[ i ])); \
|
||||
}\
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_BOOT(version){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(6)) {\
|
||||
ModemStartMessage(DL_BOOT) \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(version)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_CALIB_START(){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(4)) {\
|
||||
ModemStartMessage(DL_CALIB_START) \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_CALIB_CONTRAST(adc){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(6)) {\
|
||||
ModemStartMessage(DL_CALIB_CONTRAST) \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(adc)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_TAKEOFF(cpu_time){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(6)) {\
|
||||
ModemStartMessage(DL_TAKEOFF) \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(cpu_time)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_RAD_OF_IR(ir, rad, rad_of_ir, ir_roll_ntrl, ir_pitch_ntrl){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(20)) {\
|
||||
ModemStartMessage(DL_RAD_OF_IR) \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(ir)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(rad)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(rad_of_ir)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(ir_roll_ntrl)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(ir_pitch_ntrl)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_ATTITUDE(phi, psi, theta){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(16)) {\
|
||||
ModemStartMessage(DL_ATTITUDE) \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(phi)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(psi)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(theta)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_ADC(roll, pitch){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(8)) {\
|
||||
ModemStartMessage(DL_ADC) \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(roll)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(pitch)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_GPS(mode, east, north, course, alt, speed, climb, tow){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(33)) {\
|
||||
ModemStartMessage(DL_GPS) \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(mode)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(east)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(north)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(course)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(alt)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(speed)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(climb)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(tow)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_NAVIGATION_REF(utm_east, utm_north){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(12)) {\
|
||||
ModemStartMessage(DL_NAVIGATION_REF) \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(utm_east)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(utm_north)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_NAVIGATION(cur_block, cur_stage, pos_x, pos_y, desired_course, dist2_wp, course_pgain, dist2_home){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(30)) {\
|
||||
ModemStartMessage(DL_NAVIGATION) \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(cur_block)); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(cur_stage)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(pos_x)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(pos_y)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(desired_course)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(dist2_wp)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(course_pgain)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(dist2_home)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_PPRZ_MODE(ap_mode, ap_altitude, if_calib_mode, mcu1_status, lls_calib){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(9)) {\
|
||||
ModemStartMessage(DL_PPRZ_MODE) \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(ap_mode)); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(ap_altitude)); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(if_calib_mode)); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(mcu1_status)); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(lls_calib)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_BAT(voltage, flight_time, low_battery, block_time, stage_time){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(12)) {\
|
||||
ModemStartMessage(DL_BAT) \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(voltage)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(flight_time)); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(low_battery)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(block_time)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(stage_time)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_DEBUG(i2c_nb_err, i2c_mcu1_nb_err, modem_nb_err, gps_nb_err, ppm_rate){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(9)) {\
|
||||
ModemStartMessage(DL_DEBUG) \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(i2c_nb_err)); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(i2c_mcu1_nb_err)); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(modem_nb_err)); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(gps_nb_err)); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(ppm_rate)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_CLIMB_PID(gaz, climb, sum_err, p_gain){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(18)) {\
|
||||
ModemStartMessage(DL_CLIMB_PID) \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(gaz)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(climb)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(sum_err)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(p_gain)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_DOWNLINK_STATUS(run_time, rx_bytes, rx_msgs, rx_err, rx_bytes_rate, rx_msgs_rate){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(28)) {\
|
||||
ModemStartMessage(DL_DOWNLINK_STATUS) \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(run_time)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(rx_bytes)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(rx_msgs)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(rx_err)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(rx_bytes_rate)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(rx_msgs_rate)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_MODEM_STATUS(detected, valim, cd, nb_byte, nb_msg, nb_err){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(22)) {\
|
||||
ModemStartMessage(DL_MODEM_STATUS) \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(detected)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(valim)); \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(cd)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(nb_byte)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(nb_msg)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(nb_err)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_SETTINGS(mode, slider_1_val, slider_2_val){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(13)) {\
|
||||
ModemStartMessage(DL_SETTINGS) \
|
||||
MODEM_PUT_1_BYTE_BY_ADDR((uint8_t*)(mode)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(slider_1_val)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(slider_2_val)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_DESIRED(roll, pitch, desired_x, desired_y, desired_altitude){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(24)) {\
|
||||
ModemStartMessage(DL_DESIRED) \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(roll)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(pitch)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(desired_x)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(desired_y)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(desired_altitude)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_WIND(dir, speed, mean_as, nb_sample, stddev){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(22)) {\
|
||||
ModemStartMessage(DL_WIND) \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(dir)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(speed)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(mean_as)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(nb_sample)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(stddev)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_IMU(gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(28)) {\
|
||||
ModemStartMessage(DL_IMU) \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(gyro_x)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(gyro_y)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(gyro_z)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(accel_x)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(accel_y)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(accel_z)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_RAW_IMU(raw_gx, raw_gy, raw_gz, raw_ax, raw_ay, raw_az){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(16)) {\
|
||||
ModemStartMessage(DL_RAW_IMU) \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(raw_gx)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(raw_gy)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(raw_gz)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(raw_ax)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(raw_ay)); \
|
||||
MODEM_PUT_2_BYTE_BY_ADDR((uint8_t*)(raw_az)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define DOWNLINK_SEND_KALMAN(phi, phi_dot, phi_bias, theta, theta_dot, theta_bias){ \
|
||||
if (MODEM_CHECK_FREE_SPACE(28)) {\
|
||||
ModemStartMessage(DL_KALMAN) \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(phi)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(phi_dot)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(phi_bias)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(theta)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(theta_dot)); \
|
||||
MODEM_PUT_4_BYTE_BY_ADDR((uint8_t*)(theta_bias)); \
|
||||
ModemEndMessage() \
|
||||
} \
|
||||
else \
|
||||
modem_nb_ovrn++; \
|
||||
}
|
||||
|
||||
#define MESSAGES_MD5SUM "\120\162\150\107\166\250\102\343\211\352\231\260\061\055\031\274"
|
||||
// Load: intant(buffer) 16(37) 8(37) 13(37) 24(61) 0(61) 16(61) 8(61) 13(61) 12(49) 9(58) 16(58) 8(58) 13(58) 24(70) 0(61) 16(61) 8(61) 13(61) 18(55) 0(55) 16(55) 8(55) 13(55) 24(61) 0(61) 16(61) 8(61) 13(61) 12(49) 9(58) 16(58) 8(58) 13(58) 24(70) 0(61) 16(61) 8(61) 13(61) 18(55) 0(55) 16(55) 8(55) 13(55) 24(61) 0(61) 16(61) 8(61) 13(61) 12(49) 12(61) 16(61) 8(61) 13(61) 24(73) 0(61) 16(61) 8(61) 13(61) 18(55) 9(64) 16(64) 8(64) 13(64) 24(70) 0(61) 16(61) 8(61) 13(61) 12(49) 0(49) 16(49) 8(49) 13(49) 24(61) 0(61) 16(61) 8(61) 13(61) 18(55) 9(64) 16(64) 8(64) 13(64) 24(70) 0(61) 16(61) 8(61) 13(61) 12(49) 0(49) 16(49) 8(49) 13(49) 24(61) 0(61) 16(61) 8(61) 13(61) 18(55) 0(55) 16(55) 8(55) 13(55) 24(61) 0(61) 16(61) 8(61) 13(61) 12(49) 9(58) 16(58) 8(58) 13(58) 24(70) 0(61) 16(61) 8(61) 13(61) 18(55) 0(55) 16(55) 8(55) 13(55) 24(61) 0(61) 16(61) 8(61) 13(61) 12(49) 9(58) 16(58) 8(58) 13(58) 24(70) 0(61) 16(61) 8(61) 13(61) 18(55) 0(55) 16(55) 8(55) 13(55) 24(61) 0(61) 16(61) 8(61) 13(61) 12(49) 12(61) 16(61) 8(61) 13(61) 24(73) 0(61) 16(61) 8(61) 13(61) 18(55) 9(64) 16(64) 8(64) 13(64) 24(70) 0(61) 16(61) 8(61) 13(61) 12(49) 0(49) 16(49) 8(49) 13(49) 24(61) 0(61) 16(61) 8(61) 13(61) 18(55) 9(64) 16(64) 8(64) 13(64) 24(70) 0(61) 16(61) 8(61) 13(61) 12(49) 0(49) 16(49) 8(49) 13(49) 24(61) 0(61) 16(61) 8(61) 13(61) 18(55) 0(55) 16(55) 8(55) 13(55) 24(61) 0(61) 16(61) 8(61) 13(61) 12(49) 9(58) 16(58) 8(58) 13(58) 24(70) 0(61) 16(61) 8(61) 13(61) 18(55) 0(55) 16(55) 8(55) 13(55) 24(61) 0(61) 16(61) 8(61) 13(61) 12(49) 9(58) 16(58) 8(58) 13(58) 24(70) 0(61) 16(61) 8(61) 13(61) 18(55) 0(55) 16(55) 8(55) 13(55) 24(61) 0(61) 16(61) 8(61) 13(61) 0(37) 0(37)
|
||||
|
||||
/*#define PeriodicSend() { // 10Hz // \
|
||||
static uint8_t i;\
|
||||
i++; if (i == 250) i = 0;\
|
||||
if (i % 5 == 0) PERIODIC_SEND_ATTITUDE();\
|
||||
if (i % 5 == 1) PERIODIC_SEND_ADC();\
|
||||
if (i % 5 == 2) PERIODIC_SEND_SETTINGS();\
|
||||
if (i % 10 == 3) PERIODIC_SEND_DESIRED();\
|
||||
if (i % 20 == 8) PERIODIC_SEND_BAT();\
|
||||
if (i % 20 == 18) PERIODIC_SEND_CLIMB_PID();\
|
||||
if (i % 50 == 9) PERIODIC_SEND_PPRZ_MODE();\
|
||||
if (i % 50 == 29) PERIODIC_SEND_DEBUG();\
|
||||
if (i % 100 == 49) PERIODIC_SEND_NAVIGATION_REF();\
|
||||
}*/
|
||||
99
targets/wasm-tacle/parallel/PapaBench/sw/var/include/radio.h
Normal file
99
targets/wasm-tacle/parallel/PapaBench/sw/var/include/radio.h
Normal file
@ -0,0 +1,99 @@
|
||||
/* This file has been generated from conf/radios/mc3030.xml */
|
||||
/* Please DO NOT EDIT */
|
||||
|
||||
#ifndef RADIO_H
|
||||
#define RADIO_H
|
||||
|
||||
#define RADIO_NAME "mc3030"
|
||||
|
||||
#define RADIO_CTL_NB 9
|
||||
|
||||
#define RADIO_CTL_D 0
|
||||
#define RADIO_THROTTLE RADIO_CTL_D
|
||||
#define RADIO_CTL_C 1
|
||||
#define RADIO_ROLL RADIO_CTL_C
|
||||
#define RADIO_CTL_B 2
|
||||
#define RADIO_PITCH RADIO_CTL_B
|
||||
#define RADIO_CTL_A 3
|
||||
#define RADIO_YAW RADIO_CTL_A
|
||||
#define RADIO_CTL_G 4
|
||||
#define RADIO_MODE RADIO_CTL_G
|
||||
#define RADIO_CTL_E 5
|
||||
#define RADIO_GAIN1 RADIO_CTL_E
|
||||
#define RADIO_CTL_F 6
|
||||
#define RADIO_GAIN2 RADIO_CTL_F
|
||||
#define RADIO_CTL_H 7
|
||||
#define RADIO_LLS RADIO_CTL_H
|
||||
#define RADIO_CTL_I 8
|
||||
#define RADIO_CALIB RADIO_CTL_I
|
||||
|
||||
#define PPM_MIN_PULSE_WIDTH 850ul*CLOCK
|
||||
#define PPM_MAX_PULSE_WIDTH 2100ul*CLOCK
|
||||
#define PPM_SYNC_PULSE (uint8_t)(((uint32_t)(5000ul*CLOCK))/1024ul)
|
||||
|
||||
#define LastRadioFromPpm() {\
|
||||
static uint8_t avg_cpt = 0; /* Counter for averaging */\
|
||||
int16_t tmp_radio;\
|
||||
tmp_radio = ppm_pulses[ RADIO_THROTTLE ] - (CLOCK*1000);\
|
||||
last_radio[ RADIO_THROTTLE ] = tmp_radio * (MAX_PPRZ / 1 / (float)(CLOCK*(2200-1000)));\
|
||||
if (last_radio[ RADIO_THROTTLE ] > MAX_PPRZ) last_radio[ RADIO_THROTTLE ] = MAX_PPRZ;\
|
||||
else if (last_radio[ RADIO_THROTTLE ] < 0) last_radio[ RADIO_THROTTLE ] = 0; \
|
||||
\
|
||||
tmp_radio = ppm_pulses[ RADIO_ROLL ] - (CLOCK*1600);\
|
||||
last_radio[ RADIO_ROLL ] = tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/1/(float)(CLOCK*(2200-1600))) : (MIN_PPRZ/1/(float)(CLOCK*(1000-1600))));\
|
||||
if (last_radio[ RADIO_ROLL ] > MAX_PPRZ) last_radio[ RADIO_ROLL ] = MAX_PPRZ;\
|
||||
else if (last_radio[ RADIO_ROLL ] < MIN_PPRZ) last_radio[ RADIO_ROLL ] = MIN_PPRZ; \
|
||||
\
|
||||
tmp_radio = ppm_pulses[ RADIO_PITCH ] - (CLOCK*1600);\
|
||||
last_radio[ RADIO_PITCH ] = tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/1/(float)(CLOCK*(2200-1600))) : (MIN_PPRZ/1/(float)(CLOCK*(1000-1600))));\
|
||||
if (last_radio[ RADIO_PITCH ] > MAX_PPRZ) last_radio[ RADIO_PITCH ] = MAX_PPRZ;\
|
||||
else if (last_radio[ RADIO_PITCH ] < MIN_PPRZ) last_radio[ RADIO_PITCH ] = MIN_PPRZ; \
|
||||
\
|
||||
tmp_radio = ppm_pulses[ RADIO_YAW ] - (CLOCK*1600);\
|
||||
last_radio[ RADIO_YAW ] = tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/1/(float)(CLOCK*(2200-1600))) : (MIN_PPRZ/1/(float)(CLOCK*(1000-1600))));\
|
||||
if (last_radio[ RADIO_YAW ] > MAX_PPRZ) last_radio[ RADIO_YAW ] = MAX_PPRZ;\
|
||||
else if (last_radio[ RADIO_YAW ] < MIN_PPRZ) last_radio[ RADIO_YAW ] = MIN_PPRZ; \
|
||||
\
|
||||
tmp_radio = ppm_pulses[ RADIO_MODE ] - (CLOCK*1600);\
|
||||
avg_last_radio[ RADIO_MODE ] += tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/AVERAGING_PERIOD/(float)(CLOCK*(2200-1600))) : (MIN_PPRZ/AVERAGING_PERIOD/(float)(CLOCK*(1000-1600))));\
|
||||
tmp_radio = ppm_pulses[ RADIO_GAIN1 ] - (CLOCK*1600);\
|
||||
avg_last_radio[ RADIO_GAIN1 ] += tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/AVERAGING_PERIOD/(float)(CLOCK*(2200-1600))) : (MIN_PPRZ/AVERAGING_PERIOD/(float)(CLOCK*(1000-1600))));\
|
||||
tmp_radio = ppm_pulses[ RADIO_GAIN2 ] - (CLOCK*1600);\
|
||||
avg_last_radio[ RADIO_GAIN2 ] += tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/AVERAGING_PERIOD/(float)(CLOCK*(2200-1600))) : (MIN_PPRZ/AVERAGING_PERIOD/(float)(CLOCK*(1000-1600))));\
|
||||
tmp_radio = ppm_pulses[ RADIO_LLS ] - (CLOCK*1600);\
|
||||
avg_last_radio[ RADIO_LLS ] += tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/AVERAGING_PERIOD/(float)(CLOCK*(2200-1600))) : (MIN_PPRZ/AVERAGING_PERIOD/(float)(CLOCK*(1000-1600))));\
|
||||
tmp_radio = ppm_pulses[ RADIO_CALIB ] - (CLOCK*1600);\
|
||||
avg_last_radio[ RADIO_CALIB ] += tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/AVERAGING_PERIOD/(float)(CLOCK*(2200-1600))) : (MIN_PPRZ/AVERAGING_PERIOD/(float)(CLOCK*(1000-1600))));\
|
||||
avg_cpt++;\
|
||||
if (avg_cpt == AVERAGING_PERIOD) {\
|
||||
avg_cpt = 0;\
|
||||
last_radio[ RADIO_MODE ] = avg_last_radio[ RADIO_MODE ];\
|
||||
avg_last_radio[ RADIO_MODE ] = 0;\
|
||||
if (last_radio[ RADIO_MODE ] > MAX_PPRZ) last_radio[ RADIO_MODE ] = MAX_PPRZ;\
|
||||
else if (last_radio[ RADIO_MODE ] < MIN_PPRZ) last_radio[ RADIO_MODE ] = MIN_PPRZ; \
|
||||
\
|
||||
last_radio[ RADIO_GAIN1 ] = avg_last_radio[ RADIO_GAIN1 ];\
|
||||
avg_last_radio[ RADIO_GAIN1 ] = 0;\
|
||||
if (last_radio[ RADIO_GAIN1 ] > MAX_PPRZ) last_radio[ RADIO_GAIN1 ] = MAX_PPRZ;\
|
||||
else if (last_radio[ RADIO_GAIN1 ] < MIN_PPRZ) last_radio[ RADIO_GAIN1 ] = MIN_PPRZ; \
|
||||
\
|
||||
last_radio[ RADIO_GAIN2 ] = avg_last_radio[ RADIO_GAIN2 ];\
|
||||
avg_last_radio[ RADIO_GAIN2 ] = 0;\
|
||||
if (last_radio[ RADIO_GAIN2 ] > MAX_PPRZ) last_radio[ RADIO_GAIN2 ] = MAX_PPRZ;\
|
||||
else if (last_radio[ RADIO_GAIN2 ] < MIN_PPRZ) last_radio[ RADIO_GAIN2 ] = MIN_PPRZ; \
|
||||
\
|
||||
last_radio[ RADIO_LLS ] = avg_last_radio[ RADIO_LLS ];\
|
||||
avg_last_radio[ RADIO_LLS ] = 0;\
|
||||
if (last_radio[ RADIO_LLS ] > MAX_PPRZ) last_radio[ RADIO_LLS ] = MAX_PPRZ;\
|
||||
else if (last_radio[ RADIO_LLS ] < MIN_PPRZ) last_radio[ RADIO_LLS ] = MIN_PPRZ; \
|
||||
\
|
||||
last_radio[ RADIO_CALIB ] = avg_last_radio[ RADIO_CALIB ];\
|
||||
avg_last_radio[ RADIO_CALIB ] = 0;\
|
||||
if (last_radio[ RADIO_CALIB ] > MAX_PPRZ) last_radio[ RADIO_CALIB ] = MAX_PPRZ;\
|
||||
else if (last_radio[ RADIO_CALIB ] < MIN_PPRZ) last_radio[ RADIO_CALIB ] = MIN_PPRZ; \
|
||||
\
|
||||
last_radio_contains_avg_channels = TRUE;\
|
||||
}\
|
||||
}
|
||||
|
||||
#endif // RADIO_H
|
||||
@ -0,0 +1,72 @@
|
||||
/* Generated from conf/ubx.xml */
|
||||
/* Please DO NOT EDIT */
|
||||
|
||||
#define UBX_SYNC1 0xB5
|
||||
#define UBX_SYNC2 0x62
|
||||
|
||||
#define UBX_NAV_ID 0x01
|
||||
|
||||
#define UBX_NAV_POSLLH_ID 0x02
|
||||
#define UBX_NAV_POSLLH_ITOW(_ubx_payload) (*((uint32_t*)(_ubx_payload+0)))
|
||||
#define UBX_NAV_POSLLH_LON(_ubx_payload) (*((int32_t*)(_ubx_payload+4)))
|
||||
#define UBX_NAV_POSLLH_LAT(_ubx_payload) (*((int32_t*)(_ubx_payload+8)))
|
||||
#define UBX_NAV_POSLLH_HEIGHT(_ubx_payload) (*((int32_t*)(_ubx_payload+12)))
|
||||
#define UBX_NAV_POSLLH_HMSL(_ubx_payload) (*((int32_t*)(_ubx_payload+16)))
|
||||
#define UBX_NAV_POSLLH_Hacc(_ubx_payload) (*((uint32_t*)(_ubx_payload+20)))
|
||||
#define UBX_NAV_POSLLH_Vacc(_ubx_payload) (*((uint32_t*)(_ubx_payload+24)))
|
||||
|
||||
#define UBX_NAV_POSUTM_ID 0x08
|
||||
#define UBX_NAV_POSUTM_ITOW(_ubx_payload) (*((uint32_t*)(_ubx_payload+0)))
|
||||
#define UBX_NAV_POSUTM_EAST(_ubx_payload) (*((int32_t*)(_ubx_payload+4)))
|
||||
#define UBX_NAV_POSUTM_NORTH(_ubx_payload) (*((int32_t*)(_ubx_payload+8)))
|
||||
#define UBX_NAV_POSUTM_ALT(_ubx_payload) (*((int32_t*)(_ubx_payload+12)))
|
||||
#define UBX_NAV_POSUTM_ZONE(_ubx_payload) (*((int8_t*)(_ubx_payload+16)))
|
||||
#define UBX_NAV_POSUTM_HEM(_ubx_payload) (*((int8_t*)(_ubx_payload+17)))
|
||||
|
||||
#define UBX_NAV_STATUS_ID 0x03
|
||||
#define UBX_NAV_STATUS_ITOW(_ubx_payload) (*((uint32_t*)(_ubx_payload+0)))
|
||||
#define UBX_NAV_STATUS_GPSfix(_ubx_payload) (*((uint8_t*)(_ubx_payload+4)))
|
||||
#define UBX_NAV_STATUS_Flags(_ubx_payload) (*((uint8_t*)(_ubx_payload+5)))
|
||||
#define UBX_NAV_STATUS_DiffS(_ubx_payload) (*((uint8_t*)(_ubx_payload+6)))
|
||||
#define UBX_NAV_STATUS_res(_ubx_payload) (*((uint8_t*)(_ubx_payload+7)))
|
||||
#define UBX_NAV_STATUS_TTFF(_ubx_payload) (*((uint32_t*)(_ubx_payload+8)))
|
||||
#define UBX_NAV_STATUS_MSSS(_ubx_payload) (*((uint32_t*)(_ubx_payload+12)))
|
||||
|
||||
#define UBX_NAV_VELNED_ID 0x12
|
||||
#define UBX_NAV_VELNED_ITOW(_ubx_payload) (*((uint32_t*)(_ubx_payload+0)))
|
||||
#define UBX_NAV_VELNED_VEL_N(_ubx_payload) (*((int32_t*)(_ubx_payload+4)))
|
||||
#define UBX_NAV_VELNED_VEL_E(_ubx_payload) (*((int32_t*)(_ubx_payload+8)))
|
||||
#define UBX_NAV_VELNED_VEL_D(_ubx_payload) (*((int32_t*)(_ubx_payload+12)))
|
||||
#define UBX_NAV_VELNED_Speed(_ubx_payload) (*((uint32_t*)(_ubx_payload+16)))
|
||||
#define UBX_NAV_VELNED_GSpeed(_ubx_payload) (*((uint32_t*)(_ubx_payload+20)))
|
||||
#define UBX_NAV_VELNED_Heading(_ubx_payload) (*((int32_t*)(_ubx_payload+24)))
|
||||
#define UBX_NAV_VELNED_SAcc(_ubx_payload) (*((uint32_t*)(_ubx_payload+28)))
|
||||
#define UBX_NAV_VELNED_CAcc(_ubx_payload) (*((uint32_t*)(_ubx_payload+32)))
|
||||
|
||||
#define UBX_NAV_SVINFO_ID 0x30
|
||||
#define UBX_NAV_SVINFO_ITOW(_ubx_payload) (*((uint32_t*)(_ubx_payload+0)))
|
||||
#define UBX_NAV_SVINFO_NCH(_ubx_payload) (*((uint8_t*)(_ubx_payload+4)))
|
||||
#define UBX_NAV_SVINFO_RES1(_ubx_payload) (*((uint8_t*)(_ubx_payload+5)))
|
||||
#define UBX_NAV_SVINFO_RES2(_ubx_payload) (*((uint8_t*)(_ubx_payload+6)))
|
||||
#define UBX_NAV_SVINFO_chn(_ubx_payload,_ubx_block) (*((uint8_t*)(_ubx_payload+7+12*_ubx_block)))
|
||||
#define UBX_NAV_SVINFO_SVID(_ubx_payload,_ubx_block) (*((uint8_t*)(_ubx_payload+8+12*_ubx_block)))
|
||||
#define UBX_NAV_SVINFO_Flags(_ubx_payload,_ubx_block) (*((uint8_t*)(_ubx_payload+9+12*_ubx_block)))
|
||||
#define UBX_NAV_SVINFO_QI(_ubx_payload,_ubx_block) (*((int8_t*)(_ubx_payload+10+12*_ubx_block)))
|
||||
#define UBX_NAV_SVINFO_CNO(_ubx_payload,_ubx_block) (*((uint8_t*)(_ubx_payload+11+12*_ubx_block)))
|
||||
#define UBX_NAV_SVINFO_Elev(_ubx_payload,_ubx_block) (*((int8_t*)(_ubx_payload+12+12*_ubx_block)))
|
||||
#define UBX_NAV_SVINFO_Azim(_ubx_payload,_ubx_block) (*((int16_t*)(_ubx_payload+13+12*_ubx_block)))
|
||||
#define UBX_NAV_SVINFO_PRRes(_ubx_payload,_ubx_block) (*((int32_t*)(_ubx_payload+15+12*_ubx_block)))
|
||||
|
||||
#define UBX_USR_ID 0x40
|
||||
|
||||
#define UBX_USR_IRSIM_ID 0x01
|
||||
#define UBX_USR_IRSIM_ROLL(_ubx_payload) (*((int16_t*)(_ubx_payload+0)))
|
||||
#define UBX_USR_IRSIM_PITCH(_ubx_payload) (*((int16_t*)(_ubx_payload+2)))
|
||||
|
||||
#define UBX_USR_SERVOS_ID 0x02
|
||||
#define UBX_USR_SERVOS_N(_ubx_payload) (*((int8_t*)(_ubx_payload+0)))
|
||||
#define UBX_USR_SERVOS_WIDTH(_ubx_payload,_ubx_block) (*((uint16_t*)(_ubx_payload+1+2*_ubx_block)))
|
||||
|
||||
#define UBX_USR_PPM_ID 0x03
|
||||
#define UBX_USR_PPM_N(_ubx_payload) (*((int8_t*)(_ubx_payload+0)))
|
||||
#define UBX_USR_PPM_WIDTH(_ubx_payload,_ubx_block) (*((uint16_t*)(_ubx_payload+1+2*_ubx_block)))
|
||||
Reference in New Issue
Block a user