Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 04:13:39 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-18 18:49:14 +00:00
parent 5055737cd1
commit d9983ad07d
10 changed files with 453 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/pt_ant.xml
+62
View File
@@ -0,0 +1,62 @@
<airframe name="pt_ant">
<section name="DATALINK" prefix="DATALINK_">
<define name="DEVICE_TYPE" value="PPRZ"/>
<define name="DEVICE_ADDRESS" value="...."/>
</section>
<makefile>
# 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
</makefile>
</airframe>
conf/settings/pt_ant_settings.xml
+11
View File
@@ -0,0 +1,11 @@
<!DOCTYPE settings SYSTEM "settings.dtd">
<!-- -->
<settings>
<dl_settings>
<!-- <dl_setting var="pt_ant_mode" min="0" step="1" max="3" handler="SetMode"/> -->
<dl_setting var="pt_ant_motors_y_power" min="-1" step="0.1" max="1" module="pt_ant_motors" handler="SetYPower"/>
<dl_setting var="pt_ant_motors_z_power" min="-1" step="0.1" max="1" module="pt_ant_motors" handler="SetZPower"/>
</dl_settings>
</settings>
sw/airborne/main_pan_tilt_antenna.c
+73
View File
@@ -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
}
sw/airborne/pt_ant_datalink.c
+43
View File
@@ -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;
}
}
}
sw/airborne/pt_ant_estimator.c
+23
View File
@@ -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) {
}
sw/airborne/pt_ant_estimator.h
+32
View File
@@ -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 */
sw/airborne/pt_ant_motors.c
+25
View File
@@ -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.);
}
sw/airborne/pt_ant_motors.h
+64
View File
@@ -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 */
sw/airborne/pt_ant_sensors.c
+78
View File
@@ -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<<PT_ANT_SENSOR_SS_PIN;
/* unselect sensor */
PtAntSensorsUnselect();
/* configure SPI : 8 bits CPOL=0 CPHA=0 MSB_first master */
S0SPCR = 1<<5;
/* setup SPI clock rate : PCLK / 32 */
S0SPCCR = 0x20;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT(VIC_SPI0); // SPI0 selected as IRQ
VICIntEnable = VIC_BIT(VIC_SPI0); // SPI0 interrupt enabled
VICVectCntl10 = VIC_ENABLE | VIC_SPI0;
VICVectAddr10 = (uint32_t)SPI0_ISR; // address of the ISR
/* clear pending interrupt */
SetBit(S0SPINT, SPI0IF);
/* enable SPI interrupt */
SetBit(S0SPCR, S0SPCR_SPIE);
}
void SPI0_ISR(void) {
ISR_ENTRY();
/* FIXME : do something usefull with the status register reading */ \
uint8_t foo __attribute__((unused)) = S0SPSR; \
pt_ant_sensors_buffer[pt_ant_sensors_buffer_idx] = S0SPDR;
pt_ant_sensors_buffer_idx++;
if (pt_ant_sensors_buffer_idx < sizeof(pt_ant_sensors_data)) {
S0SPDR = 0xAA;
}
else {
PtAntSensorsUnselect();
pt_ant_sensors_data_available = TRUE;
}
/* clear the interrupt */
S0SPINT = _BV(SPI0IF);
/* clear this interrupt from the VIC */
VICVectAddr = 0x00000000;
ISR_EXIT();
}
sw/airborne/pt_ant_sensors.h
+42
View File
@@ -0,0 +1,42 @@
#ifndef PT_ANT_SENSORS_H
#define PT_ANT_SENSORS_H
#include "std.h"
#if 0
struct PtAntSensorData {
uint16_t accel_x;
uint16_t accel_y;
uint16_t accel_z;
uint16_t mag_x;
uint16_t mag_y;
uint16_t mag_z;
uint16_t checksum;
};
#else
struct PtAntSensorData {
uint8_t accel_x;
uint8_t accel_y;
uint8_t accel_z;
};
#endif
extern volatile uint8_t pt_ant_sensors_data_available;
extern struct PtAntSensorData pt_ant_sensors_data;
extern void pt_ant_sensors_init(void);
extern void pt_ant_sensors_read(void);
extern void pt_ant_sensors_init_spi(void);
#define PtAntSensorsEventCheckAndHandle() { \
if (pt_ant_sensors_data_available) { \
pt_ant_sensors_data_available = FALSE; \
DOWNLINK_SEND_ANTENNA_DEBUG(&pt_ant_sensors_data.accel_x, &pt_ant_sensors_data.accel_y, &pt_ant_sensors_data.accel_z); \
} \
}
#define PtAntSensorsPeriodic() { \
pt_ant_sensors_read(); \
}
#endif /* PT_ANT_SENSORS_H */