diff --git a/conf/airframes/pt_ant.xml b/conf/airframes/pt_ant.xml new file mode 100644 index 0000000000..9d22ba6230 --- /dev/null +++ b/conf/airframes/pt_ant.xml @@ -0,0 +1,62 @@ + + +
+ + +
+ + + + +# a test program to tunnel between both uart +tunnel.ARCHDIR = $(ARCHI) +tunnel.ARCH = arm7tdmi +tunnel.TARGET = tunnel +tunnel.TARGETDIR = tunnel + +tunnel.CFLAGS += -DFBW -DCONFIG=\"conf_demo.h\" -DLED -DTIME_LED=1 +tunnel.srcs += $(SRC_ARCH)/uart_tunnel.c + +ARCHI=arm7 + +FLASH_MODE = IAP + +main.ARCHDIR = $(ARCHI) +main.ARCH = arm7tdmi +main.TARGET = main +main.TARGETDIR = main + +main.CFLAGS += -DCONFIG=\"conf_demo.h\" +main.srcs = main_pan_tilt_antenna.c + +main.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(10e-2)' +main.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c + +main.CFLAGS += -DLED + +main.srcs += $(SRC_ARCH)/armVIC.c + +main.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 +main.srcs += $(SRC_ARCH)/uart_hw.c + +main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 +main.srcs += downlink.c pprz_transport.c + +main.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0 +main.srcs += pt_ant_datalink.c + +main.srcs += pt_ant_motors.c + +main.srcs += pt_ant_sensors.c + +main.srcs += pt_ant_estimator.c + +main.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B9600 +main.CFLAGS += -DGPS -DUBX -DGPS_LINK=Uart1 +main.CFLAGS += -DGPS_USER_CALLBACK=pt_ant_estimator_update_self_gps -DGPS_USER_CALLBACK_HEADER=\"pt_ant_estimator.h\" +main.CFLAGS += -DGPS_CONFIGURE -DGPS_BAUD=38400 +main.srcs += gps_ubx.c gps.c + + + + diff --git a/conf/settings/pt_ant_settings.xml b/conf/settings/pt_ant_settings.xml new file mode 100644 index 0000000000..5c6f8754a1 --- /dev/null +++ b/conf/settings/pt_ant_settings.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/sw/airborne/main_pan_tilt_antenna.c b/sw/airborne/main_pan_tilt_antenna.c new file mode 100644 index 0000000000..927f01ad26 --- /dev/null +++ b/sw/airborne/main_pan_tilt_antenna.c @@ -0,0 +1,73 @@ +#include "std.h" +#include "init_hw.h" +#include "sys_time.h" +#include "led.h" +#include "interrupt_hw.h" +#include "uart.h" + +#include "messages.h" +#include "downlink.h" + +#include "pt_ant_motors.h" +#include "pt_ant_sensors.h" +#include "pt_ant_estimator.h" +#include "gps.h" + +#include "pt_ant_datalink.h" +//#include "traffic_info.h" + +static inline void main_init( void ); +static inline void main_periodic_task( void ); +static inline void main_event_task( void ); + +int main( void ) { + main_init(); + while(1) { + if (sys_time_periodic()) + main_periodic_task(); + main_event_task(); + } + return 0; +} + +static inline void main_init( void ) { + hw_init(); + sys_time_init(); + led_init(); + Uart0Init(); + Uart1Init(); + gps_init(); + pt_ant_motors_init(); + pt_ant_sensors_init_spi(); + pt_ant_sensors_init(); + + int_enable(); + + gps_configure_uart(); + +} + +static inline void main_periodic_task( void ) { + // PtAntSensorsPeriodic(); + +#ifdef USE_GPS + GpsPeriodic(); +#endif + + // LED_TOGGLE(1); + // if (cpu_time_sec > 10) + // pt_ant_motors_SetZPower(0.2); + +} + +static inline void main_event_task( void ) { + + // PtAntSensorsEventCheckAndHandle(); + + // DlEventCheckAndHandle(); + +#ifdef USE_GPS + if (GpsEventCheckAndHandle()) + return; +#endif +} diff --git a/sw/airborne/pt_ant_datalink.c b/sw/airborne/pt_ant_datalink.c new file mode 100644 index 0000000000..5d859e74e0 --- /dev/null +++ b/sw/airborne/pt_ant_datalink.c @@ -0,0 +1,43 @@ +#include "pt_ant_datalink.h" + +bool_t pt_ant_dl_msg_available; +uint8_t pt_ant_dl_buffer[PT_ANT_MSG_SIZE] __attribute__ ((aligned)); +#include "settings.h" +#include "downlink.h" +#include "messages.h" +#include "dl_protocol.h" +#include "uart.h" + +#include "traffic_info.h" +#include "pt_ant_estimator.h" + +#define IdOfMsg(x) (x[1]) +#define MOfCm(_x) (((float)_x)/100.) + +void pt_ant_dl_parse_msg(void) { + uint8_t msg_id = IdOfMsg(pt_ant_dl_buffer); + switch (msg_id) { + + case DL_SETTING : { + uint8_t i = DL_SETTING_index(pt_ant_dl_buffer); + float var = DL_SETTING_value(pt_ant_dl_buffer); + DlSetting(i, var); + DOWNLINK_SEND_DL_VALUE(&i, &var); + break; + } + + case DL_ACINFO: { + LED_TOGGLE(1); + uint8_t id = DL_ACINFO_ac_id(pt_ant_dl_buffer); + struct ac_info_ ac; + ac.east = MOfCm(DL_ACINFO_utm_east(pt_ant_dl_buffer)); + ac.north = MOfCm(DL_ACINFO_utm_north(pt_ant_dl_buffer)); + ac.course = MOfCm(DL_ACINFO_alt(pt_ant_dl_buffer)); + ac.alt = RadOfDeg(((float)DL_ACINFO_course(pt_ant_dl_buffer))/ 10.); + ac.gspeed = MOfCm(DL_ACINFO_speed(pt_ant_dl_buffer)); + pt_ant_estimator_update_target(id, &ac); + break; + } + + } +} diff --git a/sw/airborne/pt_ant_estimator.c b/sw/airborne/pt_ant_estimator.c new file mode 100644 index 0000000000..034bd64f7b --- /dev/null +++ b/sw/airborne/pt_ant_estimator.c @@ -0,0 +1,23 @@ +#include "pt_ant_estimator.h" + +struct PtAntEstimatorState pt_ant_estimator_state; + +void pt_ant_estimator_update_target(uint8_t id, struct ac_info_ *ac) { + pt_ant_estimator_state.target_east = ac->east; + pt_ant_estimator_state.target_north = ac->north; + pt_ant_estimator_state.target_alt = ac->alt; +} + +void pt_ant_estimator_update_self_attitude(struct PtAntSensorData* data) { + pt_ant_estimator_state.self_phi = atan2(data->accel_y, data->accel_z); + float g2 = data->accel_x * data->accel_x + + data->accel_y * data->accel_y + + data->accel_z * data->accel_z; + pt_ant_estimator_state.self_theta = -asin(data->accel_x / sqrt(g2)); + +} + +void pt_ant_estimator_update_self_gps(void) { + + +} diff --git a/sw/airborne/pt_ant_estimator.h b/sw/airborne/pt_ant_estimator.h new file mode 100644 index 0000000000..9858c5f7c7 --- /dev/null +++ b/sw/airborne/pt_ant_estimator.h @@ -0,0 +1,32 @@ +#ifndef PT_ANT_ESTIMATOR_H +#define PT_ANT_ESTIMATOR_H + +#include "std.h" +#include "traffic_info.h" +#include "pt_ant_sensors.h" + + +struct PtAntEstimatorState { + + float self_phi; + float self_theta; + float self_psi; + + float self_east; + float self_north; + float self_alt; + + float target_east; + float target_north; + float target_alt; + +}; + +extern struct PtAntEstimatorState pt_ant_estimator_state; + +extern void pt_ant_estimator_update_target(uint8_t id, struct ac_info_ *ac); +extern void pt_ant_estimator_update_self_gps(void); +extern void pt_ant_estimator_update_self_attitude(struct PtAntSensorData* data); + + +#endif /* PT_ANT_ESTIMATOR_H */ diff --git a/sw/airborne/pt_ant_motors.c b/sw/airborne/pt_ant_motors.c new file mode 100644 index 0000000000..65b7d59157 --- /dev/null +++ b/sw/airborne/pt_ant_motors.c @@ -0,0 +1,25 @@ +#include "pt_ant_motors.h" + +float pt_ant_motors_y_power; +float pt_ant_motors_z_power; + +void pt_ant_motors_init(void) { + /* configure direction pin as output */ + MOT_Y_DIR_DIR_REG |= 1 << MOT_Y_DIR_PIN; + MOT_Z_DIR_DIR_REG |= 1 << MOT_Z_DIR_PIN; + + /* configure pins for PWM */ + MOT_Y_PWM_PINSEL |= MOT_Y_PWM_PINSEL_VAL << MOT_Y_PWM_PINSEL_BIT; + MOT_Z_PWM_PINSEL |= MOT_Z_PWM_PINSEL_VAL << MOT_Z_PWM_PINSEL_BIT; + /* set chopping frequency */ + PWMMR0 = MOT_CHOP_PERIOD; + /* enable PWM outputs in single edge mode*/ + PWMPCR = MOT_Y_PWM_ENA | MOT_Z_PWM_ENA; + /* commit PWMMRx changes */ + PWMLER = PWMLER_LATCH0; + /* enable PWM timer in PWM mode */ + PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE; + + pt_ant_motors_SetYPower(0.); + pt_ant_motors_SetZPower(0.); +} diff --git a/sw/airborne/pt_ant_motors.h b/sw/airborne/pt_ant_motors.h new file mode 100644 index 0000000000..b63100d355 --- /dev/null +++ b/sw/airborne/pt_ant_motors.h @@ -0,0 +1,64 @@ +#ifndef PT_ANT_MOTORS_H +#define PT_ANT_MOTORS_H + +#include "std.h" +#include "LPC21xx.h" +#include "sys_time.h" + +extern void pt_ant_motors_init(void); + +extern float pt_ant_motors_y_power; +extern float pt_ant_motors_z_power; + +#define pt_ant_motors_SetYPower(p) { \ + uint32_t len = p * MOT_CHOP_PERIOD; \ + if (p>=0) \ + SetBit(MOT_Y_DIR_CLR_REG, MOT_Y_DIR_PIN); \ + else { \ + SetBit(MOT_Y_DIR_SET_REG, MOT_Y_DIR_PIN); \ + len = -len; \ + } \ + MOT_Y_PWM_REG = len; \ + PWMLER = MOT_Y_PWM_LATCH; \ + } + + +#define pt_ant_motors_SetZPower(p) { \ + uint32_t len = p * MOT_CHOP_PERIOD; \ + if (p>=0) \ + SetBit(MOT_Z_DIR_CLR_REG, MOT_Z_DIR_PIN); \ + else { \ + SetBit(MOT_Z_DIR_SET_REG, MOT_Z_DIR_PIN); \ + len = -len; \ + } \ + MOT_Z_PWM_REG = len; \ + PWMLER = MOT_Z_PWM_LATCH; \ + } + +#define MOT_Z_PWM_PINSEL PINSEL0 +#define MOT_Z_PWM_PINSEL_VAL 2 +#define MOT_Z_PWM_PINSEL_BIT 14 +#define MOT_Z_PWM_ENA PWMPCR_ENA2 +#define MOT_Z_PWM_LATCH PWMLER_LATCH2 +#define MOT_Z_PWM_REG PWMMR2 +#define MOT_Z_DIR_DIR_REG IO0DIR +#define MOT_Z_DIR_CLR_REG IO0CLR +#define MOT_Z_DIR_SET_REG IO0SET +#define MOT_Z_DIR_PIN 15 + + +#define MOT_Y_PWM_PINSEL PINSEL1 +#define MOT_Y_PWM_PINSEL_VAL 1 +#define MOT_Y_PWM_PINSEL_BIT 10 +#define MOT_Y_PWM_ENA PWMPCR_ENA5 +#define MOT_Y_PWM_LATCH PWMLER_LATCH5 +#define MOT_Y_PWM_REG PWMMR5 +#define MOT_Y_DIR_DIR_REG IO0DIR +#define MOT_Y_DIR_CLR_REG IO0CLR +#define MOT_Y_DIR_SET_REG IO0SET +#define MOT_Y_DIR_PIN 19 + +/* 1KHz */ +#define MOT_CHOP_PERIOD SYS_TICS_OF_USEC(1000) + +#endif /* PT_ANT_MOTORS_H */ diff --git a/sw/airborne/pt_ant_sensors.c b/sw/airborne/pt_ant_sensors.c new file mode 100644 index 0000000000..3bc090343f --- /dev/null +++ b/sw/airborne/pt_ant_sensors.c @@ -0,0 +1,78 @@ +#include "pt_ant_sensors.h" + +#include "LPC21xx.h" +#include "armVIC.h" + +void SPI0_ISR(void) __attribute__((naked)); +#define S0SPCR_SPIE 7 /* SPI enable */ +#define PT_ANT_SENSOR_SS_PIN 3 +#define PtAntSensorsSelect() { SetBit(IO0SET, PT_ANT_SENSOR_SS_PIN);} +#define PtAntSensorsUnselect() { SetBit(IO0CLR, PT_ANT_SENSOR_SS_PIN);} + +volatile uint8_t pt_ant_sensors_data_available; +struct PtAntSensorData pt_ant_sensors_data; + +volatile uint8_t pt_ant_sensors_buffer_idx; +uint8_t* pt_ant_sensors_buffer = (uint8_t*)&pt_ant_sensors_data; + +void pt_ant_sensors_init(void) { + pt_ant_sensors_data_available = FALSE; + +} + + +void pt_ant_sensors_read(void) { + PtAntSensorsSelect(); + pt_ant_sensors_buffer_idx = 0; + S0SPDR = 0xAA; +} + + + + +void pt_ant_sensors_init_spi(void) { + /* init SPI0 */ + /* setup pins function for sck, miso, mosi */ + PINSEL0 |= 1<<8 | 1<<10| 1<<12; + /* setup ssel as output */ + IO0DIR |= 1<