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 */
|
||||
Reference in New Issue
Block a user