mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +08:00
This commit was generated by cvs2svn to compensate for changes in r2,
which included commits to RCS files with non-trunk default branches.
This commit is contained in:
Antoine Drouin
@@ -0,0 +1,25 @@
|
||||
#
|
||||
# $Id$
|
||||
# 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.
|
||||
#
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,92 @@
|
||||
#
|
||||
# $Id$
|
||||
# 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.
|
||||
#
|
||||
|
||||
|
||||
FBW=../fly_by_wire
|
||||
|
||||
|
||||
LOCAL_CFLAGS= $(CTL_BRD_FLAGS) $(GPS_FLAGS) $(SIMUL_FLAGS)
|
||||
|
||||
VARINCLUDE=$(PAPARAZZI_HOME)/var/include
|
||||
ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT)
|
||||
|
||||
ARCH = atmega128
|
||||
TARGET = autopilot
|
||||
|
||||
LOW_FUSE = e0
|
||||
HIGH_FUSE = 99
|
||||
|
||||
ifeq ($(CTL_BRD_VERSION),V1_1)
|
||||
LOW_FUSE = ff
|
||||
HIGH_FUSE = 89
|
||||
CTL_BRD_FLAGS=-DCTL_BRD_V1_1
|
||||
endif
|
||||
|
||||
ifeq ($(SIMUL),1)
|
||||
SIMUL_FLAGS= -DSIMUL
|
||||
endif
|
||||
|
||||
EXT_FUSE = ff
|
||||
LOCK_FUSE = ff
|
||||
INCLUDES = -I $(FBW) -I ../../include -I $(VARINCLUDE) -I $(ACINCLUDE)
|
||||
|
||||
GPS = gps_ubx.c
|
||||
GPS_FLAGS=-DUBX
|
||||
|
||||
$(TARGET).srcs = \
|
||||
main.c \
|
||||
modem.c \
|
||||
link_fbw.c \
|
||||
spi.c \
|
||||
adc.c \
|
||||
$(GPS) \
|
||||
infrared.c \
|
||||
pid.c \
|
||||
nav.c \
|
||||
uart.c \
|
||||
estimator.c \
|
||||
if_calib.c \
|
||||
mainloop.c
|
||||
|
||||
include ../../../conf/Makefile.local
|
||||
include ../../../conf/Makefile.avr
|
||||
|
||||
autopilot.install : warn_conf
|
||||
|
||||
warn_conf :
|
||||
@echo
|
||||
@echo '###########################################################'
|
||||
@grep AIRFRAME_NAME $(ACINCLUDE)/airframe.h
|
||||
@grep RADIO_NAME $(ACINCLUDE)/radio.h
|
||||
@grep FLIGHT_PLAN_NAME $(ACINCLUDE)/flight_plan.h
|
||||
@echo '###########################################################'
|
||||
@echo
|
||||
|
||||
|
||||
.depend : $(VARINCLUDE)/messages.h $(ACINCLUDE)/flight_plan.h $(VARINCLUDE)/ubx_protocol.h $(ACINCLUDE)/inflight_calib.h $(ACINCLUDE)/airframe.h $(ACINCLUDE)/radio.h
|
||||
main.o : $(VARINCLUDE)/messages.h
|
||||
nav.o : $(ACINCLUDE)/flight_plan.h
|
||||
gps_ubx.o : $(VARINCLUDE)/ubx_protocol.h
|
||||
if_calib.o : $(ACINCLUDE)/inflight_calib.h
|
||||
|
||||
clean : avr_clean
|
||||
rm -f *.out *.cm* messages.h flight_plan.h ubx_protocol.h inflight_calib.h
|
||||
@@ -0,0 +1,24 @@
|
||||
# $Id$
|
||||
# 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.
|
||||
#
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,126 @@
|
||||
/*
|
||||
* 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 <avr/signal.h>
|
||||
#include <avr/interrupt.h>
|
||||
#include <avr/io.h>
|
||||
#include "airframe.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 ?) */
|
||||
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,110 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* 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 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 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);
|
||||
|
||||
#endif /* AUTOPILOT_H */
|
||||
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
* Paparazzi mcu0 $Id$
|
||||
*
|
||||
* 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,170 @@
|
||||
/*
|
||||
* Paparazzi autopilot $Id$
|
||||
*
|
||||
* 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 INIT_IR2 (50.*50.)/* Ir value used for initialization */
|
||||
#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;
|
||||
|
||||
if (initialized) {
|
||||
float dt = gps_ftow - last_t;
|
||||
if (dt > 0.1) { // Against division by zero
|
||||
float phi = (estimator_hspeed_dir - last_hspeed_dir)/dt*NOMINAL_AIRSPEED/g; /* tan linearized */
|
||||
NORM_RAD_ANGLE(phi);
|
||||
estimator_ir = (float)ir_roll;
|
||||
estimator_rad = phi;
|
||||
float 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;
|
||||
estimator_rad_of_ir = sum_xy / sum_xx;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
initialized = TRUE;
|
||||
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$
|
||||
*
|
||||
* 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$
|
||||
*
|
||||
* 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,235 @@
|
||||
/*
|
||||
* Paparazzi mcu0 $Id$
|
||||
*
|
||||
* 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 <avr/io.h>
|
||||
#include <avr/signal.h>
|
||||
#include <avr/interrupt.h>
|
||||
#include <string.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 ) {
|
||||
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
|
||||
}
|
||||
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,207 @@
|
||||
/*
|
||||
* Paparazzi mcu0 $Id$
|
||||
*
|
||||
* 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 <avr/io.h>
|
||||
#include <avr/signal.h>
|
||||
#include <avr/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;
|
||||
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 inline void parse_ubx( uint8_t c ) {
|
||||
if (ubx_status < GOT_PAYLOAD) {
|
||||
ck_a += c;
|
||||
ck_b += ck_a;
|
||||
}
|
||||
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;
|
||||
}
|
||||
return;
|
||||
error:
|
||||
restart:
|
||||
ubx_status = UNINIT;
|
||||
return;
|
||||
}
|
||||
|
||||
#ifdef SIMUL
|
||||
ReceiveUart0(parse_ubx);
|
||||
#else
|
||||
ReceiveUart1(parse_ubx);
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,94 @@
|
||||
/*
|
||||
* $Id$
|
||||
* 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,18 @@
|
||||
#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
|
||||
|
||||
#define IF_CALIB_MODE_OF_PULSE(pprz) (pprz < TRESHOLD1 ? IF_CALIB_MODE_UP : \
|
||||
(pprz < TRESHOLD2 ? IF_CALIB_MODE_NONE : \
|
||||
IF_CALIB_MODE_DOWN))
|
||||
|
||||
|
||||
#endif // IF_CALIB_H
|
||||
@@ -0,0 +1,71 @@
|
||||
/*
|
||||
* Paparazzi mcu0 $Id$
|
||||
*
|
||||
* 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;
|
||||
|
||||
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,44 @@
|
||||
/*
|
||||
* Paparazzi mcu0 $Id$
|
||||
*
|
||||
* 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
|
||||
|
||||
#define AXIS_1_CHANNEL 4 /* P */
|
||||
#define AXIS_2_CHANNEL 5 /* other one */
|
||||
|
||||
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,122 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* 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 <avr/io.h>
|
||||
#include <avr/signal.h>
|
||||
#include <avr/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 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$
|
||||
*
|
||||
* 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,68 @@
|
||||
/*
|
||||
* Paparazzi mcu0 $Id$
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
/* Linear Least Square regression */
|
||||
|
||||
#include "lls.h"
|
||||
#include "infrared.h"
|
||||
|
||||
float lls_a;
|
||||
float lls_b;
|
||||
float lls_x;
|
||||
float lls_y;
|
||||
|
||||
void lls_init() {
|
||||
float lls_a_init = ir_gain;
|
||||
float lls_b_init = (float)(-ir_roll_neutral) * lls_a_init;
|
||||
lls_x = ir_roll_neutral + LLS_IR_HALF_INTERVAL;
|
||||
lls_y = lls_a_init * lls_x + lls_b_init;
|
||||
lls_update();
|
||||
lls_x = ir_roll_neutral - LLS_IR_HALF_INTERVAL;
|
||||
lls_y = lls_a_init * lls_x + lls_b_init;
|
||||
lls_update();
|
||||
}
|
||||
|
||||
void lls_update() {
|
||||
static float sum_x = 0.;
|
||||
static float sum_y = 0.;
|
||||
static float sum_xy = 0.;
|
||||
static float sum_x2 = 0.;
|
||||
static uint16_t n = 0;
|
||||
float fn;
|
||||
float mean_x, mean_y, c_xy, s2_x;
|
||||
|
||||
n++;
|
||||
sum_x += lls_x;
|
||||
sum_y += lls_y;
|
||||
sum_xy += lls_x * lls_y;
|
||||
sum_x2 += lls_x * lls_x;
|
||||
fn = (float)n;
|
||||
|
||||
mean_x = sum_x / fn;
|
||||
mean_y = sum_y / fn;
|
||||
c_xy = mean_x * mean_y + (sum_xy - mean_x * sum_y - mean_y * sum_x ) / fn;
|
||||
s2_x = mean_x * mean_x + (sum_x2 - 2* mean_x * sum_x) / fn;
|
||||
lls_a = c_xy / s2_x;
|
||||
lls_b = mean_y - lls_a * mean_x;
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
/*
|
||||
* Paparazzi mcu0 $Id$
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
/* Linear Least Square regression : y = lls_a * x + lls_b */
|
||||
|
||||
#ifndef LLS_H
|
||||
#define LLS_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#define LLS_IR_HALF_INTERVAL 150
|
||||
|
||||
extern float lls_a;
|
||||
extern float lls_b;
|
||||
extern float lls_x;
|
||||
extern float lls_y;
|
||||
|
||||
void lls_update(void);
|
||||
void lls_init(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,377 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* 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 <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 MIN_SPEED_FOR_TAKEOFF 5. // m/s
|
||||
|
||||
|
||||
uint8_t fatal_error_nb = 0;
|
||||
static const uint16_t version = 1;
|
||||
|
||||
static uint16_t cputime = 0; // seconds
|
||||
|
||||
uint8_t pprz_mode = PPRZ_MODE_MANUAL;
|
||||
uint8_t vertical_mode = VERTICAL_MODE_MANUAL;
|
||||
uint8_t ir_estim_mode = IR_ESTIM_MODE_ON;
|
||||
|
||||
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;
|
||||
|
||||
#ifdef CTL_BRD_V1_1
|
||||
struct adc_buf buf_bat;
|
||||
#endif
|
||||
|
||||
float slider_1_val, slider_2_val;
|
||||
|
||||
bool_t launch = FALSE;
|
||||
|
||||
|
||||
#define Min(x, y) (x < y ? x : y)
|
||||
#define Max(x, y) (x > y ? x : y)
|
||||
|
||||
|
||||
#define NO_CALIB 0
|
||||
#define WAITING_CALIB_CONTRAST 1
|
||||
#define CALIB_DONE 2
|
||||
|
||||
#define MAX_DELAY_FOR_CALIBRATION 10
|
||||
|
||||
|
||||
inline void ground_calibrate(void) {
|
||||
static uint8_t calib_status = NO_CALIB;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
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));
|
||||
nav_stage = 0; /* Restart the last current block */
|
||||
} else
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
#ifdef RADIO_LLS
|
||||
inline uint8_t ir_estim_mode_update( void ) {
|
||||
ModeUpdate(ir_estim_mode, IR_ESTIM_MODE_OF_PULSE(from_fbw.channels[RADIO_LLS]));
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
#define EVENT_DELAY 20
|
||||
|
||||
#define EventUpdate(_cpt, _cond, _event) \
|
||||
if (_cond) { \
|
||||
if (_cpt < EVENT_DELAY) { \
|
||||
_cpt++; \
|
||||
if (_cpt == EVENT_DELAY) \
|
||||
_event = TRUE; \
|
||||
} \
|
||||
} else { \
|
||||
_cpt = 0; \
|
||||
_event = FALSE; \
|
||||
}
|
||||
#define EventPos(_cpt, _channel, _event) \
|
||||
EventUpdate(_cpt, (inflight_calib_mode==IF_CALIB_MODE_NONE && from_fbw.channels[_channel]>MAX_PPRZ/2), _event)
|
||||
|
||||
#define EventNeg(_cpt, _channel, _event) \
|
||||
EventUpdate(_cpt, (inflight_calib_mode==IF_CALIB_MODE_NONE && from_fbw.channels[_channel]<-MAX_PPRZ/2), _event)
|
||||
|
||||
static inline void events_update(void) {
|
||||
static uint16_t event1_cpt = 0;
|
||||
EventPos(event1_cpt, RADIO_GAIN1, rc_event_1);
|
||||
static uint16_t event2_cpt = 0;
|
||||
EventNeg(event2_cpt, RADIO_GAIN1, rc_event_2);
|
||||
}
|
||||
|
||||
|
||||
/* Send back uncontrolled channels (only rudder) */
|
||||
inline void copy_from_to_fbw ( void ) {
|
||||
to_fbw.channels[RADIO_YAW] = from_fbw.channels[RADIO_YAW];
|
||||
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 INIT_MSG_NB 2
|
||||
#define HI_FREQ_PHASE_NB 5
|
||||
|
||||
static char ac_ident[16] = AIRFRAME_NAME;
|
||||
|
||||
#define PERIODIC_SEND_BAT() DOWNLINK_SEND_BAT(&vsupply, &estimator_flight_time, &low_battery, &block_time, &stage_time)
|
||||
#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);
|
||||
#define PERIODIC_SEND_ATTITUDE() DOWNLINK_SEND_ATTITUDE(&estimator_phi, &estimator_psi, &estimator_theta);
|
||||
#define PERIODIC_SEND_ADC() DOWNLINK_SEND_ADC(&ir_roll, &ir_pitch);
|
||||
#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);
|
||||
#define PERIODIC_SEND_IDENT() DOWNLINK_SEND_IDENT(ac_ident);
|
||||
|
||||
#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
|
||||
|
||||
|
||||
inline void reporting_task( void ) {
|
||||
static uint8_t boot = TRUE;
|
||||
|
||||
/* initialisation phase */
|
||||
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;
|
||||
}
|
||||
/* periodic reporting */
|
||||
else {
|
||||
PeriodicSend();
|
||||
}
|
||||
}
|
||||
|
||||
inline uint8_t inflight_calib_mode_update (void) {
|
||||
ModeUpdate(inflight_calib_mode, IF_CALIB_MODE_OF_PULSE(from_fbw.channels[RADIO_CALIB]));
|
||||
}
|
||||
|
||||
|
||||
inline void radio_control_task( void ) {
|
||||
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
|
||||
bool_t 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];
|
||||
// else asynchronously set by climb_pid_run();
|
||||
|
||||
mcu1_ppm_cpt = from_fbw.ppm_cpt;
|
||||
#ifndef CTL_BRD_V1_1
|
||||
vsupply = from_fbw.vsupply;
|
||||
#endif
|
||||
|
||||
events_update();
|
||||
|
||||
if (!estimator_flight_time) {
|
||||
ground_calibrate();
|
||||
if (pprz_mode == PPRZ_MODE_AUTO2 && from_fbw.channels[RADIO_THROTTLE] > GAZ_THRESHOLD_TAKEOFF) {
|
||||
launch = TRUE;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void navigation_task( void ) {
|
||||
|
||||
/* Compute desired_course */
|
||||
if (pprz_mode == PPRZ_MODE_HOME)
|
||||
nav_home();
|
||||
else
|
||||
nav_update_desired_course();
|
||||
|
||||
DOWNLINK_SEND_NAVIGATION(&nav_block, &nav_stage, &estimator_x, &estimator_y, &desired_course, &dist2_to_wp, &course_pgain, &dist2_to_home);
|
||||
|
||||
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
|
||||
course_pid_run(); /* aka compute desired_roll */
|
||||
desired_pitch = nav_pitch;
|
||||
|
||||
if (vertical_mode == VERTICAL_MODE_AUTO_ALT)
|
||||
altitude_pid_run();
|
||||
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.)
|
||||
inline void periodic_task( void ) { // 61 Hz
|
||||
static uint8_t _20Hz = 0;
|
||||
static uint8_t _4Hz = 0;
|
||||
static uint8_t _1Hz = 0;
|
||||
|
||||
estimator_t += PERIOD;
|
||||
|
||||
_20Hz++;
|
||||
if (_20Hz>=3) _20Hz=0;
|
||||
_4Hz++;
|
||||
if (_4Hz>=15) _4Hz=0;
|
||||
_1Hz++;
|
||||
if (_1Hz>=61) _1Hz=0;
|
||||
|
||||
if (!_1Hz) {
|
||||
if (estimator_flight_time) estimator_flight_time++;
|
||||
cputime++;
|
||||
stage_time++;
|
||||
block_time++;
|
||||
|
||||
#ifdef CTL_BRD_V1_1
|
||||
uint16_t av_bat_value = buf_bat.sum/AV_NB_SAMPLE;
|
||||
vsupply = VoltageOfAdc(av_bat_value) * 10.;
|
||||
#endif
|
||||
low_battery |= (vsupply < LOW_BATTERY);
|
||||
}
|
||||
switch(_4Hz) {
|
||||
case 0:
|
||||
estimator_propagate_state();
|
||||
navigation_task();
|
||||
break;
|
||||
// default:
|
||||
}
|
||||
switch (_20Hz) {
|
||||
case 0:
|
||||
break;
|
||||
case 1: {
|
||||
static uint8_t odd;
|
||||
odd++;
|
||||
if (odd & 0x01)
|
||||
reporting_task();
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
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;
|
||||
to_fbw.channels[RADIO_PITCH] = desired_elevator;
|
||||
|
||||
// Code for camera stabilization, FIXME put that elsewhere
|
||||
to_fbw.channels[RADIO_GAIN1] = TRIM_PPRZ(MAX_PPRZ/0.75*(-estimator_phi));
|
||||
|
||||
link_fbw_send();
|
||||
break;
|
||||
default:
|
||||
fatal_error_nb++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void use_gps_pos(void) {
|
||||
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();
|
||||
DOWNLINK_SEND_RAD_OF_IR(&estimator_ir, &estimator_rad, &estimator_rad_of_ir, &ir_roll_neutral, &ir_pitch_neutral);
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* 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 <avr/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"
|
||||
|
||||
|
||||
int main( void ) {
|
||||
/* 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();
|
||||
|
||||
/* start interrupt task */
|
||||
sei();
|
||||
|
||||
/* Wait 0.5s (for modem init ?) */
|
||||
uint8_t init_cpt = 30;
|
||||
while (init_cpt) {
|
||||
if (timer_periodic())
|
||||
init_cpt--;
|
||||
}
|
||||
|
||||
/* enter mainloop */
|
||||
while( 1 ) {
|
||||
if(timer_periodic())
|
||||
periodic_task();
|
||||
if (gps_msg_received) {
|
||||
parse_gps_msg();
|
||||
gps_msg_received = FALSE;
|
||||
if (gps_pos_available) {
|
||||
use_gps_pos();
|
||||
gps_pos_available = FALSE;
|
||||
}
|
||||
}
|
||||
if (link_fbw_receive_complete) {
|
||||
link_fbw_receive_complete = FALSE;
|
||||
radio_control_task();
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
* 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 <avr/io.h>
|
||||
#include <avr/signal.h>
|
||||
#include <avr/interrupt.h>
|
||||
#include <stdlib.h>
|
||||
#include "modem.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 ) {
|
||||
#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,204 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* 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;
|
||||
|
||||
#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; 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) { \
|
||||
while (x < 0) x += 360; \
|
||||
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); \
|
||||
}
|
||||
|
||||
static float carrot_x, carrot_y;
|
||||
|
||||
#define Goto3D(radius) { \
|
||||
int16_t yaw = from_fbw.channels[RADIO_YAW]; \
|
||||
if (yaw > MIN_DX || yaw < -MIN_DX) \
|
||||
carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \
|
||||
int16_t pitch = from_fbw.channels[RADIO_PITCH]; \
|
||||
if (pitch > MIN_DX || pitch < -MIN_DX) \
|
||||
carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \
|
||||
vertical_mode = VERTICAL_MODE_AUTO_ALT; \
|
||||
int16_t roll = from_fbw.channels[RADIO_ROLL]; \
|
||||
if (roll > MIN_DX || roll < -MIN_DX) \
|
||||
desired_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \
|
||||
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, desired_x, desired_y;
|
||||
uint16_t nav_desired_gaz;
|
||||
float nav_pitch = NAV_PITCH;
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
float 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 */
|
||||
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_desired_course(void) {
|
||||
compute_dist2_to_home();
|
||||
auto_nav();
|
||||
}
|
||||
|
||||
|
||||
void nav_init(void) {
|
||||
nav_block = 0;
|
||||
nav_stage = 0;
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
/*
|
||||
* Paparazzi mcu0 $Id$
|
||||
*
|
||||
* 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 */
|
||||
|
||||
void nav_update_desired_course(void);
|
||||
void nav_home(void);
|
||||
void nav_init(void);
|
||||
|
||||
#endif /* NAV_H */
|
||||
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* 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 <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "pid.h"
|
||||
|
||||
#include "autopilot.h"
|
||||
#include "infrared.h"
|
||||
//#include "gps.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;
|
||||
|
||||
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);
|
||||
float roll = course_pgain * err; // * fspeed / AIR_SPEED;
|
||||
if (roll > max_roll)
|
||||
desired_roll = max_roll;
|
||||
else if (roll < -max_roll)
|
||||
desired_roll = -max_roll;
|
||||
else desired_roll = 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;
|
||||
|
||||
void climb_pid_run ( void ) {
|
||||
float err = estimator_z_dot - desired_climb;
|
||||
float fgaz = climb_pgain * (err + climb_igain * climb_sum_err) + CLIMB_LEVEL_GAZ + (0.9-CLIMB_LEVEL_GAZ)/CLIMB_GAZ_MAX*desired_climb;
|
||||
climb_sum_err += err;
|
||||
desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ);
|
||||
}
|
||||
|
||||
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,58 @@
|
||||
/*
|
||||
* Paparazzi mcu0 $Id$
|
||||
*
|
||||
* 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;
|
||||
|
||||
void climb_pid_run(void);
|
||||
void altitude_pid_run(void);
|
||||
|
||||
#endif /* PID_H */
|
||||
@@ -0,0 +1,36 @@
|
||||
/*
|
||||
* Paparazzi autopilot $Id$
|
||||
*
|
||||
* 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,40 @@
|
||||
#include <inttypes.h>
|
||||
#include <avr/io.h>
|
||||
#include <avr/signal.h>
|
||||
#include <avr/interrupt.h>
|
||||
|
||||
|
||||
#include "spi.h"
|
||||
#include "autopilot.h"
|
||||
#include "link_fbw.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,52 @@
|
||||
# $Id$
|
||||
|
||||
all:
|
||||
@echo "call with 'make TARGET=... compile (or load)'"
|
||||
|
||||
TARGET=check_uart
|
||||
|
||||
ARCH = atmega128
|
||||
INCLUDES = -I ../ -I ../../../include
|
||||
|
||||
LOCAL_CFLAGS= $(CTL_BRD_FLAGS)
|
||||
|
||||
CONF_DIR = ../../../../conf
|
||||
CONF_XML = $(CONF_DIR)/conf.xml
|
||||
|
||||
ifneq ($(MAKECMDGOALS),clean)
|
||||
AIRFRAME_XML = $(CONF_DIR)/$(shell echo `../../../lib/ocaml/xml_get.out $(CONF_XML) "files" "airframe"`)
|
||||
CTL_BRD_VERSION=$(shell echo `../../../lib/ocaml/xml_get.out $(AIRFRAME_XML) "airframe" "ctl_board"`)
|
||||
endif
|
||||
|
||||
|
||||
|
||||
ifeq ($(CTL_BRD_VERSION),V1_2_1)
|
||||
CTL_BRD_FLAGS=-DCTL_BRD_V1_2_1
|
||||
endif
|
||||
|
||||
ifeq ($(CTL_BRD_VERSION),V1_2)
|
||||
CTL_BRD_FLAGS=-DCTL_BRD_V1_2
|
||||
endif
|
||||
|
||||
test_modem.srcs = test_modem.c ../modem.c
|
||||
|
||||
check_uart.srcs = check_uart.c ../uart.c
|
||||
|
||||
tx_adcs.srcs = tx_adcs.c ../uart.c ../adc.c
|
||||
|
||||
test_v2xe.srcs = test_v2xe.c
|
||||
|
||||
uart_tunnel.srcs = uart_tunnel.c ../uart.c
|
||||
check_spi.srcs = check_spi.c ../uart.c ../spi.c ../link_fbw.c
|
||||
|
||||
check_spi.o : INCLUDES = -I ../../../include -I ../ -I ../../fly_by_wire
|
||||
|
||||
test_fp: test_fp.c ../flight_plan.h
|
||||
cc -o $@ -I sim_avr -I .. -I ../../fly_by_wire ../nav.c test_fp.c -lm
|
||||
|
||||
include ../../../../conf/Makefile.local
|
||||
include ../../../../conf/Makefile.avr
|
||||
|
||||
|
||||
|
||||
clean: avr_clean
|
||||
@@ -0,0 +1,46 @@
|
||||
#include <avr/interrupt.h>
|
||||
#include "timer.h"
|
||||
#include "link_fbw.h"
|
||||
#include "uart.h"
|
||||
|
||||
uint8_t fatal_error_nb; /* Used in spi.c */
|
||||
|
||||
/* Fill the message with dummy values */
|
||||
void fill_spi_msg(void) {
|
||||
static pprz_t x;
|
||||
|
||||
uint8_t i;
|
||||
for(i = 0; i < RADIO_CTL_NB; i++)
|
||||
to_fbw.channels[i] = x++;
|
||||
to_fbw.status = 0xff;
|
||||
to_fbw.ppm_cpt = 0xff;
|
||||
to_fbw.vsupply = 0xff;
|
||||
}
|
||||
|
||||
int main( void ) {
|
||||
uart0_init();
|
||||
uart0_print_string("Booting AP MCU: $Id$\n");
|
||||
link_fbw_init();
|
||||
timer_init();
|
||||
sei();
|
||||
|
||||
uint8_t _1Hz = 0;
|
||||
while( 1 ) {
|
||||
if(timer_periodic()) {
|
||||
_1Hz++;
|
||||
if (_1Hz >= 60) {
|
||||
_1Hz = 0;
|
||||
uart0_print_string("AP MCU Alive\n");
|
||||
fill_spi_msg();
|
||||
link_fbw_send();
|
||||
}
|
||||
}
|
||||
if (link_fbw_receive_complete) {
|
||||
link_fbw_receive_complete = FALSE;
|
||||
if (link_fbw_receive_valid)
|
||||
uart0_print_string("SPI OK from fbw\n");
|
||||
else
|
||||
uart0_print_string("SPI error from fbw\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
/*
|
||||
* Paparazzi $Id$
|
||||
*
|
||||
* 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 <avr/interrupt.h>
|
||||
#include <avr/io.h>
|
||||
#include "timer.h"
|
||||
#include "uart.h"
|
||||
|
||||
int main( void ) {
|
||||
uart0_init();
|
||||
uart0_print_string("Booting AP MCU: $Id$\n");
|
||||
timer_init();
|
||||
sei();
|
||||
uint8_t _1Hz = 0;
|
||||
uint8_t foo = 0;
|
||||
while( 1 ) {
|
||||
if(timer_periodic()) {
|
||||
_1Hz++;
|
||||
if (_1Hz == 60) {
|
||||
_1Hz = 0;
|
||||
foo++;
|
||||
uart0_print_string("AP MCU uart0 check : alive [");
|
||||
uart0_print_hex(foo);
|
||||
uart0_print_string("]\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
/*
|
||||
* Paparazzi $Id$
|
||||
*
|
||||
* 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 <avr/io.h>
|
||||
#include <avr/interrupt.h>
|
||||
|
||||
#include "modem.h"
|
||||
#include "timer.h"
|
||||
|
||||
const uint8_t *msg = "ap modem alive\n";
|
||||
|
||||
int main( void ) {
|
||||
timer_init();
|
||||
modem_init();
|
||||
sei();
|
||||
while (1) {
|
||||
if (timer_periodic()) {
|
||||
uint8_t i = 0;
|
||||
while (msg[i]) {
|
||||
MODEM_PUT_1_BYTE(msg[i]);
|
||||
i++;
|
||||
}
|
||||
MODEM_CHECK_RUNNING();
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,277 @@
|
||||
/*
|
||||
* Paparazzi $Id$
|
||||
*
|
||||
* 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 <avr/io.h>
|
||||
#include <avr/interrupt.h>
|
||||
|
||||
#include "spi.h"
|
||||
#include "timer.h"
|
||||
|
||||
void uart_send(uint8_t c);
|
||||
|
||||
volatile uint8_t spi_cur_slave = SPI_NONE;
|
||||
|
||||
void my_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();
|
||||
|
||||
}
|
||||
|
||||
|
||||
void spi_start( void ) {
|
||||
uint8_t foo;
|
||||
/* Enable SPI, Master, MSB first, clock idle low, sample on leading edge, clock rate fck/128 */
|
||||
SPCR = (_BV(SPE)| _BV(MSTR) | _BV(SPR1) | _BV(SPR0));
|
||||
if (bit_is_set(SPSR, SPIF))
|
||||
foo = SPDR;
|
||||
SPI_SELECT_SLAVE1();
|
||||
}
|
||||
|
||||
uint8_t spi_transmit(uint8_t c) {
|
||||
uint8_t foo;
|
||||
SPDR = c;
|
||||
while (bit_is_clear(SPSR, SPIF));
|
||||
foo = inp(SPDR);
|
||||
|
||||
uart_send(c);
|
||||
uart_send(foo);
|
||||
return foo;
|
||||
}
|
||||
|
||||
void spi_stop(void) {
|
||||
SPI_UNSELECT_SLAVE1();
|
||||
SPI_STOP();
|
||||
}
|
||||
|
||||
|
||||
inline void delay_long(uint8_t n) {
|
||||
uint8_t ctx;
|
||||
while (n > 0) {
|
||||
ctx=0;
|
||||
do {ctx++;} while (ctx);
|
||||
n--;
|
||||
}
|
||||
}
|
||||
|
||||
#define SYNC_FLAG 0xAA
|
||||
#define TERMINATOR 0x00
|
||||
|
||||
#define GET_MODE_INFO 0x01
|
||||
#define MOD_INFO_RESP 0x02
|
||||
#define SET_DATA_COMPONENTS 0x03
|
||||
#define GET_DATA 0x04
|
||||
#define DATA_RESP 0x05
|
||||
#define SET_CONFIG 0x06
|
||||
#define GET_CONFIG 0x07
|
||||
#define CONFIG_RESP 0x08
|
||||
#define SAVE_CONFIG 0x09
|
||||
#define START_CAL 0x0A
|
||||
#define STOP_CAL 0x0B
|
||||
#define GET_CAL_DATA 0x0C
|
||||
#define CAL_DATA_RESP 0x0D
|
||||
#define SET_CAL_DATA 0x0E
|
||||
|
||||
#define DATA_XRAW 0x01 // Slnt32 counts 32768 to 32767
|
||||
#define DATA_YRAW 0x02 // Slnt32 counts 32768 to 32767
|
||||
#define DATA_XCAL 0x03 // Float32 scaled to 1.0
|
||||
#define DATA_YCAL 0x04 // Float32 scaled to 1.0
|
||||
#define DATA_HEADING 0x05 // Float32 degrees 0.0 ° to 359.9 °
|
||||
#define DATA_MAGNITUDE 0x06 // Float32 scaled to 1.0
|
||||
#define DATA_TEMPERATURE 0x07 // Float32 ° Celsius
|
||||
#define DATA_DISTORTION 0x08 // Boolean
|
||||
#define DATA_CAL_STATUS 0x09 // Boolean
|
||||
|
||||
uint8_t err_cnt;
|
||||
uint8_t errno;
|
||||
|
||||
#define DATA_FIELD_NB 3
|
||||
#define DATA_LEN 12
|
||||
void v2xe_setup_data_components(void) {
|
||||
uint8_t c;
|
||||
spi_start();
|
||||
c = spi_transmit(SYNC_FLAG);
|
||||
c = spi_transmit(SET_DATA_COMPONENTS);
|
||||
c = spi_transmit(DATA_FIELD_NB);
|
||||
c = spi_transmit(DATA_XRAW);
|
||||
c = spi_transmit(DATA_YRAW);
|
||||
c = spi_transmit(DATA_TEMPERATURE);
|
||||
c = spi_transmit(TERMINATOR);
|
||||
spi_stop();
|
||||
uart_send(0xFF);
|
||||
}
|
||||
|
||||
void v2xe_read_data(void) {
|
||||
uint8_t c, i=0;
|
||||
spi_start();
|
||||
|
||||
/* querry */
|
||||
c = spi_transmit(SYNC_FLAG);
|
||||
c = spi_transmit(GET_DATA);
|
||||
c = spi_transmit(TERMINATOR);
|
||||
|
||||
delay_long(255);
|
||||
|
||||
/* answer */
|
||||
do { c = spi_transmit(0x00); i++;}
|
||||
while (c!=SYNC_FLAG && i < 20);
|
||||
// if (i>20) TIMEOUT;
|
||||
/* frame type */
|
||||
c = spi_transmit(0x00);
|
||||
/* nb fields */
|
||||
c = spi_transmit(0x00);
|
||||
/* fields + data */
|
||||
for (i=0; i < DATA_LEN + DATA_FIELD_NB + 1; i++) {
|
||||
c = spi_transmit(0x00);
|
||||
}
|
||||
|
||||
spi_stop();
|
||||
uart_send(0xFF);
|
||||
}
|
||||
|
||||
#define ID_LEN 8
|
||||
uint8_t id_str[ID_LEN];
|
||||
void v2xe_read_id (void) {
|
||||
uint8_t c, i=0;
|
||||
|
||||
spi_start();
|
||||
c = spi_transmit(SYNC_FLAG);
|
||||
c = spi_transmit(GET_MODE_INFO);
|
||||
c = spi_transmit(TERMINATOR);
|
||||
|
||||
// delay_long(10);
|
||||
|
||||
// c = spi_transmit(0x00);
|
||||
|
||||
|
||||
do { c = spi_transmit(0x00); i++;}
|
||||
while (c!=SYNC_FLAG && i < 20);
|
||||
|
||||
/* if (c != SYNC_FLAG) { */
|
||||
/* err_cnt++; */
|
||||
/* errno = 1; */
|
||||
/* spi_stop(); */
|
||||
/* return; */
|
||||
/* } */
|
||||
// c = spi_transmit(0x00);
|
||||
/* if (c != MOD_INFO_RESP) { */
|
||||
/* err_cnt++; */
|
||||
/* errno = 2; */
|
||||
/* spi_stop(); */
|
||||
/* return; */
|
||||
/* } */
|
||||
for (c = 0; c < ID_LEN; c++)
|
||||
id_str[c] = spi_transmit(0x00);
|
||||
c = spi_transmit(0x00);
|
||||
/* if (c != TERMINATOR) { */
|
||||
/* err_cnt++; */
|
||||
/* errno = 3; */
|
||||
/* spi_stop(); */
|
||||
/* return; */
|
||||
/* } */
|
||||
|
||||
spi_stop();
|
||||
|
||||
uart_send(0xFF);
|
||||
}
|
||||
|
||||
|
||||
#define UBRRH UBRR0H
|
||||
#define UBRRL UBRR0L
|
||||
#define UCSRA UCSR0A
|
||||
#define UCSRB UCSR0B
|
||||
#define UCSRC UCSR0C
|
||||
#define UDR UDR0
|
||||
|
||||
void uart_init(void) {
|
||||
/* Baudrate is 38.4k */
|
||||
UBRRH = 0;
|
||||
UBRRL = 25;
|
||||
/* single speed */
|
||||
UCSRA = 0;
|
||||
/* Enable receiver and transmitter */
|
||||
UCSRB = _BV(RXEN) | _BV(TXEN);
|
||||
/* Set frame format: 8data, 1stop bit */
|
||||
UCSRC = _BV(UCSZ1) | _BV(UCSZ0);
|
||||
|
||||
}
|
||||
|
||||
void uart_send(uint8_t c) {
|
||||
/* Wait for empty transmit buffer */
|
||||
while ( !( UCSRA & _BV(UDRE)) ) ;
|
||||
/* Put data into buffer, sends the data */
|
||||
UDR = c;
|
||||
}
|
||||
|
||||
|
||||
inline void periodic_task(void) {
|
||||
static uint8_t foo;
|
||||
if (foo == 0)
|
||||
v2xe_read_id();
|
||||
if (foo == 10)
|
||||
v2xe_setup_data_components();
|
||||
if (foo > 10 && !(foo%10))
|
||||
v2xe_read_data();
|
||||
// uart_send(err_cnt);
|
||||
// uart_send (errno);
|
||||
// {
|
||||
// uint8_t i;
|
||||
// for (i=0; i<8; i++)
|
||||
// uart_send(id_str[i]);
|
||||
// }
|
||||
// uart_send('\n');
|
||||
foo++;
|
||||
if (foo > 60) foo=0;
|
||||
}
|
||||
|
||||
int main( void ) {
|
||||
|
||||
timer_init();
|
||||
my_spi_init();
|
||||
uart_init();
|
||||
// sei();
|
||||
|
||||
delay_long(255);
|
||||
delay_long(255);
|
||||
|
||||
while (1) {
|
||||
if (timer_periodic())
|
||||
periodic_task();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
/*
|
||||
* Paparazzi $Id$
|
||||
*
|
||||
* 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 <avr/interrupt.h>
|
||||
#include <avr/io.h>
|
||||
#include "timer.h"
|
||||
#include "uart.h"
|
||||
#include "adc.h"
|
||||
|
||||
static struct adc_buf buffers[NB_ADC];
|
||||
|
||||
void transmit_adc(void) {
|
||||
uint8_t i;
|
||||
uart0_transmit((uint8_t)0); uart0_transmit((uint8_t)0);
|
||||
for(i = 0; i < NB_ADC; i++) {
|
||||
uint16_t value = buffers[i].sum / AV_NB_SAMPLE;
|
||||
uart0_transmit((uint8_t)(value >> 8));
|
||||
uart0_transmit((uint8_t)(value & 0xff));
|
||||
}
|
||||
uart0_transmit((uint8_t)'\n');
|
||||
}
|
||||
|
||||
int main( void ) {
|
||||
uint8_t i;
|
||||
uart0_init();
|
||||
timer_init();
|
||||
adc_init();
|
||||
for(i = 0; i < NB_ADC; i++)
|
||||
adc_buf_channel(i, &buffers[i]);
|
||||
sei();
|
||||
|
||||
while( 1 ) {
|
||||
static uint8_t _1Hz = 0;
|
||||
|
||||
if(timer_periodic()) {
|
||||
_1Hz++;
|
||||
if (_1Hz == 60) {
|
||||
_1Hz = 0;
|
||||
transmit_adc();
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
#include <inttypes.h>
|
||||
#include <avr/io.h>
|
||||
#include <avr/signal.h>
|
||||
#include <avr/interrupt.h>
|
||||
#include <math.h>
|
||||
|
||||
|
||||
#include "../uart.h"
|
||||
#include "../timer.h"
|
||||
|
||||
void on_uart0_rx(uint8_t c) {
|
||||
uart1_transmit(c);
|
||||
}
|
||||
|
||||
void on_uart1_rx(uint8_t c) {
|
||||
uart0_transmit(c);
|
||||
}
|
||||
|
||||
ReceiveUart0(on_uart0_rx)
|
||||
ReceiveUart1(on_uart1_rx)
|
||||
|
||||
int main( void ) {
|
||||
uart0_init();
|
||||
uart1_init();
|
||||
|
||||
sei();
|
||||
return 0;
|
||||
|
||||
}
|
||||
@@ -0,0 +1,91 @@
|
||||
/*
|
||||
* 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 <avr/signal.h>
|
||||
#include <avr/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,138 @@
|
||||
/*
|
||||
* Paparazzi $Id$
|
||||
*
|
||||
* 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 <avr/signal.h>
|
||||
#include <avr/interrupt.h>
|
||||
#include <avr/io.h>
|
||||
#include "uart.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( 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( 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;
|
||||
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$
|
||||
*
|
||||
* 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);
|
||||
extern void uart0_transmit(const uint8_t);
|
||||
extern void uart1_transmit(const uint8_t);
|
||||
|
||||
#define ReceiveUart0(cb) \
|
||||
SIGNAL( SIG_UART0_RECV ) { \
|
||||
uint8_t c = inp(UDR0); \
|
||||
cb(c); \
|
||||
}
|
||||
#define ReceiveUart1(cb) \
|
||||
SIGNAL( SIG_UART1_RECV ) { \
|
||||
uint8_t c = inp(UDR1); \
|
||||
cb(c); \
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,38 @@
|
||||
/*
|
||||
* Paparazzi autopilot $Id$
|
||||
*
|
||||
* Copyright (C) 2004 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* UBX protocol specific code
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#ifndef UBX_H
|
||||
#define UBX_H
|
||||
|
||||
#define GPS_FIX_VALID(gps_mode) (gps_mode == 3)
|
||||
extern const int32_t utm_east0;
|
||||
extern const int32_t utm_north0;
|
||||
|
||||
#endif /* UBX_H */
|
||||
@@ -0,0 +1,77 @@
|
||||
#
|
||||
# $Id$
|
||||
# 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.
|
||||
#
|
||||
|
||||
LOCAL_CFLAGS= $(CTL_BRD_FLAGS)
|
||||
|
||||
ARCH = atmega8
|
||||
TARGET = fbw
|
||||
#LOW_FUSE = 3f # crystal #
|
||||
#LOW_FUSE = 31 # internal 1MHz #
|
||||
#LOW_FUSE = 1e # ceramic resonator slow rising power p26 #
|
||||
LOW_FUSE = 2e # ceramic resonator slow rising power p26 #
|
||||
HIGH_FUSE = cb
|
||||
EXT_FUSE = ff
|
||||
LOCK_FUSE = ff
|
||||
VARINCLUDE = $(PAPARAZZI_HOME)/var/include
|
||||
ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT)
|
||||
INCLUDES = -I ../../include -I $(VARINCLUDE) -I $(ACINCLUDE)
|
||||
|
||||
ifeq ($(CTL_BRD_VERSION),V1_2_1)
|
||||
CTL_BRD_FLAGS=-DCTL_BRD_V1_2_1
|
||||
endif
|
||||
|
||||
ifeq ($(CTL_BRD_VERSION),V1_2)
|
||||
CTL_BRD_FLAGS=-DCTL_BRD_V1_2
|
||||
endif
|
||||
|
||||
ifeq ($(CTL_BRD_VERSION),V1_1)
|
||||
CTL_BRD_FLAGS=-DCTL_BRD_V1_1
|
||||
endif
|
||||
|
||||
|
||||
$(TARGET).srcs = \
|
||||
main.c \
|
||||
ppm.c \
|
||||
servo.c \
|
||||
spi.c \
|
||||
uart.c \
|
||||
adc_fbw.c \
|
||||
|
||||
|
||||
include ../../../conf/Makefile.local
|
||||
include ../../../conf/Makefile.avr
|
||||
|
||||
fbw.install : warn_conf
|
||||
|
||||
warn_conf :
|
||||
@echo
|
||||
@echo '###########################################################'
|
||||
@grep AIRFRAME_NAME $(ACINCLUDE)/airframe.h
|
||||
@grep RADIO_NAME $(ACINCLUDE)/radio.h
|
||||
@echo '###########################################################'
|
||||
@echo
|
||||
|
||||
|
||||
main.o .depend : $(ACINCLUDE)/radio.h $(ACINCLUDE)/airframe.h
|
||||
|
||||
clean : avr_clean
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
# $Id$
|
||||
# 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.
|
||||
#
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,120 @@
|
||||
/*
|
||||
* Paparazzi fly by wire adc functions
|
||||
*
|
||||
* Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
*
|
||||
* Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
//// ADC3 MVSUP
|
||||
//// ADC6 MVSERVO
|
||||
|
||||
|
||||
#include <avr/signal.h>
|
||||
#include <avr/interrupt.h>
|
||||
#include <avr/io.h>
|
||||
#include "adc_fbw.h"
|
||||
|
||||
|
||||
|
||||
#if defined CTL_BRD_V1_2 || defined CTL_BRD_V1_2_1
|
||||
|
||||
#define VOLTAGE_TIME 0x07
|
||||
#define ANALOG_PORT PORTC
|
||||
#define ANALOG_PORT_DIR DDRC
|
||||
|
||||
|
||||
//
|
||||
#define ANALOG_VREF _BV(REFS0) | _BV(REFS1)
|
||||
|
||||
uint16_t adc_samples[ NB_ADC ];
|
||||
|
||||
static struct adc_buf* buffers[NB_ADC];
|
||||
|
||||
void 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 */
|
||||
ADMUX = ANALOG_VREF;
|
||||
|
||||
/* Select out clock, turn on the ADC interrupt and start conversion */
|
||||
ADCSRA = 0
|
||||
| VOLTAGE_TIME
|
||||
| _BV(ADEN )
|
||||
| _BV(ADIE )
|
||||
| _BV(ADSC );
|
||||
|
||||
/* Init to 0 (usefull ?) */
|
||||
for(i = 0; i < NB_ADC; i++)
|
||||
buffers[i] = (struct adc_buf*)0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Called when the voltage conversion is finished
|
||||
*
|
||||
* 8.913kHz on mega128@16MHz 1kHz/channel ??
|
||||
*/
|
||||
|
||||
|
||||
SIGNAL( SIG_ADC )
|
||||
{
|
||||
uint8_t adc_input = ADMUX & 0x7;
|
||||
struct adc_buf* buf = buffers[adc_input];
|
||||
uint16_t adc_value = ADCW;
|
||||
/* Store result */
|
||||
adc_samples[ adc_input ] = adc_value;
|
||||
|
||||
if (buf) {
|
||||
uint8_t new_head = buf->head + 1;
|
||||
if (new_head >= AV_NB_SAMPLE) new_head = 0;
|
||||
buf->sum -= buf->values[new_head];
|
||||
buf->values[new_head] = adc_value;
|
||||
buf->sum += adc_value;
|
||||
buf->head = new_head;
|
||||
}
|
||||
|
||||
/* Find the next input */
|
||||
adc_input++;
|
||||
if (adc_input == 4)
|
||||
adc_input = 6; // ADC 4 and 5 for i2c
|
||||
if( adc_input >= 8 ) {
|
||||
adc_input = 0;
|
||||
#ifdef CTL_BRD_V1_2
|
||||
adc_input = 1; // WARNING ADC0 is for rservo driver reset on v1.2.0
|
||||
#endif /* CTL_BRD_V1_2 */
|
||||
}
|
||||
/* Select it */
|
||||
ADMUX = adc_input | ANALOG_VREF;
|
||||
/* Restart the conversion */
|
||||
sbi( ADCSR, ADSC );
|
||||
}
|
||||
|
||||
#endif /* CTL_BRD_V1_2 || CTL_BRD_V1_2_1 */
|
||||
@@ -0,0 +1,61 @@
|
||||
/*
|
||||
* Paparazzi fly by wire adc functions
|
||||
*
|
||||
* Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
*
|
||||
* Copyright (C) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef _ADC_H_
|
||||
#define _ADC_H_
|
||||
|
||||
#include "airframe.h"
|
||||
|
||||
#if defined CTL_BRD_V1_2 || defined CTL_BRD_V1_2_1
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
|
||||
#define NB_ADC 8
|
||||
|
||||
/* Array containing the last measured value */
|
||||
extern uint16_t adc_samples[ NB_ADC ];
|
||||
|
||||
void adc_init( void );
|
||||
|
||||
#define AV_NB_SAMPLE 0x20
|
||||
|
||||
struct adc_buf {
|
||||
uint16_t sum;
|
||||
uint16_t values[AV_NB_SAMPLE];
|
||||
uint8_t head;
|
||||
};
|
||||
|
||||
/* Facility to store last values in a circular buffer for a specific
|
||||
channel: allocate a (struct adc_buf) and register it with the following
|
||||
function */
|
||||
void adc_buf_channel(uint8_t adc_channel, struct adc_buf* s);
|
||||
|
||||
|
||||
#endif /* CTL_BRD_V1_2 || CTL_BRD_V1_2 */
|
||||
|
||||
#endif /* _ADC_H_ */
|
||||
@@ -0,0 +1,65 @@
|
||||
/* $Id$
|
||||
*
|
||||
* (c) 2003 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef LINK_AUTOPILOT_H
|
||||
#define LINK_AUTOPILOT_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#include "std.h"
|
||||
#include "radio.h"
|
||||
#include "airframe.h"
|
||||
|
||||
/*
|
||||
* System clock in MHz.
|
||||
*/
|
||||
#define CLOCK 16
|
||||
|
||||
typedef int16_t pprz_t; // type of commands
|
||||
|
||||
/* !!!!!!!!!!!!!!!!!!! Value used in gen_airframe.ml !!!!!!!!!!!!!!!!! */
|
||||
#define MAX_PPRZ (600 * CLOCK)
|
||||
#define MIN_PPRZ -MAX_PPRZ
|
||||
|
||||
struct inter_mcu_msg {
|
||||
int16_t channels[RADIO_CTL_NB];
|
||||
uint8_t ppm_cpt;
|
||||
uint8_t status;
|
||||
uint8_t nb_err;
|
||||
uint8_t vsupply; /* 1e-1 V */
|
||||
};
|
||||
|
||||
// Status bits from FBW to AUTOPILOT
|
||||
#define STATUS_RADIO_OK 0
|
||||
#define RADIO_REALLY_LOST 1
|
||||
#define AVERAGED_CHANNELS_SENT 2
|
||||
#define MASK_FBW_CHANGED 0x3
|
||||
|
||||
// Statut bits from AUTOPILOT to FBW
|
||||
#define STATUS_AUTO_OK 0
|
||||
|
||||
#define FRAME_LENGTH (sizeof(struct inter_mcu_msg)+1)
|
||||
|
||||
#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
|
||||
|
||||
#endif // LINK_AUTOPILOT_H
|
||||
@@ -0,0 +1,182 @@
|
||||
/*
|
||||
* Paparazzi $Id$
|
||||
*
|
||||
* 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 <avr/io.h>
|
||||
#include <avr/signal.h>
|
||||
#include <avr/interrupt.h>
|
||||
|
||||
#include "timer.h"
|
||||
#include "servo.h"
|
||||
#include "ppm.h"
|
||||
#include "spi.h"
|
||||
#include "link_autopilot.h"
|
||||
#include "radio.h"
|
||||
|
||||
|
||||
#include "uart.h"
|
||||
|
||||
|
||||
#ifndef CTL_BRD_V1_1
|
||||
#include "adc_fbw.h"
|
||||
struct adc_buf vsupply_adc_buf;
|
||||
struct adc_buf vservos_adc_buf;
|
||||
#endif
|
||||
|
||||
uint8_t mode;
|
||||
static uint8_t time_since_last_mega128;
|
||||
static uint16_t time_since_last_ppm;
|
||||
bool_t radio_ok, mega128_ok, radio_really_lost;
|
||||
|
||||
static const pprz_t failsafe[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||
|
||||
static uint8_t ppm_cpt, last_ppm_cpt;
|
||||
|
||||
#define STALLED_TIME 30 // 500ms with a 60Hz timer
|
||||
#define REALLY_STALLED_TIME 300 // 5s with a 60Hz timer
|
||||
|
||||
|
||||
/* static inline void status_transmit( void ) { */
|
||||
/* uint8_t i; */
|
||||
/* uart_transmit(7); */
|
||||
/* uart_transmit(7); */
|
||||
/* for (i=0; i<sizeof(struct inter_mcu_msg); i++) */
|
||||
/* uart_transmit(((uint8_t*)&to_mega128)[i]); */
|
||||
/* uart_transmit('\n'); */
|
||||
|
||||
/* uart_transmit(7); */
|
||||
/* uart_transmit(7); */
|
||||
/* uint8_t i; */
|
||||
/* for(i = 0; i < RADIO_CTL_NB; i++) { */
|
||||
/* extern uint16_t ppm_pulses[]; */
|
||||
/* uart_transmit(ppm_pulses[i]>>8); */
|
||||
/* uart_transmit(ppm_pulses[i] & 0xff); */
|
||||
/* } */
|
||||
/* uart_transmit('\n'); */
|
||||
/* } */
|
||||
|
||||
|
||||
/* Prepare data to be sent to mcu0 */
|
||||
static inline void to_autopilot_from_last_radio (void) {
|
||||
uint8_t i;
|
||||
for(i = 0; i < RADIO_CTL_NB; i++)
|
||||
to_mega128.channels[i] = last_radio[i];
|
||||
to_mega128.status = (radio_ok ? _BV(STATUS_RADIO_OK) : 0);
|
||||
to_mega128.status |= (radio_really_lost ? _BV(RADIO_REALLY_LOST) : 0);
|
||||
if (last_radio_contains_avg_channels) {
|
||||
to_mega128.status |= _BV(AVERAGED_CHANNELS_SENT);
|
||||
last_radio_contains_avg_channels = FALSE;
|
||||
}
|
||||
to_mega128.ppm_cpt = last_ppm_cpt;
|
||||
#ifndef CTL_BRD_V1_1
|
||||
to_mega128.vsupply = VoltageOfAdc(vsupply_adc_buf.sum/AV_NB_SAMPLE) * 10;
|
||||
#else
|
||||
to_mega128.vsupply = 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
int main( void )
|
||||
{
|
||||
uart_init_tx();
|
||||
uart_print_string("FBW Booting $Id$\n");
|
||||
|
||||
#ifndef CTL_BRD_V1_1
|
||||
adc_init();
|
||||
adc_buf_channel(3, &vsupply_adc_buf);
|
||||
adc_buf_channel(6, &vservos_adc_buf);
|
||||
#endif
|
||||
timer_init();
|
||||
servo_init();
|
||||
ppm_init();
|
||||
spi_init();
|
||||
sei();
|
||||
while( 1 ) {
|
||||
if( ppm_valid ) {
|
||||
ppm_valid = FALSE;
|
||||
ppm_cpt++;
|
||||
radio_ok = TRUE;
|
||||
radio_really_lost = FALSE;
|
||||
time_since_last_ppm = 0;
|
||||
last_radio_from_ppm();
|
||||
if (last_radio_contains_avg_channels) {
|
||||
mode = MODE_OF_PPRZ(last_radio[RADIO_MODE]);
|
||||
}
|
||||
if (mode == MODE_MANUAL) {
|
||||
servo_set(last_radio);
|
||||
}
|
||||
} else if (mode == MODE_MANUAL && radio_really_lost) {
|
||||
mode = MODE_AUTO;
|
||||
}
|
||||
|
||||
if ( !SpiIsSelected() && spi_was_interrupted ) {
|
||||
spi_was_interrupted = FALSE;
|
||||
if (mega128_receive_valid) {
|
||||
time_since_last_mega128 = 0;
|
||||
mega128_ok = TRUE;
|
||||
if (mode == MODE_AUTO)
|
||||
servo_set(from_mega128.channels);
|
||||
}
|
||||
to_autopilot_from_last_radio();
|
||||
spi_reset();
|
||||
}
|
||||
|
||||
if (time_since_last_ppm >= STALLED_TIME) {
|
||||
radio_ok = FALSE;
|
||||
}
|
||||
if (time_since_last_ppm >= REALLY_STALLED_TIME) {
|
||||
radio_really_lost = TRUE;
|
||||
}
|
||||
if (time_since_last_mega128 == STALLED_TIME) {
|
||||
mega128_ok = FALSE;
|
||||
}
|
||||
|
||||
if ((mode == MODE_MANUAL && !radio_ok) ||
|
||||
(mode == MODE_AUTO && !mega128_ok)) {
|
||||
servo_set(failsafe);
|
||||
}
|
||||
|
||||
if(timer_periodic()) {
|
||||
static uint8_t _1Hz;
|
||||
static uint8_t _20Hz;
|
||||
_1Hz++;
|
||||
_20Hz++;
|
||||
if (_1Hz >= 60) {
|
||||
_1Hz = 0;
|
||||
last_ppm_cpt = ppm_cpt;
|
||||
ppm_cpt = 0;
|
||||
}
|
||||
if (_20Hz >= 3) {
|
||||
_20Hz = 0;
|
||||
servo_transmit();
|
||||
// status_transmit();
|
||||
}
|
||||
if (time_since_last_mega128 < STALLED_TIME)
|
||||
time_since_last_mega128++;
|
||||
if (time_since_last_ppm < REALLY_STALLED_TIME)
|
||||
time_since_last_ppm++;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,135 @@
|
||||
/* $Id$
|
||||
* Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
*
|
||||
* (c) 2003 Trammell Hudson <hudson@rotomotion.com>
|
||||
* (c) 2003 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "radio.h"
|
||||
#include "ppm.h"
|
||||
|
||||
#define AVERAGING_PERIOD (PPM_FREQ/4)
|
||||
|
||||
/*
|
||||
* Pulse width is computed as the difference between now and the
|
||||
* previous pulse. If no pulse has been received between then and
|
||||
* now, the time of the last pulse will be equal to the last pulse
|
||||
* we measured. Unfortunately, the Input Capture Flag (ICF1) will
|
||||
* not be set since the interrupt routine disables it.
|
||||
*
|
||||
* Sync pulses are timed with Timer2, which runs at Clk/1024. This
|
||||
* is slow enough at both 4 and 8 Mhz to measure the lengthy (10ms
|
||||
* or longer) pulse.
|
||||
*
|
||||
* Otherwise, compute the pulse width with the 16-bit timer1,
|
||||
* push the pulse width onto the stack and increment the
|
||||
* pulse counter until we have received eight pulses.
|
||||
*/
|
||||
|
||||
uint16_t ppm_pulses[ PPM_NB_PULSES ];
|
||||
pprz_t last_radio[ PPM_NB_PULSES ];
|
||||
pprz_t avg_last_radio[ PPM_NB_PULSES ];
|
||||
bool_t last_radio_contains_avg_channels = FALSE;
|
||||
volatile bool_t ppm_valid;
|
||||
|
||||
/* MC3030, Trame PPM7: 25ms, 10.4 au neutre,
|
||||
sync pulse = 16.2ms with low value on every channels */
|
||||
|
||||
|
||||
|
||||
|
||||
#define RestartPpmCycle() { state = 0; sync_start = TCNT2; return; }
|
||||
|
||||
SIGNAL( SIG_INPUT_CAPTURE1 )
|
||||
{
|
||||
static uint16_t last;
|
||||
uint16_t this;
|
||||
uint16_t width;
|
||||
static uint8_t state;
|
||||
static uint8_t sync_start;
|
||||
|
||||
this = ICR1;
|
||||
width = this - last;
|
||||
last = this;
|
||||
|
||||
if( state == 0 ) {
|
||||
uint8_t end = inp( TCNT2 );
|
||||
uint8_t diff = (end - sync_start);
|
||||
sync_start = end;
|
||||
|
||||
/* The frame period of the mc3030 seems to be 25ms.
|
||||
* One pulse lasts from 1.05ms to 2.150ms.
|
||||
* Sync pulse is at least 7ms : (7000*CLOCK)/1024 = 109
|
||||
*/
|
||||
if( diff > (uint8_t)(((uint32_t)(7000ul*CLOCK))/1024ul) ) {
|
||||
state = 1;
|
||||
}
|
||||
}
|
||||
else {
|
||||
/* Read a data pulses */
|
||||
if( width < 700ul*CLOCK || width > 2300ul*CLOCK)
|
||||
RestartPpmCycle();
|
||||
ppm_pulses[state - 1] = width;
|
||||
|
||||
if (state >= PPM_NB_PULSES) {
|
||||
ppm_valid = 1;
|
||||
RestartPpmCycle();
|
||||
} else
|
||||
state++;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
#define Int16FromPulse(i) (int16_t)((ppm_pulses[(i)] - PpmOfUs(((int[])RADIO_NEUTRALS_US)[i]))*(2*MAX_PPRZ)/(PpmOfUs(((int[])RADIO_MAXS_US[i])-((int[])RADIO_MINS_US[i]))))
|
||||
|
||||
|
||||
/* Copy from the ppm receiving buffer to the buffer sent to mcu0 */
|
||||
void last_radio_from_ppm() {
|
||||
static uint8_t avg_cpt = 0; /* Counter for averaging */
|
||||
uint8_t i;
|
||||
|
||||
for(i = 0; i < RADIO_CTL_NB; i++) {
|
||||
int16_t pprz = Int16FromPulse(i);
|
||||
if (pprz > MAX_PPRZ)
|
||||
pprz = MAX_PPRZ;
|
||||
else if (pprz < MIN_PPRZ)
|
||||
pprz = MIN_PPRZ;
|
||||
|
||||
if (i == RADIO_THROTTLE) {
|
||||
int16_t gaz = pprz/2;
|
||||
last_radio[i] = (gaz < 0 ? 0 : gaz);
|
||||
} else if (AveragedChannel(i)) {
|
||||
avg_last_radio[i] += pprz / AVERAGING_PERIOD;
|
||||
} else
|
||||
last_radio[i] = pprz;
|
||||
}
|
||||
|
||||
avg_cpt++;
|
||||
if (avg_cpt == AVERAGING_PERIOD) {
|
||||
avg_cpt = 0;
|
||||
for(i = 0; i < RADIO_CTL_NB; i++)
|
||||
if (AveragedChannel(i)) {
|
||||
last_radio[i] = avg_last_radio[i];
|
||||
avg_last_radio[i] = 0;
|
||||
}
|
||||
last_radio_contains_avg_channels = TRUE;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
/* $Id$
|
||||
*
|
||||
* Decoder for the trainer ports or hacked receivers for both
|
||||
* Futaba and JR formats. The ppm_valid flag is set whenever
|
||||
* a valid frame is received.
|
||||
*
|
||||
* Pulse widths are stored as unscaled 16-bit values in ppm_pulses[].
|
||||
* If you require actual microsecond values, divide by CLOCK.
|
||||
* For an 8 Mhz clock and typical servo values, these will range
|
||||
* from 0x1F00 to 0x4000.
|
||||
*
|
||||
* Copied from autopilot (autopilot.sf.net) thanx alot Trammell
|
||||
*
|
||||
* (c) 2002 Trammell Hudson <hudson@rotomotion.com>
|
||||
* (c) 2003 Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PPM_H
|
||||
#define PPM_H
|
||||
|
||||
|
||||
/**
|
||||
* Receiver types
|
||||
*/
|
||||
#define RXFUTABA 0
|
||||
#define RXJR 1
|
||||
|
||||
#define PPM_RX_TYPE RXFUTABA
|
||||
#define PPM_FREQ 40 // 25ms
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <avr/signal.h>
|
||||
|
||||
#include "timer.h"
|
||||
#include "link_autopilot.h"
|
||||
|
||||
#define PpmOfUs(x) ((x)*CLOCK)
|
||||
|
||||
#define PPM_DDR DDRB
|
||||
#define PPM_PORT PORTB
|
||||
#define PPM_PIN PB0
|
||||
|
||||
/*
|
||||
* PPM pulses are falling edge clocked on the ICP, which records
|
||||
* the state of the global clock. We do not use any noise
|
||||
* canceling features.
|
||||
*
|
||||
* JR might be rising edge clocked; set that as an option
|
||||
*/
|
||||
static inline void
|
||||
ppm_init( void )
|
||||
{
|
||||
#if PPM_RX_TYPE == RXFUTABA
|
||||
cbi( TCCR1B, ICES1 );
|
||||
#elif PPM_RX_TYPE == RXJR
|
||||
sbi( TCCR1B, ICES1 );
|
||||
#else
|
||||
# error "ppm.h: Unknown receiver type in PPM_RX_TYPE"
|
||||
#endif
|
||||
|
||||
/* No noise cancelation */
|
||||
sbi( TCCR1B, ICNC1 );
|
||||
|
||||
/* Set ICP to input, no internal pull up */
|
||||
cbi( PPM_DDR, PPM_PIN);
|
||||
|
||||
/* Enable interrupt on input capture */
|
||||
sbi( TIMSK, TICIE1 );
|
||||
}
|
||||
|
||||
#define PPM_NB_PULSES RADIO_CTL_NB
|
||||
|
||||
extern volatile bool_t ppm_valid;
|
||||
extern pprz_t last_radio[PPM_NB_PULSES];
|
||||
extern bool_t last_radio_contains_avg_channels;
|
||||
|
||||
|
||||
#define MODE_MANUAL 0
|
||||
#define MODE_AUTO 1
|
||||
|
||||
#define MODE_OF_PPRZ(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ? MODE_MANUAL : MODE_AUTO)
|
||||
|
||||
extern void last_radio_from_ppm(void);
|
||||
#endif
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user