Beispiel #1
0
void loadDefaults() {

    state.memoryAvailable = 0;
    state.memoryPercentage = 50;

#ifndef NO_GUI

    video.sdlStarted = 0;
    video.agarStarted = 0;
    video.screenWtoApply = 0;
    video.screenHtoApply = 0;
    video.screenBPP = 0;
#ifdef WITHOUT_AGAR
    video.screenFS = 0;
#else
    video.screenFS = 1;
#endif
    video.screenAA = 0;

    strcpy(video.fontFile, "Vera.ttf");
    video.fontSize = 11;

#endif

    viewInit();
    spawnDefaults();
    stateInit();
    commandInit();

}
static inline void main_init(void)
{
  mcu_init();
  sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);

  stateInit();
  actuators_init();

  imu_init();
#if USE_AHRS_ALIGNER
  ahrs_aligner_init();
#endif
  ahrs_init();

  settings_init();

  mcu_int_enable();

  downlink_init();

  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive);
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands);
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);

  // send body_to_imu from here for now
  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
Beispiel #3
0
        void NumberParser::run() {
            prepare();

            States state = States::Init;
            Position before;
            while ( true ) {
                before = _input.position();

                char c{};

                if ( _input.eof() )
                    state = States::Quit;
                else
                    c = _input.readChar();

                switch ( state ) {
                case States::Init:
                    state = stateInit( c );
                    break;
                case States::Minus:
                    state = stateMinus( c );
                    break;
                case States::Zero:
                    state = stateZero( c );
                    break;
                case States::Digits:
                    state = stateDigits( c );
                    break;
                case States::Point:
                    state = statePoint( c );
                    break;
                case States::DecimalDigits:
                    state = stateDecimalDigits( c );
                    break;
                case States::E:
                    state = stateE( c );
                    break;
                case States::EPlusMinus:
                    state = stateEplusMinus( c );
                    break;
                case States::EDigits:
                    state = stateEDigits( c );
                    break;
                default:
                    break;
                }
                if ( state == States::Quit )
                    break;
            }
            _input.position( before );// return back the last character

            if ( _isE )
                postProcessing();
            if ( _isMinus ) {
                if ( isReal() )
                    _real *= -1;
                if ( isInteger() )
                    _integer *= -1;
            }
        }
Beispiel #4
0
STATIC_INLINE void main_init( void ) {

  mcu_init();

  electrical_init();

  stateInit();

  actuators_init();
#if USE_MOTOR_MIXING
  motor_mixing_init();
#endif

  radio_control_init();

  air_data_init();
#if USE_BARO_BOARD
  baro_init();
#endif
  imu_init();
#if USE_IMU_FLOAT
  imu_float_init();
#endif
  ahrs_aligner_init();
  ahrs_init();

  ins_init();

#if USE_GPS
  gps_init();
#endif

  autopilot_init();

  modules_init();

  settings_init();

  mcu_int_enable();

#if DATALINK == XBEE
  xbee_init();
#endif

#if DATALINK == UDP
  udp_init();
#endif

  // register the timers for the periodic functions
  main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
  modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
  radio_control_tid = sys_time_register_timer((1./60.), NULL);
  failsafe_tid = sys_time_register_timer(0.05, NULL);
  electrical_tid = sys_time_register_timer(0.1, NULL);
  telemetry_tid = sys_time_register_timer((1./TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
  baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL);
#endif
}
Beispiel #5
0
STATIC_INLINE void main_init( void ) {

  mcu_init();

  electrical_init();

  stateInit();

  actuators_init();
#if USE_MOTOR_MIXING
 // motor_mixing_init();
 // servo_mixing_init();
  set_servo_init();
#endif

  radio_control_init();

  baro_init();
  imu_init();
  autopilot_init();
  nav_init();
  guidance_h_init();
  guidance_v_init();
  stabilization_init();

  ahrs_aligner_init();
  ahrs_init();

  ins_init();

#if USE_GPS
  gps_init();
#endif

  modules_init();

  settings_init();

  mcu_int_enable();

#if DATALINK == XBEE
  xbee_init();
#endif

#if DATALINK == UDP
  udp_init();
#endif

  // register the timers for the periodic functions
  main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
  modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
  radio_control_tid = sys_time_register_timer((1./60.), NULL);
  failsafe_tid = sys_time_register_timer(0.05, NULL);
  electrical_tid = sys_time_register_timer(0.1, NULL);
  baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL);
  telemetry_tid = sys_time_register_timer((1./TELEMETRY_FREQUENCY), NULL);
}
 CoreAgent(ros::NodeHandle nh) :
     nh_(nh)
 {
     ROS_INFO("Core Agent Initiated");
     if(stateInit())
     {
         state_ = 1;
     }
 }
Beispiel #7
0
void init_ap( void ) 
{
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
  mcu_init();
#endif /* SINGLE_MCU */

  /************* Sensors initialization ***************/
#if USE_GPS
  gps_init();
#endif

#ifdef USE_GPIO
  GpioInit();
#endif

#if USE_IMU
  imu_init();
#endif

#if USE_AHRS_ALIGNER
  ahrs_aligner_init();
#endif

#if USE_AHRS
  ahrs_init();
#endif

#if USE_BAROMETER
  baro_init();
#endif

  ins_init();

  stateInit();

  /************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
  link_mcu_init();
#endif
#if USE_AUDIO_TELEMETRY
  audio_telemetry_init();
#endif

  /************ Internal status ***************/
  autopilot_init();
  h_ctl_init();
  v_ctl_init();
  nav_init();

  modules_init();

  settings_init();

  /**** start timers for periodic functions *****/
  sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL);
  navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL);
  attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL);
  modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
  telemetry_tid = sys_time_register_timer(1./60, NULL);
  monitor_tid = sys_time_register_timer(1.0, NULL);

  /** - start interrupt task */
  mcu_int_enable();

#if defined DATALINK
#if DATALINK == XBEE
  xbee_init();
#endif
#if DATALINK == W5100
  w5100_init();
#endif
#endif /* DATALINK */

#if defined AEROCOMM_DATA_PIN
  IO0DIR |= _BV(AEROCOMM_DATA_PIN);
  IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif

  /************ Multi-uavs status ***************/

#ifdef TRAFFIC_INFO
  traffic_info_init();
#endif
}
Beispiel #8
0
STATIC_INLINE void main_init(void)
{
  mcu_init();

  electrical_init();

  stateInit();

  actuators_init();
#if USE_MOTOR_MIXING
  motor_mixing_init();
#endif

  radio_control_init();

#if USE_BARO_BOARD
  baro_init();
#endif
#if USE_IMU
  imu_init();
#endif
#if USE_AHRS_ALIGNER
  ahrs_aligner_init();
#endif

#if USE_AHRS
  ahrs_init();
#endif

  ins_init();

#if USE_GPS
  gps_init();
#endif

  autopilot_init();

  modules_init();

  settings_init();

  mcu_int_enable();

#if DOWNLINK
  downlink_init();
#endif

  // register the timers for the periodic functions
  main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
  radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
  failsafe_tid = sys_time_register_timer(0.05, NULL);
  electrical_tid = sys_time_register_timer(0.1, NULL);
  telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
  baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif

#if USE_IMU
  // send body_to_imu from here for now
  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif

  // Do a failsafe check first
  failsafe_check();
}
Beispiel #9
0
int
main(int ac, char *av[])
{
        int          status = 0;
        char*        logfname = 0;
        /// Data directory, conffile paths may be relative
        const char*  datadir;
        int          interval = DEFAULT_INTERVAL;
        prod_spec    spec;
        prod_class_t clss;
        int          toffset = TOFFSET_NONE;
        int          loggingToStdErr = 0;
        unsigned     queue_size = 5000;
        const char*  progname = basename(av[0]);
        unsigned     logopts = LOG_CONS|LOG_PID;

        /*
         * Setup default logging before anything else.
         */
        (void)log_init(progname);

        const char*  pqfname = getQueuePath();

        spec.feedtype = DEFAULT_FEEDTYPE;
        spec.pattern = DEFAULT_PATTERN;

        if(set_timestamp(&clss.from)) /* corrected by toffset below */
        {
                int errnum = errno;
                log_error("Couldn't set timestamp: %s", strerror(errnum));
                exit(EXIT_FAILURE);
                /*NOTREACHED*/
        }
        clss.to = TS_ENDT;
        clss.psa.psa_len = 1;
        clss.psa.psa_val = &spec;

        /*
         * deal with the command line, set options
         */
        {
            extern int optind;
            extern int opterr;
            extern char *optarg;

            int ch;
            int fterr;

            opterr = 1;

            while ((ch = getopt(ac, av, "vxel:d:f:q:o:p:i:t:")) != EOF) {
                switch (ch) {
                case 'v':
                        if (!log_is_enabled_info)
                            (void)log_set_level(LOG_LEVEL_INFO);
                        break;
                case 'x':
                        (void)log_set_level(LOG_LEVEL_DEBUG);
                        break;
                case 'e':
                        key = ftok("/etc/rc.d/rc.local",'R');
                        semkey = ftok("/etc/rc.d/rc.local",'e');
                        shmid = shmget(key, sizeof(edex_message) * queue_size,
                                0666 | IPC_CREAT);
                        semid = semget(semkey, 2, 0666 | IPC_CREAT);
                        break;
                case 'l':
                        logfname = optarg;
                        (void)log_set_destination(logfname);
                        break;
                case 'd':
                        setPqactDataDirPath(optarg);
                        break;
                case 'f':
                        fterr = strfeedtypet(optarg, &spec.feedtype);
                        if(fterr != FEEDTYPE_OK)
                        {
                                log_error("Bad feedtype \"%s\", %s\n",
                                        optarg, strfeederr(fterr));
                                usage(progname);
                        }
                        break;
                case 'q':
                        pqfname = optarg;
                        break;
                case 'o':
                        toffset = atoi(optarg);
                        if(toffset == 0 && *optarg != '0')
                        {
                                log_error("invalid offset %s\n", optarg);
                                usage(progname);   
                        }
                        break;
                case 'i':
                        interval = atoi(optarg);
                        if(interval == 0 && *optarg != '0')
                        {
                                log_error("invalid interval %s\n", optarg);
                                usage(progname);   
                        }
                        break;
                case 't':
                        pipe_timeo = atoi(optarg);
                        if(pipe_timeo == 0 && *optarg != 0)
                        {
                                log_error("invalid pipe_timeo %s", optarg);
                                usage(progname);   
                        }
                        break;
                case 'p':
                        spec.pattern = optarg;
                        break;
                default:
                        usage(progname);
                        break;
                }
            }

            conffilename = getPqactConfigPath();
            datadir = getPqactDataDirPath();

            {
                int numOperands = ac - optind;

                if (1 < numOperands) {
                    log_error("Too many operands");
                    usage(progname);
                }
                else if (1 == numOperands) {
                    conffilename = av[optind];
                }
            }
        }

        setQueuePath(pqfname);
        log_notice("Starting Up");

        if ('/' != conffilename[0]) {
            /*
             * The pathname of the configuration-file is relative. Convert it
             * to absolute so that it can be (re)read even if the current
             * working directory changes.
             */
#ifdef PATH_MAX
            char    buf[PATH_MAX];          /* includes NUL */
#else
            char    buf[_POSIX_PATH_MAX];   /* includes NUL */
#endif
            if (getcwd(buf, sizeof(buf)) == NULL) {
                log_syserr("Couldn't get current working directory");
                exit(EXIT_FAILURE);
            }
            (void)strncat(buf, "/", sizeof(buf)-strlen(buf)-1);
            (void)strncat(buf, conffilename, sizeof(buf)-strlen(buf)-1);
            conffilename = strdup(buf);
            if (conffilename == NULL) {
                log_syserr("Couldn't duplicate string \"%s\"", buf);
                exit(EXIT_FAILURE);
            }
        }

        /*
         * Initialize the previous-state module for this process.
         */
        if (stateInit(conffilename) < 0) {
            log_error("Couldn't initialize previous-state module");
            exit(EXIT_FAILURE);
            /*NOTREACHED*/
        }

        /*
         * Configure the standard I/O streams for execution of child processes.
         */
        if (configure_stdio_file_descriptors()) {
            log_error("Couldn't configure standard I/O streams for execution "
                    "of child processes");
            exit(EXIT_FAILURE);
        }

        /*
         * Inform the "filel" module about the number of available file
         * descriptors.  File descriptors are reserved for stdin, stdout,
         * stderr, the product-queue, the configuration-file, and (possibly) 
         * logging.
         */
        if (0 != set_avail_fd_count(openMax() - 6))
        {
            log_error("Couldn't set number of available file-descriptors");
            log_notice("Exiting");
            exit(EXIT_FAILURE);
            /*NOTREACHED*/
        }

        /*
         * Inform the "filel" module of the shared memory segment
         */
        if (shmid != -1 && semid != -1)
        {
            set_shared_space(shmid, semid, queue_size);
        }

        /*
         * Compile the pattern.
         */
        if (re_isPathological(spec.pattern))
        {
                log_error("Adjusting pathological regular-expression: \"%s\"",
                    spec.pattern);
                re_vetSpec(spec.pattern);
        }
        status = regcomp(&spec.rgx, spec.pattern, REG_EXTENDED|REG_NOSUB);
        if(status != 0)
        {
                log_error("Can't compile regular expression \"%s\"",
                        spec.pattern);
                log_notice("Exiting");
                exit(EXIT_FAILURE);
                /*NOTREACHED*/
        }

        /*
         * register exit handler
         */
        if(atexit(cleanup) != 0)
        {
                log_syserr("atexit");
                log_notice("Exiting");
                exit(EXIT_FAILURE);
                /*NOTREACHED*/
        }

        /*
         * set up signal handlers
         */
        set_sigactions();

        /*
         * Read in (compile) the configuration file.  We do this first so
         * its syntax may be checked without opening a product queue.
         */
        if ((status = readPatFile(conffilename)) < 0) {
                exit(EXIT_FAILURE);
                /*NOTREACHED*/
        }
        else if (status == 0) {
            log_notice("Configuration-file \"%s\" has no entries. "
                "You should probably not start this program instead.",
                conffilename);
        }

        /*
         * Open the product queue
         */
        status = pq_open(pqfname, PQ_READONLY, &pq);
        if(status)
        {
                if (PQ_CORRUPT == status) {
                    log_error("The product-queue \"%s\" is inconsistent\n",
                            pqfname);
                }
                else {
                    log_error("pq_open failed: %s: %s\n",
                            pqfname, strerror(status));
                }
                exit(EXIT_FAILURE);
                /*NOTREACHED*/
        }

        if(toffset != TOFFSET_NONE) {
            /*
             * Filter and queue position set by "toffset".
             */
            clss.from.tv_sec -= toffset;
            pq_cset(pq, &clss.from);
        }
        else {
            bool       startAtTailEnd = true;
            timestampt insertTime;

            clss.from = TS_ZERO;

            /*
             * Try getting the insertion-time of the last,
             * successfully-processed data-product from the previous session.
             */
            status = stateRead(&insertTime);

            if (status) {
                log_warning("Couldn't get insertion-time of last-processed "
                        "data-product from previous session");
            }
            else {
                timestampt now;
                (void)set_timestamp(&now);

                if (tvCmp(now, insertTime, <)) {
                    log_warning("Time of last-processed data-product from previous "
                            "session is in the future");
                }
                else {
                    char buf[80];
                    (void)strftime(buf, sizeof(buf), "%Y-%m-%d %T",
                        gmtime(&insertTime.tv_sec));
                    log_notice("Starting from insertion-time %s.%06lu UTC", buf,
                        (unsigned long)insertTime.tv_usec);

                    pq_cset(pq, &insertTime);
                    startAtTailEnd = false;
                }
            }

            if (startAtTailEnd) {
                log_notice("Starting at tail-end of product-queue");
                (void)pq_last(pq, &clss, NULL);
            }
        }
Beispiel #10
0
void init_ap( void ) {
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
  mcu_init();
#endif /* SINGLE_MCU */

  /****** initialize and reset state interface ********/

  stateInit();

  /************* Sensors initialization ***************/
#if USE_GPS
  gps_init();
#endif

#if USE_IMU
  imu_init();
#if USE_IMU_FLOAT
  imu_float_init();
#endif
#endif

#if USE_AHRS_ALIGNER
  ahrs_aligner_init();
#endif

#if USE_AHRS
  ahrs_init();
#endif

#if USE_AHRS && USE_IMU
  register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_fliter_status);
#endif

  air_data_init();
#if USE_BARO_BOARD
  baro_init();
#endif

  ins_init();

  /************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
  link_mcu_init();
#endif
#if USE_AUDIO_TELEMETRY
  audio_telemetry_init();
#endif

  /************ Internal status ***************/
  autopilot_init();
  h_ctl_init();
  v_ctl_init();
  nav_init();

  modules_init();

  settings_init();

  /**** start timers for periodic functions *****/
  sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL);
  navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL);
  attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL);
  modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
  telemetry_tid = sys_time_register_timer(1./TELEMETRY_FREQUENCY, NULL);
  monitor_tid = sys_time_register_timer(1.0, NULL);

  /** - start interrupt task */
  mcu_int_enable();

#if defined DATALINK
#if DATALINK == XBEE
  xbee_init();
#endif
#if DATALINK == W5100
  w5100_init();
#endif
#endif /* DATALINK */

#if defined AEROCOMM_DATA_PIN
  IO0DIR |= _BV(AEROCOMM_DATA_PIN);
  IO0SET = _BV(AEROCOMM_DATA_PIN);
#endif

  /************ Multi-uavs status ***************/

#ifdef TRAFFIC_INFO
  traffic_info_init();
#endif

  /* set initial trim values.
   * these are passed to fbw via inter_mcu.
   */
  ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
  ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
  ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
}
Beispiel #11
0
void init_ap(void)
{
#ifndef SINGLE_MCU /** init done in main_fbw in single MCU */
  mcu_init();
#endif /* SINGLE_MCU */

  /** - start interrupt task */
  mcu_int_enable();

#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
  pprz_trig_int_init();
#endif

  /****** initialize and reset state interface ********/

  stateInit();

  /************* Sensors initialization ***************/

#if USE_AHRS
  ahrs_init();
#endif

#if USE_BARO_BOARD
  baro_init();
#endif

  /************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
  link_mcu_init();
#endif

  /************ Internal status ***************/
  autopilot_init();

  modules_init();

  // call autopilot implementation init after guidance modules init
  // it will set startup mode
#if USE_GENERATED_AUTOPILOT
  autopilot_generated_init();
#else
  autopilot_static_init();
#endif

  settings_init();

  /**** start timers for periodic functions *****/
  sensors_tid = sys_time_register_timer(1. / PERIODIC_FREQUENCY, NULL);
#if !USE_GENERATED_AUTOPILOT
  navigation_tid = sys_time_register_timer(1. / NAVIGATION_FREQUENCY, NULL);
#endif
  attitude_tid = sys_time_register_timer(1. / CONTROL_FREQUENCY, NULL);
  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
  telemetry_tid = sys_time_register_timer(1. / TELEMETRY_FREQUENCY, NULL);
  monitor_tid = sys_time_register_timer(1.0, NULL);
#if USE_BARO_BOARD
  baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif

#if DOWNLINK
  downlink_init();
#endif

  /* set initial trim values.
   * these are passed to fbw via inter_mcu.
   */
  PPRZ_MUTEX_LOCK(ap_state_mtx);
  ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
  ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
  ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
  PPRZ_MUTEX_UNLOCK(ap_state_mtx);

#if USE_IMU
  // send body_to_imu from here for now
  AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
}
Beispiel #12
0
ui_state_t UserInterface::getInput(void)
{
    uint32_t switch_depressed = 0;

    noInterrupts();

    if (getState() != _last_ui_state)
    {
        stateInit();
        _last_ui_state = _ui_state;
        _sleep = millis();
    }
    else if (s_switch_state != SWITCH_STATE__NONE)
    {
        _switch_state = s_switch_state;
        if (s_switch_state != SWITCH_STATE__DEPRESSED)
            s_switch_state = SWITCH_STATE__NONE;
        switch_depressed = millis() - s_switch_depressed;
        s_encoder_turn = 0;
    }
    else if (s_encoder_turn != 0)
    {
        _encoder_turn = s_encoder_turn;
        s_encoder_turn = 0;
        s_switch_state = SWITCH_STATE__NONE;
    }
    else if (s_brightness != _brightness)
    {
        _brightness = s_brightness;
    }

    interrupts();

    if (_switch_state == SWITCH_STATE__DEPRESSED)
    {
        if (switch_depressed >= RESET_TIME)
        {
            static int display_blink = 0;
            static uint32_t blink_last = 0;

            timerStop();

            if ((switch_depressed - blink_last) > 300)
            {
                blink_last = switch_depressed;

                display_blink ^= 1;

                if (display_blink)
                    Display::dashes();
                else
                    Display::blank();
            }
        }

        return UI_STATE__PASS;
    }

    if (sleep())
        return UI_STATE__PASS;

    return _ui_state;
}
Beispiel #13
0
STATIC_INLINE void main_init(void)
{
    mcu_init();

#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
    pprz_trig_int_init();
#endif

    electrical_init();

    stateInit();

#ifndef INTER_MCU_AP
    actuators_init();
#else
    intermcu_init();
#endif

#if USE_MOTOR_MIXING
    motor_mixing_init();
#endif

#ifndef INTER_MCU_AP
    radio_control_init();
#endif

#if USE_BARO_BOARD
    baro_init();
#endif

#if USE_AHRS_ALIGNER
    ahrs_aligner_init();
#endif

#if USE_AHRS
    ahrs_init();
#endif

    autopilot_init();

    modules_init();

    /* temporary hack:
     * Since INS is now a module, LTP_DEF is not yet initialized when autopilot_init is called
     * This led to the problem that global waypoints were not "localized",
     * so as a stop-gap measure we localize them all (again) here..
     */
    waypoints_localize_all();

    settings_init();

    mcu_int_enable();

#if DOWNLINK
    downlink_init();
#endif

#ifdef INTER_MCU_AP
    intermcu_init();
#endif

    // register the timers for the periodic functions
    main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
#if PERIODIC_FREQUENCY != MODULES_FREQUENCY
    modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
#endif
    radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
    failsafe_tid = sys_time_register_timer(0.05, NULL);
    electrical_tid = sys_time_register_timer(0.1, NULL);
    telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL);
#if USE_BARO_BOARD
    baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif

#if USE_IMU
    // send body_to_imu from here for now
    AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif

    // Do a failsafe check first
    failsafe_check();
}