Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 19:47:50 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

first flight gorrazoptere

Browse Source
This commit is contained in:
Pascal Brisset
2005-09-02 19:05:12 +00:00
parent 80bacdd520
commit ebc92ae1a2
10 changed files with 85 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/autopilot/estimator.c
+7 -7
View File
@@ -121,13 +121,13 @@ void estimator_update_state_3DMG( void ) {
estimator_psi = from_fbw.euler[1];
estimator_theta = from_fbw.euler[2];
}
#elif defined SECTION_IMU_ANALOG
#include "ahrs.h"
void estimator_update_state_ANALOG( void ) {
estimator_phi = ahrs_euler[0];
estimator_theta = ahrs_euler[1];
estimator_psi = ahrs_euler[2];
}
//#elif defined SECTION_IMU_ANALOG
//#include "ahrs.h"
//void estimator_update_state_ANALOG( void ) {
// estimator_phi = ahrs_euler[0];
// estimator_theta = ahrs_euler[1];
// estimator_psi = ahrs_euler[2];
//}
#else //NO_IMU
void estimator_update_state_infrared( void ) {
float rad_of_ir = (ir_estim_mode == IR_ESTIM_MODE_ON && EstimatorIrGainIsCorrect()) ?
sw/airborne/autopilot/main.c
+21 -5
View File
@@ -433,7 +433,7 @@ void navigation_task( void ) {
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
|| pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
if (lateral_mode >= LATERAL_MODE_COURSE)
if (lateral_mode >=LATERAL_MODE_COURSE)
course_pid_run(); /* aka compute nav_desired_roll */
desired_roll = nav_desired_roll;
if (vertical_mode == VERTICAL_MODE_AUTO_ALT)
@@ -525,12 +525,12 @@ inline void periodic_task( void ) {
#if defined SECTION_IMU_3DMG
#elif defined SECTION_IMU_ANALOG
//#elif defined SECTION_IMU_ANALOG
//20Hz/3 it could be possible since it can run standalone at 60Hz/3
ahrs_update();
estimator_update_state_ANALOG();
// ahrs_update();
// estimator_update_state_ANALOG();
#else //NO IMU
ir_update();
ir_update();
estimator_update_state_infrared();
#endif
roll_pitch_pid_run(); /* Set desired_aileron & desired_elevator */
@@ -578,3 +578,19 @@ void use_gps_pos( void ) {
DOWNLINK_SEND_TAKEOFF(&cputime);
}
}
sw/airborne/autopilot/mainloop.c
+5
View File
@@ -39,6 +39,7 @@
#include "infrared.h"
#include "estimator.h"
#include "downlink.h"
#include "uart.h"
#ifdef SECTION_IMU_ANALOG
#include "ahrs.h"
@@ -123,6 +124,8 @@ int main( void ) {
* -it's not safe always
*/
ahrs_save_pqr_from_fbw();
int16_t dummy;
DOWNLINK_SEND_IMU_3DMG(&from_fbw.euler_dot[0], &from_fbw.euler_dot[1], &from_fbw.euler_dot[2], &dummy, &dummy, &dummy);
uart0_transmit('E');
uart0_transmit(' ');
@@ -148,3 +151,5 @@ int main( void ) {
return 0;
}
sw/airborne/autopilot/uart.c
+1 -1
View File
@@ -124,7 +124,7 @@ void uart0_init( void ) {
/* Set frame format: 8data, 1stop bit */
UCSR0C = _BV(UCSZ1) | _BV(UCSZ0);
/* Enable uart receive interrupt */
sbi(UCSR0B, RXCIE );
// sbi(UCSR0B, RXCIE );
}
void uart1_init( void ) {
sw/airborne/fly_by_wire/adc_fbw.c
+6 -1
View File
@@ -43,8 +43,13 @@
#define ANALOG_PORT_DIR DDRC
//
#ifdef SECTION_IMU_ANALOG
#define ANALOG_VREF _BV(REFS0)
#else
#define ANALOG_VREF _BV(REFS0) | _BV(REFS1)
#endif
uint16_t adc_samples[ NB_ADC ];
sw/airborne/fly_by_wire/adc_fbw.h
+1 -1
View File
@@ -42,7 +42,7 @@ extern uint16_t adc_samples[ NB_ADC ];
void adc_init( void );
#define AV_NB_SAMPLE 0x20
#define AV_NB_SAMPLE 0x08
struct adc_buf {
uint16_t sum;
sw/airborne/fly_by_wire/imu.c
+6 -6
View File
@@ -69,9 +69,9 @@ 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;
uint16_t raw_roll_dot_neutral = IMU_ADC_ROLL_DOT_ZERO*AV_NB_SAMPLE;
uint16_t raw_pitch_dot_neutral = IMU_ADC_PITCH_DOT_ZERO*AV_NB_SAMPLE;
uint16_t raw_yaw_dot_neutral = IMU_ADC_YAW_DOT_ZERO*AV_NB_SAMPLE;
void imu_init ( void ) {
adc_buf_channel(IMU_ADC_ROLL_DOT, &buf_roll_dot);
@@ -80,9 +80,9 @@ void imu_init ( void ) {
}
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;
raw_roll_dot = IMU_ADC_ROLL_DOT_SIGN * buf_roll_dot.sum;
raw_pitch_dot = IMU_ADC_PITCH_DOT_SIGN * buf_pitch_dot.sum;
raw_yaw_dot = IMU_ADC_YAW_DOT_SIGN * 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;;
sw/airborne/fly_by_wire/led.h
+9
View File
@@ -20,6 +20,15 @@
#define GREEN_LED_OFF() PORTB |= _BV(1)
#define GREEN_LED_TOGGLE() PORTB ^= _BV(1)
#define CounterPin 4
#define CounterLedInit() DDRD |= _BV(CounterPin)
#define CounterLedOn() PORTD &= ~_BV(CounterPin)
#define CounterLedOff() PORTD |= _BV(CounterPin)
#define CounterLedToggle() PORTD ^= _BV(CounterPin)
sw/airborne/fly_by_wire/main.c
+28 -3
View File
@@ -38,6 +38,7 @@
#include "spi.h"
#include "link_autopilot.h"
#include "radio.h"
#include "led.h"
#include "uart.h"
@@ -53,7 +54,6 @@
#include "adc_fbw.h"
struct adc_buf vsupply_adc_buf;
struct adc_buf vservos_adc_buf;
uint8_t mode;
static uint8_t time_since_last_mega128;
@@ -104,8 +104,20 @@ inline void radio_control_task(void) {
if (last_radio_contains_avg_channels) {
mode = MODE_OF_PPRZ(last_radio[RADIO_MODE]);
}
#if defined SECTION_IMU_ANALOG
if (last_radio[RADIO_SWITCH1] > MAX_PPRZ/2) {
imu_capture_neutral();
CounterLedOn();
} else {
CounterLedOff();
}
#endif
if (mode == MODE_MANUAL) {
#if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG
roll_dot_pgain = -100. + (float)last_radio[RADIO_GAIN1] * 0.010;
roll_dot_dgain = 2.5 - (float)last_radio[RADIO_GAIN2] * 0.00025;
pitch_dot_pgain = roll_dot_pgain;
pitch_dot_dgain = roll_dot_dgain;
control_set_desired(last_radio);
#else
servo_set(last_radio);
@@ -129,6 +141,11 @@ inline void spi_task(void) {
spi_reset();
}
#ifndef ADC_CHANNEL_VSUPPLY
#define ADC_CHANNEL_VSUPPLY 3
// for compatibility
#endif
int main( void )
{
uart_init_tx();
@@ -138,9 +155,9 @@ int main( void )
uart_print_string("FBW Booting $Id$\n");
#endif
adc_init();
adc_buf_channel(3, &vsupply_adc_buf);
adc_buf_channel(6, &vservos_adc_buf);
adc_buf_channel(ADC_CHANNEL_VSUPPLY, &vsupply_adc_buf);
#if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG
CounterLedInit();
imu_init();
#endif
timer_init();
@@ -208,6 +225,14 @@ int main( void )
#endif
#if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG
control_run();
if (radio_ok) {
if (last_radio[RADIO_THROTTLE] > 0.1*MAX_PPRZ) {
servo_set(control_commands);
}
else {
servo_set(failsafe);
}
}
#endif
if (_1Hz >= 60) {
_1Hz = 0;
sw/airborne/fly_by_wire/servo.c
+1 -1
View File
@@ -50,7 +50,7 @@
#define _4017_RESET_PORT PORTD
#define _4017_RESET_DDR DDRD
#define _4017_RESET_PIN 7
#endif /* CTL_BRD_V1_2 */
#endif /* CTL_BRD_V1_2_1 */
#define _4017_CLOCK_PORT PORTB
#define _4017_CLOCK_DDR DDRB