From 53d768f32862563d561d054caca2634f9db8b6b9 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Mon, 5 Feb 2007 15:08:24 +0000 Subject: [PATCH] *** empty log message *** --- conf/airframes/antenna.xml | 26 ++++++++++------- conf/autopilot/antenna.h | 8 +++--- sw/airborne/avr/ant_h_bridge.c | 51 ++++++++++++++++++++++++++++++++++ sw/airborne/avr/ant_h_bridge.h | 11 ++++++++ sw/airborne/avr/ant_servo.c | 28 +++++++++++++++++++ sw/airborne/avr/ant_servo.h | 13 +++++++++ sw/airborne/avr/ant_tracker.c | 9 +----- sw/airborne/avr/ant_v2x.c | 3 ++ sw/airborne/main_antenna.c | 38 ++++++++++++++++++++----- 9 files changed, 158 insertions(+), 29 deletions(-) create mode 100644 sw/airborne/avr/ant_h_bridge.c create mode 100644 sw/airborne/avr/ant_h_bridge.h create mode 100644 sw/airborne/avr/ant_servo.c create mode 100644 sw/airborne/avr/ant_servo.h diff --git a/conf/airframes/antenna.xml b/conf/airframes/antenna.xml index 3a6afa2d45..8b1522db1f 100644 --- a/conf/airframes/antenna.xml +++ b/conf/airframes/antenna.xml @@ -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 diff --git a/conf/autopilot/antenna.h b/conf/autopilot/antenna.h index f18fb43d86..0dfb527c26 100644 --- a/conf/autopilot/antenna.h +++ b/conf/autopilot/antenna.h @@ -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 */ diff --git a/sw/airborne/avr/ant_h_bridge.c b/sw/airborne/avr/ant_h_bridge.c new file mode 100644 index 0000000000..9da1305a08 --- /dev/null +++ b/sw/airborne/avr/ant_h_bridge.c @@ -0,0 +1,51 @@ +#include "ant_h_bridge.h" + +#include + +#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; + } +} diff --git a/sw/airborne/avr/ant_h_bridge.h b/sw/airborne/avr/ant_h_bridge.h new file mode 100644 index 0000000000..043767c553 --- /dev/null +++ b/sw/airborne/avr/ant_h_bridge.h @@ -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 */ diff --git a/sw/airborne/avr/ant_servo.c b/sw/airborne/avr/ant_servo.c new file mode 100644 index 0000000000..4a1964b64e --- /dev/null +++ b/sw/airborne/avr/ant_servo.c @@ -0,0 +1,28 @@ +#include "ant_servo.h" + +#include + +#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); +} diff --git a/sw/airborne/avr/ant_servo.h b/sw/airborne/avr/ant_servo.h new file mode 100644 index 0000000000..451efa3462 --- /dev/null +++ b/sw/airborne/avr/ant_servo.h @@ -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 */ diff --git a/sw/airborne/avr/ant_tracker.c b/sw/airborne/avr/ant_tracker.c index 3da636adf7..9c3207ce22 100644 --- a/sw/airborne/avr/ant_tracker.c +++ b/sw/airborne/avr/ant_tracker.c @@ -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; } diff --git a/sw/airborne/avr/ant_v2x.c b/sw/airborne/avr/ant_v2x.c index 188d401919..4dbc56a1ef 100644 --- a/sw/airborne/avr/ant_v2x.c +++ b/sw/airborne/avr/ant_v2x.c @@ -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(); } diff --git a/sw/airborne/main_antenna.c b/sw/airborne/main_antenna.c index bef7aabd29..8f89adab6c 100644 --- a/sw/airborne/main_antenna.c +++ b/sw/airborne/main_antenna.c @@ -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.)