mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -0,0 +1,44 @@
|
||||
<airframe name="ANTENNA">
|
||||
|
||||
<makefile>
|
||||
|
||||
ARCHI=avr
|
||||
ant.ARCHDIR = $(ARCHI)
|
||||
ant.ARCH = atmega128
|
||||
ant.TARGET = ant
|
||||
ant.TARGETDIR = ant
|
||||
|
||||
|
||||
# 16MHz resonator
|
||||
ant.LOW_FUSE = CF
|
||||
ant.HIGH_FUSE = 89
|
||||
ant.EXT_FUSE = ff
|
||||
ant.LOCK_FUSE = ff
|
||||
|
||||
|
||||
ant.CFLAGS += -DCONFIG=\"antenna.h\"
|
||||
ant.srcs = main_antenna.c sys_time.c $(SRC_ARCH)/sys_time_hw.c
|
||||
|
||||
ant.CFLAGS += -DLED
|
||||
|
||||
ant.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400
|
||||
ant.srcs += $(SRC_ARCH)/uart_hw.c
|
||||
|
||||
ant.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
|
||||
ant.srcs += downlink.c pprz_transport.c
|
||||
|
||||
ant.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
|
||||
ant.srcs += datalink.c
|
||||
|
||||
#ant.CFLAGS +=
|
||||
ant.srcs += avr/ant_v2x.c avr/ant_spi.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
|
||||
|
||||
|
||||
</makefile>
|
||||
|
||||
</airframe>
|
||||
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
<airframe name="IMU">
|
||||
<airframe name="MOTOR_BENCH">
|
||||
|
||||
<makefile>
|
||||
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
#ifndef CONFIG_ANTENNA_H
|
||||
#define CONFIG_ANTENNA_H
|
||||
|
||||
|
||||
#define CLOCK 16
|
||||
|
||||
#define LED_1_BANK C
|
||||
#define LED_1_PIN 0
|
||||
|
||||
#define LED_2_BANK C
|
||||
#define LED_2_PIN 1
|
||||
|
||||
#endif /* CONFIG_MOTOR_BENCH_H */
|
||||
@@ -344,6 +344,17 @@
|
||||
<message name="AHRS_OVERRUN" ID="207">
|
||||
</message>
|
||||
|
||||
<message name="ANTENNA_DEBUG" ID="220">
|
||||
<field name="mag_xraw" type="int32"/>
|
||||
<field name="mag_yraw" type="int32"/>
|
||||
<field name="mag_xcal" type="float"/>
|
||||
<field name="mag_ycal" type="float"/>
|
||||
<field name="mag_heading" type="float"/>
|
||||
<field name="mag_magnitude" type="float"/>
|
||||
<field name="mag_temp" type="float"/>
|
||||
<field name="mag_distor" type="uint8"/>
|
||||
<field name="mag_cal_status" type="uint8"/>
|
||||
</message>
|
||||
|
||||
</class>
|
||||
|
||||
|
||||
@@ -0,0 +1,71 @@
|
||||
#include "ant_spi.h"
|
||||
|
||||
#include <avr/io.h>
|
||||
#include "std.h"
|
||||
|
||||
|
||||
/*********************************Flush SPI*****************************************/
|
||||
|
||||
|
||||
void flush_SPI( void )
|
||||
{
|
||||
if (bit_is_set(SPSR, SPIF))
|
||||
{
|
||||
uint8_t foo __attribute__ ((unused)) = SPDR;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*****************************Initialisation SPI************************************/
|
||||
|
||||
#define DDR_SPI DDRB
|
||||
#define PORT_SPI PORTB
|
||||
#define PIN_SS 0
|
||||
#define PIN_SCK 1
|
||||
#define PIN_MOSI 2
|
||||
#define PIN_SYNC 4
|
||||
|
||||
void SPI_select_slave ( void )
|
||||
{
|
||||
ClearBit(PORT_SPI, PIN_SS);
|
||||
}
|
||||
|
||||
/***********************************************************************************/
|
||||
|
||||
void SPI_unselect_slave ( void )
|
||||
{
|
||||
SetBit(PORT_SPI, PIN_SS);
|
||||
}
|
||||
|
||||
/***********************************************************************************/
|
||||
|
||||
void SPI_master_init( void )
|
||||
{
|
||||
/* Set SS, MOSI and SCK output, all others input */
|
||||
DDR_SPI |= _BV(PIN_SS) | _BV(PIN_SCK) | _BV(PIN_MOSI);
|
||||
/* unselect slave */
|
||||
SPI_unselect_slave();
|
||||
/* 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));
|
||||
}
|
||||
|
||||
/***********************************************************************************/
|
||||
|
||||
void SPI_start( void )
|
||||
{
|
||||
SPI_select_slave();
|
||||
if (bit_is_set(SPSR, SPIF)) {
|
||||
uint8_t foo __attribute__ ((unused)) = SPDR;
|
||||
}
|
||||
/* enable interrupt */
|
||||
SetBit(SPCR,SPIE);
|
||||
}
|
||||
|
||||
/***********************************************************************************/
|
||||
|
||||
void SPI_stop(void)
|
||||
{
|
||||
SPI_unselect_slave ();
|
||||
/* disable interrupt */
|
||||
ClearBit(SPCR,SPIE);
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
#ifndef ANT_SPI_H
|
||||
#define ANT_SPI_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#include <avr/io.h>
|
||||
#include "std.h"
|
||||
|
||||
void flush_SPI( void );
|
||||
void SPI_select_slave ( void );
|
||||
void SPI_unselect_slave ( void );
|
||||
void SPI_master_init( void );
|
||||
void SPI_start( void );
|
||||
void SPI_stop( void );
|
||||
|
||||
#define SPI_transmit(c) { SPDR = c; }
|
||||
#define SPI_read() (SPDR)
|
||||
|
||||
#endif /* ANT_SPI_H */
|
||||
@@ -0,0 +1,255 @@
|
||||
#include "ant_v2x.h"
|
||||
|
||||
#include <avr/io.h>
|
||||
#include <avr/interrupt.h>
|
||||
|
||||
//#include "stdlib.h"
|
||||
#include "string.h"
|
||||
#include "std.h"
|
||||
//#include "systime.h"
|
||||
//#include "signalisation.h"
|
||||
//#include "utils.h"
|
||||
|
||||
#include "ant_spi.h"
|
||||
|
||||
//#include "downlink.h"
|
||||
|
||||
volatile bool_t ant_v2x_data_available;
|
||||
struct Ant_V2xData ant_v2x_data;
|
||||
|
||||
|
||||
#define MAG_S_RESET 0
|
||||
#define MAG_S_UNINIT 1
|
||||
#define MAG_S_READY 2
|
||||
#define MAG_S_WAIT_MEAS 3
|
||||
volatile uint8_t ant_v2x_status;
|
||||
|
||||
#define MAG_CS_IDLE 0
|
||||
#define MAG_CS_READING 1
|
||||
#define MAG_CS_WRITING 2
|
||||
volatile uint8_t ant_v2x_com_status;
|
||||
|
||||
|
||||
uint8_t ant_v2x_req[64];
|
||||
uint8_t ant_v2x_req_len;
|
||||
volatile uint8_t ant_v2x_req_idx;
|
||||
|
||||
uint8_t ant_v2x_res[128];
|
||||
uint8_t ant_v2x_res_len;
|
||||
volatile uint8_t ant_v2x_res_idx;
|
||||
|
||||
volatile uint8_t ant_v2x_periodic_count;
|
||||
|
||||
/******************************Ant_V2x define************************************/
|
||||
|
||||
#define ANT_V2X_DDR DDRB
|
||||
#define ANT_V2X_PORT PORTB
|
||||
#define ANT_V2X_PIN 4
|
||||
|
||||
#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 <B0> to 359.9 <B0>
|
||||
#define DATA_MAGNITUDE 0x06 // Float32 scalebreak;
|
||||
#define DATA_TEMPERATURE 0x07 // Float32 <B0> Celsius
|
||||
#define DATA_DISTORTION 0x08 // Boolean
|
||||
#define DATA_CAL_STATUS 0x09 // Boolean
|
||||
|
||||
|
||||
extern void ant_v2x_periodic_initialise(void);
|
||||
extern void ant_v2x_send_req ( const uint8_t* req, uint8_t len);
|
||||
extern void ant_v2x_send_get_data ( void );
|
||||
|
||||
|
||||
/***************************Initialisation ant_v2x*******************************/
|
||||
|
||||
void ant_v2x_init( void )
|
||||
{
|
||||
SPI_master_init();
|
||||
/* set sync as ouptut */
|
||||
SetBit(ANT_V2X_DDR, ANT_V2X_PIN);
|
||||
/* pull it down */
|
||||
SetBit(ANT_V2X_PORT, ANT_V2X_PIN);
|
||||
|
||||
ant_v2x_data_available = FALSE;
|
||||
ant_v2x_com_status = MAG_CS_IDLE;
|
||||
ant_v2x_reset();
|
||||
}
|
||||
|
||||
|
||||
void ant_v2x_periodic_initialise( void ) {
|
||||
static uint8_t init_status = 0;
|
||||
if (ant_v2x_com_status != MAG_CS_IDLE)
|
||||
return;
|
||||
switch (init_status) {
|
||||
case 0 : /* set data response format */
|
||||
{
|
||||
const uint8_t req[] = {SET_DATA_COMPONENTS, 0x08, DATA_XRAW, DATA_YRAW, DATA_XCAL, DATA_YCAL, DATA_HEADING, DATA_MAGNITUDE, DATA_DISTORTION, DATA_CAL_STATUS };
|
||||
ant_v2x_send_req(req, sizeof(req));
|
||||
}
|
||||
break;
|
||||
case 1 : /* set little endian */
|
||||
{
|
||||
const uint8_t req[] = {SET_CONFIG, 0x06, 0x00};
|
||||
ant_v2x_send_req(req, sizeof(req));
|
||||
}
|
||||
break;
|
||||
case 2 :
|
||||
{ /* set period */
|
||||
const uint8_t req[] = {SET_CONFIG, 0x05, 0x07};
|
||||
ant_v2x_send_req(req, sizeof(req));
|
||||
}
|
||||
break;
|
||||
default:
|
||||
ant_v2x_status = MAG_S_READY;
|
||||
}
|
||||
init_status++;
|
||||
}
|
||||
void ant_v2x_periodic( void ) { /* Run initialisation and communication request */
|
||||
switch (ant_v2x_status) {
|
||||
case MAG_S_RESET:
|
||||
SetBit(ANT_V2X_PORT, ANT_V2X_PIN);
|
||||
ant_v2x_status = MAG_S_UNINIT;
|
||||
break;
|
||||
case MAG_S_UNINIT:
|
||||
ant_v2x_periodic_initialise();
|
||||
break;
|
||||
case MAG_S_READY: /* Ready to receive request */
|
||||
/*GREEN_LED_ON();*/
|
||||
/*YELLOW_LED_OFF();*/
|
||||
// if (ant_v2x_data_available) return
|
||||
ant_v2x_send_get_data();
|
||||
ant_v2x_status = MAG_S_WAIT_MEAS;
|
||||
ant_v2x_periodic_count = 0;
|
||||
break;
|
||||
case MAG_S_WAIT_MEAS: { /* Waiting for measures */
|
||||
ant_v2x_periodic_count++;
|
||||
if (ant_v2x_periodic_count > 5) {
|
||||
ant_v2x_com_status = MAG_CS_READING;
|
||||
SPI_start();
|
||||
ant_v2x_res_idx = 0;
|
||||
ant_v2x_res_len = 43;
|
||||
SPI_transmit(0x00);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************Ant_V2x reset****************************************/
|
||||
void ant_v2x_reset (void){
|
||||
ClearBit(ANT_V2X_PORT, ANT_V2X_PIN);
|
||||
ant_v2x_status = MAG_S_RESET;
|
||||
ant_v2x_com_status = MAG_CS_IDLE;
|
||||
}
|
||||
|
||||
/**************************Ant_V2x data communication******************************/
|
||||
void ant_v2x_send_get_data ( void ) {
|
||||
const uint8_t req[] = {GET_DATA};
|
||||
ant_v2x_send_req(req, sizeof(req));
|
||||
}
|
||||
|
||||
/********************************Request procedure***********************************/
|
||||
void ant_v2x_send_req(const uint8_t* req, uint8_t len) {
|
||||
memcpy(ant_v2x_req, req, len);
|
||||
ant_v2x_req_len = len;
|
||||
ant_v2x_req_idx = 0;
|
||||
ant_v2x_com_status = MAG_CS_WRITING;
|
||||
SPI_start();
|
||||
SPI_transmit(SYNC_FLAG);/* transmit SYNC_FLAG first for every beginning of transmition */
|
||||
}
|
||||
|
||||
void ant_v2x_read_data( void ) {
|
||||
ant_v2x_data.xraw = *(int32_t*)&ant_v2x_res[4];
|
||||
ant_v2x_data.yraw = *(int32_t*)&ant_v2x_res[9];
|
||||
ant_v2x_data.xcal = *(float*)&ant_v2x_res[14];
|
||||
ant_v2x_data.ycal = *(float*)&ant_v2x_res[19];
|
||||
ant_v2x_data.heading = *(float*)&ant_v2x_res[24];
|
||||
ant_v2x_data.magnitude = *(float*)&ant_v2x_res[29];
|
||||
ant_v2x_data.temp = *(float*)&ant_v2x_res[34];
|
||||
ant_v2x_data.distor = *(int8_t*)&ant_v2x_res[39];
|
||||
ant_v2x_data.cal_status = *(int8_t*)&ant_v2x_res[41];
|
||||
}
|
||||
|
||||
|
||||
#define SPI_SIG_ON_WRITING() { \
|
||||
uint8_t c __attribute__ ((unused)) = SPI_read(); \
|
||||
if (ant_v2x_req_idx < ant_v2x_req_len) { \
|
||||
SPI_transmit(ant_v2x_req[ant_v2x_req_idx]); \
|
||||
} \
|
||||
else if (ant_v2x_req_idx == ant_v2x_req_len) { \
|
||||
SPI_transmit(TERMINATOR); \
|
||||
} \
|
||||
else { \
|
||||
ant_v2x_com_status = MAG_CS_IDLE; \
|
||||
SPI_stop(); \
|
||||
} \
|
||||
ant_v2x_req_idx++; \
|
||||
}
|
||||
|
||||
|
||||
static uint8_t nb_retry = 0;
|
||||
#define MAX_RETRY 10
|
||||
|
||||
#define SPI_SIG_ON_READING() { \
|
||||
ant_v2x_res[ant_v2x_res_idx] = SPI_read(); \
|
||||
if (ant_v2x_res_idx == 0) { \
|
||||
if (nb_retry > MAX_RETRY) { \
|
||||
ant_v2x_reset(); \
|
||||
nb_retry = 0; \
|
||||
/*YELLOW_LED_ON();*/ \
|
||||
goto sig_exit; \
|
||||
} \
|
||||
if (ant_v2x_res[ant_v2x_res_idx] != SYNC_FLAG) { \
|
||||
nb_retry++; \
|
||||
SPI_transmit(0x00); \
|
||||
goto sig_exit; \
|
||||
} \
|
||||
else { \
|
||||
nb_retry = 0; \
|
||||
} \
|
||||
} \
|
||||
ant_v2x_res_idx++; \
|
||||
if (ant_v2x_res_idx < ant_v2x_res_len) { \
|
||||
SPI_transmit(0x00); \
|
||||
} \
|
||||
else { \
|
||||
ant_v2x_com_status = MAG_CS_IDLE; \
|
||||
SPI_stop(); \
|
||||
ant_v2x_status = MAG_S_READY; \
|
||||
ant_v2x_data_available = TRUE; \
|
||||
/*GREEN_LED_OFF();*/ \
|
||||
} \
|
||||
} \
|
||||
|
||||
SIGNAL(SIG_SPI) {
|
||||
switch ( ant_v2x_com_status) {
|
||||
case MAG_CS_WRITING:
|
||||
SPI_SIG_ON_WRITING();
|
||||
break;
|
||||
case MAG_CS_READING:
|
||||
SPI_SIG_ON_READING();
|
||||
}
|
||||
sig_exit:
|
||||
/*GREEN_LED_OFF();*/
|
||||
asm("nop");
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
#ifndef ANT_V2X_H
|
||||
#define ANT_V2X_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
extern void ant_v2x_init( void );
|
||||
extern void ant_v2x_periodic(void);
|
||||
extern bool_t ant_v2x_is_in_calibration(void);
|
||||
extern void ant_v2x_reset( void );
|
||||
extern void ant_v2x_read_data( void );
|
||||
|
||||
struct Ant_V2xConfig
|
||||
{
|
||||
float declination;
|
||||
uint8_t true_north;
|
||||
uint8_t cal_sample_freq;
|
||||
uint8_t sample_freq;
|
||||
uint8_t period;
|
||||
uint8_t big_idian;
|
||||
uint8_t damping_size;
|
||||
};
|
||||
|
||||
struct Ant_V2xData
|
||||
{
|
||||
int32_t xraw;
|
||||
int32_t yraw;
|
||||
float xcal;
|
||||
float ycal;
|
||||
float heading;
|
||||
float magnitude;
|
||||
float temp;
|
||||
uint8_t distor;
|
||||
uint8_t cal_status;
|
||||
};
|
||||
|
||||
struct Ant_V2xCal
|
||||
{
|
||||
int8_t byte_count;
|
||||
int32_t x_offset;
|
||||
int32_t y_offset;
|
||||
int32_t x_gain;
|
||||
int32_t y_gain;
|
||||
float phi;
|
||||
float cal_magnitude;
|
||||
};
|
||||
|
||||
extern volatile bool_t ant_v2x_data_available;
|
||||
extern struct Ant_V2xData ant_v2x_data;
|
||||
|
||||
#endif /* ANT_V2X_H */
|
||||
|
||||
@@ -0,0 +1,68 @@
|
||||
|
||||
#include "std.h"
|
||||
#include "sys_time.h"
|
||||
#include "init_hw.h"
|
||||
#include "interrupt_hw.h"
|
||||
|
||||
#include "led.h"
|
||||
|
||||
#include "uart.h"
|
||||
#include "print.h"
|
||||
|
||||
#include "adc.h"
|
||||
|
||||
#include "ant_v2x.h"
|
||||
|
||||
#include "messages.h"
|
||||
#include "downlink.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();
|
||||
LED_ON(1);
|
||||
LED_OFF(2);
|
||||
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();
|
||||
uart1_init_tx();
|
||||
adc_init();
|
||||
int_enable();
|
||||
}
|
||||
|
||||
static inline void main_event_task( void ) {
|
||||
if (ant_v2x_data_available) {
|
||||
ant_v2x_read_data();
|
||||
DOWNLINK_SEND_ANTENNA_DEBUG(&ant_v2x_data.xraw, &ant_v2x_data.yraw, \
|
||||
&ant_v2x_data.xcal, &ant_v2x_data.ycal, \
|
||||
&ant_v2x_data.heading, &ant_v2x_data.magnitude, \
|
||||
&ant_v2x_data.temp, &ant_v2x_data.distor, \
|
||||
&ant_v2x_data.cal_status);
|
||||
ant_v2x_data_available = FALSE;
|
||||
}
|
||||
}
|
||||
|
||||
static inline void main_periodic_task( void ) {
|
||||
static uint8_t cnt;
|
||||
cnt++;
|
||||
if (!(cnt%16)) {
|
||||
LED_TOGGLE(2);
|
||||
// uart1_transmit('#');
|
||||
// Uart1PrintHex(cnt);
|
||||
// uart1_transmit('\n');
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user