christoph/failnix
1
Fork 0
Code Releases Activity
Files
main
failnix/targets/wasm-tacle/parallel/PapaBench/sw/airborne/autopilot/main.c

678 lines
18 KiB
C
Raw Permalink Blame History

/*
$Id: main.c,v 1.2 2011-01-18 14:55:52 moellmer Exp $
Copyright (C) 2003 Pascal Brisset, Antoine Drouin
This file is part of paparazzi.
paparazzi is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2, or (at your option)
any later version.
paparazzi is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with paparazzi; see the file COPYING. If not, write to
the Free Software Foundation, 59 Temple Place - Suite 330,
Boston, MA 02111-1307, USA.
*/
/** \file main.c
\brief Regroup main functions
*/
#include <inttypes.h>
#include <math.h>
#include "link_autopilot.h"
#include "timer.h"
#include "adc.h"
#include "pid.h"
#include "gps.h"
#include "infrared.h"
#include "downlink.h"
#include "nav.h"
#include "autopilot.h"
#include "estimator.h"
#include "if_calib.h"
//
//
// FIXME estimator_flight_time should not be manipuled here anymore
//
/** Define minimal speed for takeoff in m/s */
#define MIN_SPEED_FOR_TAKEOFF 5.
uint8_t fatal_error_nb = 0;
static const uint16_t version = 1;
/** in seconds */
static uint16_t cputime = 0;
uint8_t pprz_mode = PPRZ_MODE_MANUAL;
uint8_t vertical_mode = VERTICAL_MODE_MANUAL;
uint8_t lateral_mode = LATERAL_MODE_MANUAL;
uint8_t ir_estim_mode = IR_ESTIM_MODE_ON;
bool_t auto_pitch = FALSE;
bool_t rc_event_1, rc_event_2;
uint8_t vsupply;
static uint8_t mcu1_status, mcu1_ppm_cpt;
static bool_t low_battery = FALSE;
float slider_1_val, slider_2_val;
bool_t launch = FALSE;
static uint8_t boot = TRUE;
static uint8_t count;
//#define Min(x, y) (x < y ? x : y)
//#define Max(x, y) (x > y ? x : y)
#define NO_CALIB 0 /**< \enum No calibration state */
#define WAITING_CALIB_CONTRAST 1 /**< \enum Waiting calibration contrast state */
#define CALIB_DONE 2 /**< \enum Calibration done state */
/** Maximal delay for calibration */
#define MAX_DELAY_FOR_CALIBRATION 10
/** \fn inline void ground_calibrate( void )
\brief Calibrate contrast if paparazzi mode is
set to auto1 before MAX_DELAY_FOR_CALIBRATION secondes */
/**User must put verticaly the uav (nose bottom) and push
radio roll stick to get new calibration
If not, the default calibration is used.
*/
inline void ground_calibrate( void )
{
static uint8_t calib_status = NO_CALIB;
/*#ifdef WITH_SWITCH
switch (calib_status) {
case NO_CALIB:
if (cputime < MAX_DELAY_FOR_CALIBRATION && pprz_mode == PPRZ_MODE_AUTO1 ) {
calib_status = WAITING_CALIB_CONTRAST;
DOWNLINK_SEND_CALIB_START();
}
break;
case WAITING_CALIB_CONTRAST:
if (STICK_PUSHED(from_fbw.channels[ RADIO_ROLL ])) {
ir_gain_calib();
estimator_rad_of_ir = ir_rad_of_ir;
DOWNLINK_SEND_RAD_OF_IR(&estimator_ir, &estimator_rad, &estimator_rad_of_ir, &ir_roll_neutral, &ir_pitch_neutral);
calib_status = CALIB_DONE;
DOWNLINK_SEND_CALIB_CONTRAST(&ir_contrast);
}
break;
case CALIB_DONE:
break;
}
#else*/
if ( calib_status == NO_CALIB ) {
if ( cputime < MAX_DELAY_FOR_CALIBRATION && pprz_mode == PPRZ_MODE_AUTO1 ) {
calib_status = WAITING_CALIB_CONTRAST;
DOWNLINK_SEND_CALIB_START();
}
} else
if ( calib_status == WAITING_CALIB_CONTRAST ) {
if ( STICK_PUSHED( from_fbw.channels[ RADIO_ROLL ] ) ) {
ir_gain_calib();
estimator_rad_of_ir = ir_rad_of_ir;
DOWNLINK_SEND_RAD_OF_IR( &estimator_ir, &estimator_rad, &estimator_rad_of_ir,
&ir_roll_neutral, &ir_pitch_neutral );
calib_status = CALIB_DONE;
DOWNLINK_SEND_CALIB_CONTRAST( &ir_contrast );
}
} else {}
//#endif
}
/** \fn inline uint8_t pprz_mode_update( void )
\brief Update paparazzi mode
*/
inline uint8_t pprz_mode_update( void )
{
/** We remain in home mode until explicit reset from the RC */
if ( pprz_mode != PPRZ_MODE_HOME || CheckEvent( rc_event_1 ) )
ModeUpdate( pprz_mode, PPRZ_MODE_OF_PULSE( from_fbw.channels[ RADIO_MODE ],
from_fbw.status ) );
else
return FALSE;
}
#ifdef RADIO_LLS
/** \fn inline uint8_t ir_estim_mode_update( void )
\brief update ir estimation if RADIO_LLS is true \n
*/
inline uint8_t ir_estim_mode_update( void )
{
ModeUpdate( ir_estim_mode,
IR_ESTIM_MODE_OF_PULSE( from_fbw.channels[ RADIO_LLS ] ) );
}
#endif
/** \fn inline uint8_t mcu1_status_update( void )
\brief @@@@@ A FIXER @@@@@
*/
inline uint8_t mcu1_status_update( void )
{
uint8_t new_mode = from_fbw.status;
if ( mcu1_status != new_mode ) {
bool_t changed = ( ( mcu1_status & MASK_FBW_CHANGED ) !=
( new_mode & MASK_FBW_CHANGED ) );
mcu1_status = new_mode;
return changed;
}
return FALSE;
}
/** Delay between @@@@@ A FIXER @@@@@ */
#define EVENT_DELAY 20
/** \def EventUpdate(_cpt, _cond, _event)
@@@@@ A FIXER @@@@@
*/
#define EventUpdate(_cpt, _cond, _event) \
if (_cond) { \
if (_cpt < EVENT_DELAY) { \
_cpt++; \
if (_cpt == EVENT_DELAY) \
_event = TRUE; \
} \
} else { \
_cpt = 0; \
_event = FALSE; \
}
/** \def EventPos(_cpt, _channel, _event)
@@@@@ A FIXER @@@@@
*/
#define EventPos(_cpt, _channel, _event) \
EventUpdate(_cpt, (inflight_calib_mode==IF_CALIB_MODE_NONE && from_fbw.channels[ _channel ]>(int)(0.75*MAX_PPRZ)), _event)
/** \def EventNeg(_cpt, _channel, _event)
@@@@@ A FIXER @@@@@
*/
#define EventNeg(_cpt, _channel, _event) \
EventUpdate(_cpt, (inflight_calib_mode==IF_CALIB_MODE_NONE && from_fbw.channels[ _channel ]<(int)(-0.75*MAX_PPRZ)), _event)
/** \fn static inline void events_update( void )
@@@@@ A FIXER @@@@@
*/
static inline void events_update( void )
{
static uint16_t event1_cpt = 0;
static uint16_t event2_cpt = 0;
EventPos( event1_cpt, RADIO_GAIN1, rc_event_1 );
EventNeg( event2_cpt, RADIO_GAIN1, rc_event_2 );
}
/** \fn inline void copy_from_to_fbw ( void )
\brief Send back uncontrolled channels (only rudder)
*/
inline void copy_from_to_fbw ( void )
{
to_fbw.channels[ RADIO_YAW ] = from_fbw.channels[ RADIO_YAW ];
#ifdef ANTON_T7
to_fbw.channels[ RADIO_PITCH ] = from_fbw.channels[ RADIO_PITCH ];
#endif
to_fbw.status = 0;
}
#ifdef EST_TEST
float est_pos_x;
float est_pos_y;
float est_fcourse;
uint8_t ticks_last_est; // 20Hz
#endif /* EST_TEST */
/*
called at 20Hz.
sends a serie of initialisation messages followed by a stream of periodic ones
*/
/** Define number of message at initialisation */
#define INIT_MSG_NB 2
/** @@@@@ A FIXER @@@@ */
#define HI_FREQ_PHASE_NB 5
//static char signature[ 16 ] = MESSAGES_MD5SUM;
/** \def PERIODIC_SEND_BAT()
@@@@@ A FIXER @@@@@
*/
#define PERIODIC_SEND_BAT() DOWNLINK_SEND_BAT(&vsupply, &estimator_flight_time, &low_battery, &block_time, &stage_time)
/** \def EventPos(_cpt, _channel, _event)
@@@@@ A FIXER @@@@@
*/
#define PERIODIC_SEND_DEBUG() DOWNLINK_SEND_DEBUG(&link_fbw_nb_err, &link_fbw_fbw_nb_err, &modem_nb_ovrn, &gps_nb_ovrn, &mcu1_ppm_cpt);
/** \def EventPos(_cpt, _channel, _event)
@@@@@ A FIXER @@@@@
*/
#define PERIODIC_SEND_ATTITUDE() DOWNLINK_SEND_ATTITUDE(&estimator_phi, &estimator_psi, &estimator_theta);
/** \def EventPos(_cpt, _channel, _event)
@@@@@ A FIXER @@@@@
*/
#define PERIODIC_SEND_ADC() DOWNLINK_SEND_ADC(&ir_roll, &ir_pitch);
/** \def EventPos(_cpt, _channel, _event)
@@@@@ A FIXER @@@@@
*/
#define PERIODIC_SEND_STABILISATION() DOWNLINK_SEND_STABILISATION(&roll_pgain, &pitch_pgain);
#define PERIODIC_SEND_CLIMB_PID() DOWNLINK_SEND_CLIMB_PID(&desired_gaz, &desired_climb, &climb_sum_err, &climb_pgain);
#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode);
#define PERIODIC_SEND_DESIRED() DOWNLINK_SEND_DESIRED(&desired_roll, &desired_pitch, &desired_x, &desired_y, &desired_altitude);
#define PERIODIC_SEND_PITCH() DOWNLINK_SEND_PITCH(&ir_pitch, &ir_pitch_neutral, &ir_gain);
#define PERIODIC_SEND_NAVIGATION_REF() DOWNLINK_SEND_NAVIGATION_REF(&utm_east0, &utm_north0);
#ifdef RADIO_CALIB
#define PERIODIC_SEND_SETTINGS() if (inflight_calib_mode != IF_CALIB_MODE_NONE) DOWNLINK_SEND_SETTINGS(&inflight_calib_mode, &slider_1_val, &slider_2_val);
#else
#define PERIODIC_SEND_SETTINGS()
#endif
/** \fn inline void reporting_task( void )
\brief Send a serie of initialisation messages followed by a stream of periodic ones\n
Called at 20Hz.
*/
void send_boot( void )
{
/** initialisation phase during boot */
if ( boot ) {
DOWNLINK_SEND_BOOT( &version );
DOWNLINK_SEND_RAD_OF_IR( &estimator_ir, &estimator_rad, &estimator_rad_of_ir,
&ir_roll_neutral, &ir_pitch_neutral );
boot = FALSE;
}
}
void send_attitude( void ) //500ms
{
if ( !boot ) {
count++;
if ( count == 250 ) count = 0;
if ( count % 5 == 0 )
PERIODIC_SEND_ATTITUDE();
}
}
void send_adc( void ) //500ms
{
if ( !boot ) {
if ( count % 5 == 1 ) PERIODIC_SEND_ADC();
}
}
void send_settings( void ) //500ms
{
if ( !boot ) {
if ( count % 5 == 2 ) PERIODIC_SEND_SETTINGS();
}
}
void send_desired( void ) //1000ms
{
if ( !boot ) {
if ( count % 10 == 3 ) PERIODIC_SEND_DESIRED();
}
}
void send_bat( void ) //2000ms
{
if ( !boot ) {
if ( count % 20 == 8 ) PERIODIC_SEND_BAT();
}
}
void send_climb( void ) //2000ms
{
if ( !boot ) {
if ( count % 20 == 18 ) PERIODIC_SEND_CLIMB_PID();
}
}
void send_mode( void ) //5000ms
{
if ( !boot ) {
if ( count % 50 == 9 ) PERIODIC_SEND_PPRZ_MODE();
}
}
void send_debug( void ) //5000ms
{
if ( !boot ) {
if ( count % 50 == 29 ) PERIODIC_SEND_DEBUG();
}
}
void send_nav_ref( void ) //10000ms
{
if ( !boot ) {
if ( count % 100 == 49 ) PERIODIC_SEND_NAVIGATION_REF();
}
}
/** \fn inline uint8_t inflight_calib_mode_update ( void )
\brief @@@@@ A FIXER @@@@@
*/
inline uint8_t inflight_calib_mode_update ( void )
{
ModeUpdate( inflight_calib_mode,
IF_CALIB_MODE_OF_PULSE( from_fbw.channels[ RADIO_CALIB ] ) );
}
/** \fn inline void radio_control_task( void )
\brief @@@@@ A FIXER @@@@@
*/
void _Pragma( "entrypoint" ) radio_control_task( void )
{
bool_t calib_mode_changed;
if ( link_fbw_receive_valid ) {
uint8_t mode_changed = FALSE;
copy_from_to_fbw();
if ( ( bit_is_set( from_fbw.status, RADIO_REALLY_LOST ) &&
( pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL ) ) ||
too_far_from_home ) {
pprz_mode = PPRZ_MODE_HOME;
mode_changed = TRUE;
}
if ( bit_is_set( from_fbw.status, AVERAGED_CHANNELS_SENT ) ) {
bool_t pprz_mode_changed = pprz_mode_update();
mode_changed |= pprz_mode_changed;
#ifdef RADIO_LLS
mode_changed |= ir_estim_mode_update();
#endif
#ifdef RADIO_CALIB
calib_mode_changed = inflight_calib_mode_update();
inflight_calib( calib_mode_changed || pprz_mode_changed );
mode_changed |= calib_mode_changed;
#endif
}
mode_changed |= mcu1_status_update();
if ( mode_changed )
DOWNLINK_SEND_PPRZ_MODE( &pprz_mode, &vertical_mode, &inflight_calib_mode,
&mcu1_status, &ir_estim_mode );
if ( pprz_mode == PPRZ_MODE_AUTO1 ) {
desired_roll = FLOAT_OF_PPRZ( from_fbw.channels[ RADIO_ROLL ], 0., -0.6 );
desired_pitch = FLOAT_OF_PPRZ( from_fbw.channels[ RADIO_PITCH ], 0., 0.5 );
} // else asynchronously set by course_pid_run()
if ( pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1 ) {
desired_gaz = from_fbw.channels[ RADIO_THROTTLE ];
#ifdef ANTON_MAGICAL_MISTERY_GAINS
roll_pgain = ROLL_PGAIN * ( 1 - 5. / 7. * from_fbw.channels[ RADIO_THROTTLE ] /
MAX_PPRZ );
pitch_pgain = PITCH_PGAIN * ( 1 - 1. / 3. * from_fbw.channels[ RADIO_THROTTLE ] /
MAX_PPRZ );
#endif /* ANTON_MAGICAL_MISTERY_GAINS */
}
// else asynchronously set by climb_pid_run();
mcu1_ppm_cpt = from_fbw.ppm_cpt;
vsupply = from_fbw.vsupply;
events_update();
if ( !estimator_flight_time ) {
ground_calibrate();
if ( pprz_mode == PPRZ_MODE_AUTO2 &&
from_fbw.channels[ RADIO_THROTTLE ] > GAZ_THRESHOLD_TAKEOFF )
launch = TRUE;
}
}
}
/** \fn void navigation_task( void )
\brief Compute desired_course
*/
void navigation_update( void )
{
/* Default to keep compatibility with previous behaviour */
lateral_mode = LATERAL_MODE_COURSE;
if ( pprz_mode == PPRZ_MODE_HOME )
nav_home();
else
nav_update();
}
void send_nav_values( void )
{
DOWNLINK_SEND_NAVIGATION( &nav_block, &nav_stage, &estimator_x, &estimator_y,
&desired_course, &dist2_to_wp, &course_pgain, &dist2_to_home );
}
void course_run( void )
{
if ( pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME ) {
if ( lateral_mode >= LATERAL_MODE_COURSE )
course_pid_run(); /* aka compute nav_desired_roll */
desired_roll = nav_desired_roll;
}
}
void _Pragma( "entrypoint" ) altitude_control_task( void )
{
if ( pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME ) {
if ( vertical_mode == VERTICAL_MODE_AUTO_ALT )
altitude_pid_run();
}
}
void _Pragma( "entrypoint" ) climb_control_task( void )
{
if ( pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME ) {
if ( vertical_mode >= VERTICAL_MODE_AUTO_CLIMB )
climb_pid_run();
if ( vertical_mode == VERTICAL_MODE_AUTO_GAZ )
desired_gaz = nav_desired_gaz;
if ( low_battery || ( !estimator_flight_time && !launch ) )
desired_gaz = 0.;
}
}
#define PERIOD (256. * 1024. / CLOCK / 1000000.)
/** Maximum time allowed for low battery level */
#define LOW_BATTERY_DELAY 5
/** \fn inline void periodic_task( void )
\brief Do periodic tasks at 61 Hz
*/
/**There are four @@@@@ boucles @@@@@:
- 20 Hz:
- lets use \a reporting_task at 10 Hz
- updates ir with \a ir_update
- updates estimator of ir with \a estimator_update_state_infrared
- set \a desired_aileron and \a desired_elevator with \a roll_pitch_pid_run
- sends to \a fbw \a desired_gaz, \a desired_aileron and
\a desired_elevator \note \a desired_gaz is set upon GPS
message reception
- 10 Hz: to get a \a stage_time_ds
- 4 Hz:
- calls \a estimator_propagate_state
- do navigation with \a navigation_task
*/
#ifdef PAPABENCH_SINGLE
uint8_t _20Hz = 0;
uint8_t _1Hz = 0;
#else
static uint8_t _20Hz = 0;
static uint8_t _1Hz = 0;
#endif
void periodic_task( void )
{
static uint8_t _10Hz = 0;
static uint8_t _4Hz = 0;
static uint8_t t = 0;
estimator_t += PERIOD;
_20Hz++;
if ( _20Hz >= 3 ) _20Hz = 0;
_10Hz++;
if ( _10Hz >= 6 ) _10Hz = 0;
_4Hz++;
if ( _4Hz >= 15 ) _4Hz = 0;
_1Hz++;
if ( _1Hz >= 61 ) _1Hz = 0;
if ( !_10Hz )
stage_time_ds = stage_time_ds + .1;
if ( !_1Hz ) {
if ( estimator_flight_time ) estimator_flight_time++;
cputime++;
stage_time_ds = ( int16_t )( stage_time_ds + .5 );
stage_time++;
block_time++;
if ( vsupply < LOW_BATTERY ) t++;
else t = 0;
low_battery |= ( t >= LOW_BATTERY_DELAY );
}
/*#ifdef WITH_SWITCH
switch(_4Hz) {
case 0:
estimator_propagate_state();
navigation_task();
altitude_control_task();//added 4-05-06
climb_control_task();//added 04-05-06
break;
// default:
}
switch (_20Hz) {
case 0:
break;
case 1: {
static uint8_t odd;
odd++;
if (odd & 0x01)
reporting_task();
break;
}
case 2:
stabilisation_task();
link_fbw_send();
break;
default:
fatal_error_nb++;
}
#else */
if ( _4Hz == 0 ) {
estimator_propagate_state();
/*navigation_task */
navigation_update();
send_nav_values();
course_run();
/*end navigation*/
altitude_control_task();
climb_control_task();
}
if ( _20Hz == 0 )
{}
else
if ( _20Hz == 1 ) {
static uint8_t odd;
odd++;
if ( odd & 0x01 ) {
/*reporting_task()*/
send_boot();
send_attitude();
send_adc();
send_settings();
send_desired();
send_bat();
send_climb();
send_mode();
send_debug();
send_nav_ref();
}
} else
if ( _20Hz == 2 ) {
stabilisation_task();
link_fbw_send();
} else
fatal_error_nb++;
//#endif
}
void _Pragma( "entrypoint" ) stabilisation_task( void )
{
ir_update();
estimator_update_state_infrared();
roll_pitch_pid_run(); // Set desired_aileron & desired_elevator
to_fbw.channels[ RADIO_THROTTLE ] =
desired_gaz; // desired_gaz is set upon GPS message reception
to_fbw.channels[ RADIO_ROLL ] = desired_aileron;
#ifndef ANTON_T7
to_fbw.channels[ RADIO_PITCH ] = desired_elevator;
#endif
// Code for camera stabilization, FIXME put that elsewhere
to_fbw.channels[ RADIO_GAIN1 ] = TRIM_PPRZ( MAX_PPRZ / 0.75 *
( -estimator_phi ) );
}
/*void receive_gps_data_task(void)
{
parse_gps_msg();
gps_msg_received = FALSE;
if (gps_pos_available)
{
use_gps_pos();
gps_pos_available = FALSE;
}
}*/
/** \fn void use_gps_pos( void )
\brief use GPS
*/
/**Send by downlink the GPS and rad_of_ir messages with \a DOWNLINK_SEND_GPS
and \a DOWNLINK_SEND_RAD_OF_IR \n
If \a estimator_flight_time is null and \a estimator_hspeed_mod is greater
than \a MIN_SPEED_FOR_TAKEOFF, set the \a estimator_flight_time to 1 and \a
launch to true (which is not set in non auto launch. Then call
\a DOWNLINK_SEND_TAKEOFF
*/
void send_gps_pos( void )
{
gps_msg_received = FALSE;
if ( gps_pos_available ) {
DOWNLINK_SEND_GPS( &gps_mode, &gps_utm_east, &gps_utm_north, &gps_fcourse,
&gps_falt, &gps_fspeed, &gps_fclimb, &gps_ftow );
estimator_update_state_gps();
}
}
void send_radIR( void )
{
if ( gps_pos_available )
DOWNLINK_SEND_RAD_OF_IR( &estimator_ir, &estimator_rad, &estimator_rad_of_ir,
&ir_roll_neutral, &ir_pitch_neutral );
}
void send_takeOff( void )
{
if ( gps_pos_available ) {
if ( !estimator_flight_time &&
( estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF ) ) {
estimator_flight_time = 1;
launch = TRUE; /* Not set in non auto launch */
DOWNLINK_SEND_TAKEOFF( &cputime );
}
gps_pos_available = FALSE;
}
}
View Git Blame Copy Permalink