Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2005-08-22 14:56:09 +00:00
parent bccc511022
commit 0dbc6b1b1b
10 changed files with 311 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/fly_by_wire/Makefile
-5
View File
@@ -43,11 +43,6 @@ ifeq ($(CTL_BRD_VERSION),V1_2)
CTL_BRD_FLAGS=-DCTL_BRD_V1_2
endif
ifeq ($(CTL_BRD_VERSION),V1_1)
CTL_BRD_FLAGS=-DCTL_BRD_V1_1
endif
$(TARGET).srcs = \
main.c \
ppm.c \
sw/airborne/fly_by_wire/control.c
+75
View File
@@ -0,0 +1,75 @@
#include "control.h"
#include "imu.h"
#include "airframe.h"
/* WARNING : taken from ../autopilot/autopilot.h */
#define TRIM_PPRZ(pprz) (pprz < MIN_PPRZ ? MIN_PPRZ : \
(pprz > MAX_PPRZ ? MAX_PPRZ : \
pprz))
int16_t desired_roll_dot;
float roll_dot_pgain = ROLL_DOT_PGAIN;
float roll_dot_igain = ROLL_DOT_IGAIN;
float roll_dot_dgain = ROLL_DOT_DGAIN;
int16_t roll_dot_sum_err = 0;
int16_t roll_dot_last_err = 0;
pprz_t command_roll;
int16_t desired_pitch_dot;
float pitch_dot_pgain = PITCH_DOT_PGAIN;
float pitch_dot_igain = PITCH_DOT_IGAIN;
float pitch_dot_dgain = PITCH_DOT_DGAIN;
int16_t pitch_dot_sum_err = 0;
int16_t pitch_dot_last_err = 0;
pprz_t command_pitch;
int16_t desired_yaw_dot;
float yaw_dot_pgain = YAW_DOT_PGAIN;
float yaw_dot_igain = YAW_DOT_IGAIN;
float yaw_dot_dgain = YAW_DOT_DGAIN;
int16_t yaw_dot_sum_err = 0;
int16_t yaw_dot_last_err = 0;
pprz_t command_yaw;
int16_t desired_throttle;
pprz_t command_throttle;
pprz_t control_commands[] = {0, 0, 0, 0};
#define TRIM(a, m) ((a) > m ? m : ((a) < -m ? -m : (a)))
#define Int16(x) (x > 32768. ? 32768 : x < -32768 ? -32768 : (int16_t)x)
void control_run ( void ) {
int16_t err = roll_dot - desired_roll_dot;
int16_t d_err = err - roll_dot_last_err;
roll_dot_last_err = err;
// roll_dot_sum_err += err;
// control_commands[RADIO_ROLL] = TRIM_PPRZ((err + d_err * roll_dot_dgain + roll_dot_sum_err * roll_dot_igain) * roll_dot_pgain);
control_commands[RADIO_ROLL] = TRIM_PPRZ(Int16((err +d_err * roll_dot_dgain) * roll_dot_pgain));
err = pitch_dot - desired_pitch_dot;
d_err = err - pitch_dot_last_err;
pitch_dot_last_err = err;
// pitch_dot_sum_err += err;
// control_commands[RADIO_PITCH] = TRIM_PPRZ((err + d_err * pitch_dot_dgain + pitch_dot_sum_err * pitch_dot_igain) * pitch_dot_pgain);
control_commands[RADIO_PITCH] = TRIM_PPRZ(Int16((err + d_err * pitch_dot_dgain) * pitch_dot_pgain));
err = yaw_dot - desired_yaw_dot;
// d_err = err - yaw_dot_last_err;
// yaw_dot_last_err = err;
// yaw_dot_sum_err += err;
// control_commands[RADIO_YAW] = TRIM_PPRZ((err + d_err * yaw_dot_dgain + yaw_dot_sum_err * yaw_dot_igain) * yaw_dot_pgain);
control_commands[RADIO_YAW] = TRIM_PPRZ(Int16(err * yaw_dot_pgain));
control_commands[RADIO_THROTTLE] = TRIM_PPRZ(desired_throttle);
}
void control_set_desired ( const pprz_t values[] ) {
desired_roll_dot = DESIRED_OF_RADIO_ROLL_DOT*values[RADIO_ROLL];
desired_pitch_dot = DESIRED_OF_RADIO_PITCH_DOT*values[RADIO_PITCH];
desired_yaw_dot = DESIRED_OF_RADIO_YAW_DOT*values[RADIO_YAW];
desired_throttle = values[RADIO_THROTTLE];
}
sw/airborne/fly_by_wire/control.h
+26
View File
@@ -0,0 +1,26 @@
#ifndef CONTROL_H
#define CONTROL_H
#include <inttypes.h>
#include "link_autopilot.h"
extern int16_t desired_roll_dot;
extern float roll_dot_pgain;
extern float roll_dot_dgain;
extern int16_t desired_pitch_dot;
extern float pitch_dot_pgain;
extern float pitch_dot_dgain;
extern int16_t desired_yaw_dot;
extern float yaw_dot_dgain;
extern float yaw_dot_pgain;
extern int16_t desired_throttle;
extern pprz_t control_commands[];
void control_run ( void );
void control_set_desired ( const pprz_t values[] );
#endif /* CONTROL_H */
sw/airborne/fly_by_wire/imu.c
+67
View File
@@ -0,0 +1,67 @@
#include "imu.h"
#include "airframe.h"
int16_t roll_dot, pitch_dot, yaw_dot;
#ifdef IMU_TYPE_3DMG
#include "3dmg.h"
int16_t roll, pitch, yaw;
void imu_init ( void ) {
_3dmg_set_continuous_mode();
}
void imu_update ( void ) {
roll_dot = _3dmg_roll_dot;
pitch_dot = _3dmg_pitch_dot;
yaw_dot = _3dmg_yaw_dot;
roll = _3dmg_roll;
pitch = _3dmg_pitch;
yaw = _3dmg_yaw;
}
void imu_capture_neutral ( void ) {
_3dmg_capture_neutral();
}
#endif
#ifdef IMU_TYPE_ANALOG
#include "adc_fbw.h"
struct adc_buf buf_roll_dot;
struct adc_buf buf_pitch_dot;
struct adc_buf buf_yaw_dot;
uint16_t raw_roll_dot = 512*AV_NB_SAMPLE;
uint16_t raw_pitch_dot = 512*AV_NB_SAMPLE;
uint16_t raw_yaw_dot = 512*AV_NB_SAMPLE;
uint16_t raw_roll_dot_neutral = 512*AV_NB_SAMPLE;
uint16_t raw_pitch_dot_neutral = 512*AV_NB_SAMPLE;
uint16_t raw_yaw_dot_neutral = 512*AV_NB_SAMPLE;
void imu_init ( void ) {
adc_buf_channel(IMU_ADC_ROLL_DOT, &buf_roll_dot);
adc_buf_channel(IMU_ADC_PITCH_DOT, &buf_pitch_dot);
adc_buf_channel(IMU_ADC_YAW_DOT, &buf_yaw_dot);
}
void imu_update ( void ) {
raw_roll_dot = buf_roll_dot.sum;
raw_pitch_dot = buf_pitch_dot.sum;
raw_yaw_dot = buf_yaw_dot.sum;
roll_dot = (int16_t)(raw_roll_dot - raw_roll_dot_neutral) / AV_NB_SAMPLE;
pitch_dot = (int16_t)(raw_pitch_dot - raw_pitch_dot_neutral) / AV_NB_SAMPLE;
yaw_dot = (int16_t)(raw_yaw_dot - raw_yaw_dot_neutral) / AV_NB_SAMPLE;;
}
void imu_capture_neutral ( void ) {
raw_roll_dot_neutral = raw_roll_dot;
raw_pitch_dot_neutral = raw_pitch_dot;
raw_yaw_dot_neutral = raw_yaw_dot;
}
#endif
sw/airborne/fly_by_wire/imu.h
+15
View File
@@ -0,0 +1,15 @@
#ifndef IMU_H
#define IMU_H
#include <inttypes.h>
#ifdef IMU_TYPE_3DMG
extern int16_t roll, pitch, yaw;
#endif
extern int16_t roll_dot, pitch_dot, yaw_dot;
extern void imu_init ( void );
extern void imu_update ( void );
extern void imu_capture_neutral ( void );
#endif /* IMU_H */
sw/airborne/fly_by_wire/led.h
+26
View File
@@ -0,0 +1,26 @@
/* 3 leds plugged on rc receiver port for debug purpose */
#ifndef LED_H
#define LED_H
#include <avr/io.h>
#define LEDS_INIT() { \
DDRB |= _BV(0) | _BV(1); \
DDRC |= _BV(0); \
}
#define RED_LED_ON() PORTC &= ~_BV(0)
#define RED_LED_OFF() PORTC |= _BV(0)
#define RED_LED_TOGGLE() PORTC ^= _BV(0)
#define YELLOW_LED_ON() PORTB &= ~_BV(0)
#define YELLOW_LED_OFF() PORTB |= _BV(0)
#define YELLOW_LED_TOGGLE() PORTB ^= _BV(0)
#define GREEN_LED_ON() PORTB &= ~_BV(1)
#define GREEN_LED_OFF() PORTB |= _BV(1)
#define GREEN_LED_TOGGLE() PORTB ^= _BV(1)
#endif /* LED_H */
sw/airborne/fly_by_wire/link_autopilot.h
+6
View File
@@ -47,6 +47,12 @@ struct inter_mcu_msg {
uint8_t status;
uint8_t nb_err;
uint8_t vsupply; /* 1e-1 V */
#if defined IMU_TYPE_3DMG || defined IMU_TYPE_ANALOG
int16_t euler_dot[3];
#endif
#ifdef IMU_TYPE_3DMG
int16_t euler[3];
#endif
};
// Status bits from FBW to AUTOPILOT
sw/airborne/fly_by_wire/main.c
+90 -50
View File
@@ -22,11 +22,18 @@
*
*/
//#define LED_DEBUG
#include <inttypes.h>
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#ifdef LED_DEBUG
#include "led.h"
#endif
#include "timer.h"
#include "servo.h"
#include "ppm.h"
@@ -37,12 +44,18 @@
#include "uart.h"
#ifdef IMU_TYPE_3DMG
#include "3dmg.h"
#endif
#if defined IMU_TYPE_ANALOG || defined IMU_TYPE_3DMG
#include "imu.h"
#include "control.h"
#endif
#ifndef CTL_BRD_V1_1
#include "adc_fbw.h"
struct adc_buf vsupply_adc_buf;
struct adc_buf vservos_adc_buf;
#endif
uint8_t mode;
static uint8_t time_since_last_mega128;
@@ -57,26 +70,6 @@ static uint8_t ppm_cpt, last_ppm_cpt;
#define REALLY_STALLED_TIME 300 // 5s with a 60Hz timer
/* static inline void status_transmit( void ) { */
/* uint8_t i; */
/* uart_transmit(7); */
/* uart_transmit(7); */
/* for (i=0; i<sizeof(struct inter_mcu_msg); i++) */
/* uart_transmit(((uint8_t*)&to_mega128)[i]); */
/* uart_transmit('\n'); */
/* uart_transmit(7); */
/* uart_transmit(7); */
/* uint8_t i; */
/* for(i = 0; i < RADIO_CTL_NB; i++) { */
/* extern uint16_t ppm_pulses[]; */
/* uart_transmit(ppm_pulses[i]>>8); */
/* uart_transmit(ppm_pulses[i] & 0xff); */
/* } */
/* uart_transmit('\n'); */
/* } */
/* Prepare data to be sent to mcu0 */
static inline void to_autopilot_from_last_radio (void) {
uint8_t i;
@@ -91,59 +84,102 @@ static inline void to_autopilot_from_last_radio (void) {
last_radio_contains_avg_channels = FALSE;
}
to_mega128.ppm_cpt = last_ppm_cpt;
#ifndef CTL_BRD_V1_1
to_mega128.vsupply = VoltageOfAdc(vsupply_adc_buf.sum/AV_NB_SAMPLE) * 10;
#else
to_mega128.vsupply = 0;
#if defined IMU_TYPE_3DMG || defined IMU_TYPE_ANALOG
to_mega128.euler_dot[0] = roll_dot;
to_mega128.euler_dot[1] = pitch_dot;
to_mega128.euler_dot[2] = yaw_dot;
#endif
#ifdef IMU_TYPE_3DMG
to_mega128.euler[0] = roll;
to_mega128.euler[1] = 777; //pitch;
to_mega128.euler[2] = yaw;
#endif
}
inline void radio_control_task(void) {
ppm_cpt++;
radio_ok = TRUE;
radio_really_lost = FALSE;
time_since_last_ppm = 0;
last_radio_from_ppm();
if (last_radio_contains_avg_channels) {
mode = MODE_OF_PPRZ(last_radio[RADIO_MODE]);
}
if (mode == MODE_MANUAL) {
#if defined IMU_TYPE_3DMG
// control_set_desired(last_radio);
#elif defined IMU_TYPE_ANALOG
control_set_desired(last_radio);
#else
servo_set(last_radio);
#endif
}
}
inline void spi_task(void) {
if (mega128_receive_valid) {
time_since_last_mega128 = 0;
mega128_ok = TRUE;
if (mode == MODE_AUTO)
#if defined IMU_TYPE_ANALOG || defined IMU_TYPE_3DMG
control_set_desired(from_mega128.channels);
#else
servo_set(from_mega128.channels);
#endif
}
to_autopilot_from_last_radio();
spi_reset();
}
int main( void )
{
uart_init_tx();
#if defined IMU_TYPE_3DMG
uart_init_rx();
#else
uart_print_string("FBW Booting $Id$\n");
#ifndef CTL_BRD_V1_1
#endif
adc_init();
adc_buf_channel(3, &vsupply_adc_buf);
adc_buf_channel(6, &vservos_adc_buf);
#if defined IMU_TYPE_3DMG || defined IMU_TYPE_ANALOG
imu_init();
#endif
timer_init();
#if defined LED_DEBUG
LEDS_INIT();
RED_LED_ON();
GREEN_LED_ON();
YELLOW_LED_ON();
#else
servo_init();
ppm_init();
#endif
spi_init();
sei();
while( 1 ) {
if( ppm_valid ) {
ppm_valid = FALSE;
ppm_cpt++;
radio_ok = TRUE;
radio_really_lost = FALSE;
time_since_last_ppm = 0;
last_radio_from_ppm();
if (last_radio_contains_avg_channels) {
mode = MODE_OF_PPRZ(last_radio[RADIO_MODE]);
}
if (mode == MODE_MANUAL) {
servo_set(last_radio);
}
} else if (mode == MODE_MANUAL && radio_really_lost) {
radio_control_task();
}
else if (mode == MODE_MANUAL && radio_really_lost) {
mode = MODE_AUTO;
}
if ( !SpiIsSelected() && spi_was_interrupted ) {
spi_was_interrupted = FALSE;
if (mega128_receive_valid) {
time_since_last_mega128 = 0;
mega128_ok = TRUE;
if (mode == MODE_AUTO)
servo_set(from_mega128.channels);
}
to_autopilot_from_last_radio();
spi_reset();
spi_task();
}
#ifdef IMU_TYPE_3DMG
if (_3dmg_data_ready) {
imu_update();
RED_LED_TOGGLE();
if (roll>0) {GREEN_LED_ON();}
else {GREEN_LED_OFF();}
if (pitch>0) {YELLOW_LED_ON();}
else {YELLOW_LED_OFF();}
}
#endif
if (time_since_last_ppm >= STALLED_TIME) {
radio_ok = FALSE;
}
@@ -166,6 +202,9 @@ int main( void )
static uint8_t _20Hz;
_1Hz++;
_20Hz++;
#ifdef IMU_TYPE_3DMG
// control_run();
#endif
if (_1Hz >= 60) {
_1Hz = 0;
last_ppm_cpt = ppm_cpt;
@@ -173,8 +212,9 @@ int main( void )
}
if (_20Hz >= 3) {
_20Hz = 0;
#ifndef IMU_TYPE_3DMG
servo_transmit();
// status_transmit();
#endif
}
if (time_since_last_mega128 < STALLED_TIME)
time_since_last_mega128++;
sw/airborne/fly_by_wire/servo.c
-6
View File
@@ -40,12 +40,6 @@
*/
#define _4017_NB_CHANNELS 10
#ifdef CTL_BRD_V1_1
#define _4017_RESET_PORT PORTD
#define _4017_RESET_DDR DDRD
#define _4017_RESET_PIN 5
#endif /* CTL_BRD_V1_1 */
#ifdef CTL_BRD_V1_2
#define _4017_RESET_PORT PORTC
#define _4017_RESET_DDR DDRC
sw/airborne/fly_by_wire/uart.h
+6
View File
@@ -36,4 +36,10 @@ void uart_print_hex16 ( uint16_t c );
void uart_print_string(const uint8_t* s);
void uart_print_float( const float * f);
#define ReceiveUart(cb) \
SIGNAL( SIG_UART_RECV ) { \
uint8_t c = UDR; \
cb(c); \
}
#endif