Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-02-05 15:08:24 +00:00
parent 0dd9e2c86c
commit 53d768f328
9 changed files with 158 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/antenna.xml
+16 -10
View File
@@ -27,26 +27,32 @@ ant.srcs = main_antenna.c sys_time.c $(SRC_ARCH)/sys_time_hw.c
ant.CFLAGS += -DLED
ant.CFLAGS += -DDOWNLINK_AP_DEVICE=Uart1 -DAP
ant.CFLAGS += -DGPS -DUBX -DUSE_UART0 -DGPS_LINK=Uart0 -DUART0_BAUD=B9600
ant.srcs += estimator.c gps_ubx.c gps.c
ant.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400
ant.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
ant.srcs += $(SRC_ARCH)/uart_hw.c
ant.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
ant.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
ant.srcs += downlink.c pprz_transport.c
ant.srcs += avr/ant_tracker.c
ant.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
ant.CFLAGS += -DDOWNLINK_AP_DEVICE=Uart0 -DAP
ant.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0
ant.srcs += datalink.c
ant.srcs += traffic_info.c
ant.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B9600
ant.srcs += estimator.c gps_ubx.c gps.c
ant.srcs += avr/ant_tracker.c
#ant.CFLAGS +=
ant.srcs += avr/ant_v2x.c avr/ant_spi.c
ant.srcs += avr/ant_servo.c
ant.srcs += avr/ant_h_bridge.c
ant.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4
ant.srcs += $(SRC_ARCH)/adc_hw.c
conf/autopilot/antenna.h
+4 -4
View File
@@ -4,10 +4,10 @@
#define CLOCK 16
#define LED_1_BANK C
#define LED_1_PIN 0
#define LED_1_BANK D
#define LED_1_PIN 4
#define LED_2_BANK C
#define LED_2_PIN 1
#define LED_2_BANK D
#define LED_2_PIN 5
#endif /* CONFIG_MOTOR_BENCH_H */
sw/airborne/avr/ant_h_bridge.c
+51
View File
@@ -0,0 +1,51 @@
#include "ant_h_bridge.h"
#include <avr/io.h>
#include CONFIG
#define HB_DDR DDRE
#define HB_PORT PORTE
/* OC3A */
#define HB_IN1_PIN 3
/* OC3C */
#define HB_IN2_PIN 5
/* ENABLE */
#define HB_EN_PIN 7
#define HB_REFRESH 500
void ant_h_bridge_init( void ) {
/* set PE1, PE3 and PE5 (OC3A, OC3C and PD0) as output */
SetBit (HB_DDR,HB_IN1_PIN); /* input 1 */
SetBit (HB_DDR,HB_IN2_PIN); /* input 2 */
SetBit (HB_DDR,HB_EN_PIN); /* enable */
/* disable motor */
// ClearBit(HB_PORT, HB_EN_PIN);
SetBit(HB_PORT, HB_EN_PIN);
/* set timer3 in fast PWM mode, with TOP defined by ICR3 , prescaled to 8 */
TCCR3A = _BV(WGM31) | _BV(COM3A1) | _BV(COM3C1);
TCCR3B = _BV(WGM32) | _BV(WGM33) | _BV(CS31);
ICR3 = HB_REFRESH;
ant_h_bridge_set(0);
// ant_h_bridge_set(HB_REFRESH/5);
// SetBit(HB_PORT, HB_IN1_PIN);
// ClearBit(HB_PORT, HB_IN2_PIN);
}
void ant_h_bridge_set ( int16_t value) {
if (value > 0) {
OCR3A = value;
OCR3C = 0;
}
else {
OCR3A = 0;
OCR3C = -value;
}
}
sw/airborne/avr/ant_h_bridge.h
+11
View File
@@ -0,0 +1,11 @@
#ifndef ANT_H_BRIDGE_H
#define ANT_H_BRIDGE_H
#include "std.h"
void ant_h_bridge_init( void );
void ant_h_bridge_set ( int16_t value);
#endif /* ANT_H_BRIDGE_H */
sw/airborne/avr/ant_servo.c
+28
View File
@@ -0,0 +1,28 @@
#include "ant_servo.h"
#include <avr/io.h>
#include CONFIG
#define PRESCALER 8
#define TICKS_OF_US(nb_us) (((nb_us) * (float)(CLOCK / PRESCALER)-1))
#define SERVO_REFRESH 25000
void ant_servo_init( void ) {
/* set PB5 and PB6 (OC1A and OC1B ) as output */
SetBit (DDRB, 5);
SetBit (DDRB, 6);
/* set timer1 in fast PWM mode, with TOP defined by ICR3 , prescaled to 8 */
TCCR1A = _BV(WGM11) | _BV(COM1A1) | _BV(COM1B1);
TCCR1B = _BV(WGM12) | _BV(WGM13) | _BV(CS11);
ICR1=TICKS_OF_US(SERVO_REFRESH);
ant_servo_set(NEUTRAL_SERVO, NEUTRAL_SERVO);
}
void ant_servo_set ( uint16_t value1_us, uint16_t value2_us) {
/* code pour regler la valeur en ms a l'etat haut du signal PWM */
Bound(value1_us, MIN_SERVO, MAX_SERVO);
OCR1A = TICKS_OF_US(value1_us);
Bound(value2_us, MIN_SERVO, MAX_SERVO);
OCR1B = TICKS_OF_US(value2_us);
}
sw/airborne/avr/ant_servo.h
+13
View File
@@ -0,0 +1,13 @@
#ifndef ANT_SERVO_H
#define ANT_SERVO_H
#include "std.h"
#define MAX_SERVO 2000
#define NEUTRAL_SERVO 1500
#define MIN_SERVO 1000
void ant_servo_init( void );
void ant_servo_set ( uint16_t value1_us, uint16_t value2_us);
#endif /* ANT_SERVO_H */
sw/airborne/avr/ant_tracker.c
+1 -8
View File
@@ -24,20 +24,13 @@ void ant_tracker_init( void ) {
void ant_tracker_periodic( void ) {
if (ant_track_mode == ANT_TRACK_AUTO) {
#if 0
ant_track_azim += 0.5;
if (ant_track_azim > 360.)
ant_track_azim = 0.;
ant_track_elev += 0.1;
if (ant_track_elev > 90.)
ant_track_elev = 0.;
#endif
struct ac_info_ * ac = get_ac_info(ant_track_id);
ant_track_azim = atan2(ac->north, ac->east) * 180. / M_PI;
ant_track_azim = 90. - ant_track_azim;
if (ant_track_azim < 0)
ant_track_azim += 360.;
float dist = sqrt(ac->north*ac->north + ac->east*ac->east);
if ( dist < 1.) dist = 1.;
float height = ac->alt - ant_track_elev;
ant_track_elev = atan2( height, dist) * 180. / M_PI;
}
sw/airborne/avr/ant_v2x.c
+3
View File
@@ -92,6 +92,9 @@ void ant_v2x_init( void )
ant_v2x_data_available = FALSE;
ant_v2x_com_status = MAG_CS_IDLE;
ant_v2x_data.heading = 0.;
ant_v2x_reset();
}
sw/airborne/main_antenna.c
+31 -7
View File
@@ -12,12 +12,20 @@
#include "adc.h"
#include "ant_v2x.h"
#include "ant_servo.h"
#include "ant_h_bridge.h"
#include "messages.h"
#include "downlink.h"
#include "ant_tracker.h"
#include "settings.h"
#include "traffic_info.h"
#define NAV_UTM_EAST0 360285
#define NAV_UTM_NORTH0 4813595
#define NAV_UTM_ZONE0 31
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void);
@@ -40,17 +48,25 @@ int main( void ) {
static inline void main_init( void ) {
hw_init();
sys_time_init();
led_init();
// led_init();
uart1_init_tx();
uart1_init_rx();
adc_init();
uart0_init_tx();
uart0_init_rx();
// adc_init();
ant_servo_init();
ant_h_bridge_init();
ant_v2x_init();
ant_tracker_init();
SetAcInfo(5, 1, 1, 0., 300., 0.);
int_enable();
}
static inline void main_event_task( void ) {
if (ant_v2x_data_available) {
LED_TOGGLE(1);
LED_TOGGLE(2);
ant_v2x_read_data();
DOWNLINK_SEND_ANTENNA_DEBUG(&ant_v2x_data.xraw, &ant_v2x_data.yraw, \
&ant_v2x_data.xcal, &ant_v2x_data.ycal, \
@@ -74,22 +90,30 @@ static inline void main_event_task( void ) {
}
static inline void main_periodic_task( void ) {
ant_v2x_periodic();
static uint8_t cnt;
cnt++;
if (!(cnt%16)) {
// LED_TOGGLE(1);
// LED_TOGGLE(2);
DOWNLINK_SEND_ANTENNA_STATUS(&ant_track_azim, &ant_track_elev, &ant_track_id, &ant_track_mode);
}
if (!(cnt%4)) {
ant_tracker_periodic();
ant_servo_set(1100 + ant_track_elev * 9.2, NEUTRAL_SERVO);
float err_heading = ant_v2x_data.heading - ant_track_azim;
while (err_heading > 180.) {err_heading -= 360.;}
while (err_heading < -180.) {err_heading += 360.;}
if (abs(err_heading) < 10.) err_heading = 0.;
float heading_pgain = -15.;
int16_t mot_cmd = heading_pgain * err_heading;
Bound(mot_cmd, -300, 300);
if (abs(mot_cmd) < 75) mot_cmd = 0;
ant_h_bridge_set(mot_cmd);
}
}
#include "settings.h"
#include "traffic_info.h"
#define NAV_UTM_EAST0 360285
#define NAV_UTM_NORTH0 4813595
#define NAV_UTM_ZONE0 31
#define IdOfMsg(x) (x[1])
#define MOfCm(_x) (((float)_x)/100.)