C++ (Cpp) inter_mcu_fill_fbw_state 예제들

예제 #1

파일: main_fbw.c 프로젝트: AshuLara/lisa

void event_task_fbw( void) {
#ifdef RADIO_CONTROL
  RadioControlEvent(handle_rc_frame);
#endif


#ifdef INTER_MCU
#ifdef MCU_SPI_LINK
    link_mcu_event_task();
#endif /* MCU_SPI_LINK */


  if (inter_mcu_received_ap) {
    inter_mcu_received_ap = FALSE;
    inter_mcu_event_task();
    if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
      fbw_mode = FBW_MODE_AUTO;
    }
    if (fbw_mode == FBW_MODE_AUTO) {
      SetCommands(ap_state->commands);
    }
#ifdef SetApOnlyCommands
    else
    {
      SetApOnlyCommands(ap_state->commands);
    }
#endif
    fbw_new_actuators = 1;

#ifdef SINGLE_MCU
    inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
  }

#ifdef ACTUATORS
  if (fbw_new_actuators > 0)
  {
    SetActuatorsFromCommands(commands);
    fbw_new_actuators = 0;
  }
#endif




#ifdef MCU_SPI_LINK
  if (link_mcu_received) {
    link_mcu_received = FALSE;
    inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
    link_mcu_restart(); /** Prepares the next SPI communication */
  }
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */

}

예제 #2

파일: main_fbw.c 프로젝트: Fokker/paparazzi-1

/************* PERIODIC ******************************************************/
void periodic_task_fbw( void ) {

#ifdef RADIO_CONTROL
  radio_control_periodic_task();
  if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) {
#ifdef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
#warning WARNING DANGER: OUTBACK_CHALLENGE RULE RC_LOST_NO_AP defined. If you loose RC you will NOT go to automatically go to AUTO2 Anymore!!
set_failsafe_mode();
#else
    fbw_mode = FBW_MODE_AUTO;
#endif
  }
#endif

#ifdef INTER_MCU
  inter_mcu_periodic_task();
  if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
  {
    set_failsafe_mode();
  }
#endif

#ifdef MCU_UART_LINK
  inter_mcu_fill_fbw_state();
  link_mcu_periodic_task();
#endif

#ifdef DOWNLINK
  fbw_downlink_periodic_task();
#endif

}

예제 #3

파일: main_fbw.c 프로젝트: EwoudSmeur/paparazzi

/**
 * Handles inter_mcu event
 */
void inter_mcu_event_handle(void)
{
#if defined MCU_SPI_LINK | defined MCU_UART_LINK
  link_mcu_event_task();
#endif /* MCU_SPI_LINK */

  pre_inter_mcu_received_ap();

  if (inter_mcu_received_ap) {
    inter_mcu_received_ap = false;
    inter_mcu_event_task();

    PPRZ_MUTEX_LOCK(ap_state_mtx);
    command_roll_trim = ap_state->command_roll_trim;
    command_pitch_trim = ap_state->command_pitch_trim;
    command_yaw_trim = ap_state->command_yaw_trim;
    if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
      fbw_mode = FBW_MODE_INTER_MCU_FAILSAFE;
    }
    if (fbw_mode == FBW_MODE_AUTO) {
      SetCommands(ap_state->commands);
    } else {
#if SET_AP_ONLY_COMMANDS
      SetApOnlyCommands(ap_state->commands);
#endif /* SET_AP_ONLY_COMMANDS */
    }
    fbw_new_actuators = 1;
    PPRZ_MUTEX_UNLOCK(ap_state_mtx);

#ifdef SINGLE_MCU
    inter_mcu_fill_fbw_state();
#endif /* SINGLE_MCU - The buffer is filled even if the last receive was not correct */
  }

  post_inter_mcu_received_ap();

  update_actuators();

#ifdef MCU_SPI_LINK
  if (link_mcu_received) {
    link_mcu_received = false;
    inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
    link_mcu_restart(); /** Prepares the next SPI communication */
  }
#endif /* MCU_SPI_LINK */
}

예제 #4

파일: main_fbw.c 프로젝트: jziesing/paparazzi

/************* PERIODIC ******************************************************/
void periodic_task_fbw(void)
{

#ifdef FBW_DATALINK
  fbw_datalink_periodic();
#endif

#ifdef RADIO_CONTROL
  radio_control_periodic_task();
  if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) {
#if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
#warning WARNING DANGER: OUTBACK_CHALLENGE RULE RC_LOST_NO_AP defined. If you loose RC you will NOT go to automatically go to AUTO2 Anymore!!
    set_failsafe_mode();
#if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE
#warning WARNING DANGER: OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE defined. If you ever temporarly lost RC while in manual, you will failsafe forever even if RC is restored
    commands[COMMAND_FORCECRASH] = 9600;
#endif
#else
    fbw_mode = FBW_MODE_AUTO;
#endif
  }
#endif

#ifdef INTER_MCU
  inter_mcu_periodic_task();
  if (fbw_mode == FBW_MODE_AUTO && !ap_ok) {
    set_failsafe_mode();
  }
#endif

#ifdef MCU_UART_LINK
  inter_mcu_fill_fbw_state();
  link_mcu_periodic_task();
#endif

#ifdef MCU_CAN_LINK
  inter_mcu_fill_fbw_state();
  link_mcu_periodic_task();
#endif

#if PERIODIC_TELEMETRY
  periodic_telemetry_send_Fbw(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
#endif

}

예제 #5

파일: main_fbw.c 프로젝트: nvelozsavino/paparazzi

/**
 * Handles inter_mcu periodic checks
 */
void inter_mcu_periodic_handle(void)
{
    inter_mcu_periodic_task();
    if (fbw_mode == FBW_MODE_AUTO && !ap_ok) {
        set_failsafe_mode();
    }

#if defined MCU_UART_LINK || defined MCU_CAN_LINK
    inter_mcu_fill_fbw_state();
    link_mcu_periodic_task();
#endif /* defined MCU_UART_LINK || defined MCU_CAN_LINK */
}

예제 #6

파일: link_mcu_usart.c 프로젝트: 1bitsquared/paparazzi

void link_mcu_periodic_task( void )
{
  SixtyHzCounter++;
  if (SixtyHzCounter >= 3)
  {
    // 20 Hz
    SixtyHzCounter = 0;
    inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */

    InterMcuSend_INTERMCU_FBW(
                              fbw_state->ppm_cpt,
                              fbw_state->status,
                              fbw_state->nb_err,
                              fbw_state->vsupply,
                              fbw_state->current);
#if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
    InterMcuSend_INTERMCU_RADIO( fbw_state->channels );
#endif

  }
}

예제 #7

파일: main_fbw.c 프로젝트: BrandoJS/paparazzi

void event_task_fbw( void) {
#ifdef RADIO_CONTROL
  RadioControlEvent(handle_rc_frame);
#endif

  i2c_event();

#ifdef INTER_MCU
#ifdef MCU_SPI_LINK
    link_mcu_event_task();
#endif /* MCU_SPI_LINK */


  if (inter_mcu_received_ap) {
    inter_mcu_received_ap = FALSE;
    inter_mcu_event_task();
    if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
      fbw_mode = FBW_MODE_AUTO;
    }
    if (fbw_mode == FBW_MODE_AUTO) {
      SetCommands(ap_state->commands);
    }
#ifdef SetApOnlyCommands
    else
    {
      SetApOnlyCommands(ap_state->commands);
    }
#endif
    fbw_new_actuators = 1;

#ifdef SINGLE_MCU
    inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
  }

#ifdef ACTUATORS
  if (fbw_new_actuators > 0)
  {
    pprz_t trimmed_commands[COMMANDS_NB];
    int i;
    for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i];

    #ifdef COMMAND_ROLL
    trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10);
    #endif
    #ifdef COMMAND_PITCH
    trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
    #endif

    SetActuatorsFromCommands(trimmed_commands);
    fbw_new_actuators = 0;
  }
#endif


#ifdef MCU_SPI_LINK
  if (link_mcu_received) {
    link_mcu_received = FALSE;
    inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
    link_mcu_restart(); /** Prepares the next SPI communication */
  }
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */

}

예제 #8

파일: main_fbw.c 프로젝트: Fokker/paparazzi-1

void event_task_fbw( void) {
#ifdef RADIO_CONTROL
  RadioControlEvent(handle_rc_frame);
#endif

  i2c_event();

#ifdef INTER_MCU
#if defined MCU_SPI_LINK | defined MCU_UART_LINK
    link_mcu_event_task();
#endif /* MCU_SPI_LINK */


  if (inter_mcu_received_ap) {
    inter_mcu_received_ap = FALSE;
    inter_mcu_event_task();
    command_roll_trim = ap_state->command_roll_trim;
    command_pitch_trim = ap_state->command_pitch_trim;
#ifndef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
    if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
      fbw_mode = FBW_MODE_AUTO;
    }
#endif
    if (fbw_mode == FBW_MODE_AUTO) {
      SetCommands(ap_state->commands);
    }
#ifdef SetApOnlyCommands
    else
    {
      SetApOnlyCommands(ap_state->commands);
    }
#endif
    fbw_new_actuators = 1;

#ifdef SINGLE_MCU
    inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
  }

#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
#warning DANGER DANGER DANGER DANGER: Outback Challenge Rule FORCE-CRASH-RULE: DANGER DANGER: AP is now capable to FORCE your FBW in failsafe mode EVEN IF RC IS NOT LOST: Consider the consequences.

  int crash = 0;
  if (commands[COMMAND_FORCECRASH] >= 8000)
  {
    set_failsafe_mode();
    crash = 1;
  }

#endif
#ifdef ACTUATORS
  if (fbw_new_actuators > 0)
  {
    pprz_t trimmed_commands[COMMANDS_NB];
    int i;
    for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i];

    #ifdef COMMAND_ROLL
    trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10);
    #endif
    #ifdef COMMAND_PITCH
    trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
    #endif

    SetActuatorsFromCommands(trimmed_commands);
    fbw_new_actuators = 0;
    #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
    if (crash == 1)
    {
      for (;;) {}
    }
    #endif

  }
#endif


#ifdef MCU_SPI_LINK
  if (link_mcu_received) {
    link_mcu_received = FALSE;
    inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
    link_mcu_restart(); /** Prepares the next SPI communication */
  }
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */

}

예제 #9

파일: main_fbw.c 프로젝트: jziesing/paparazzi

void event_task_fbw(void)
{
#ifdef RADIO_CONTROL
  RadioControlEvent(handle_rc_frame);
#endif

  /* event functions for mcu peripherals, like i2c, uart, etc.. */
  mcu_event();

#ifdef INTER_MCU
#if defined MCU_SPI_LINK | defined MCU_UART_LINK
  link_mcu_event_task();
#endif /* MCU_SPI_LINK */


#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE
#warning OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE loose ap is forced crash
  if (ap_ok) {
    ap_has_been_ok = TRUE;
  }

  if ((ap_has_been_ok) && (!ap_ok)) {
    commands[COMMAND_FORCECRASH] = 9600;
  }
#endif

  if (inter_mcu_received_ap) {
    inter_mcu_received_ap = FALSE;
    inter_mcu_event_task();
    command_roll_trim = ap_state->command_roll_trim;
    command_pitch_trim = ap_state->command_pitch_trim;
    command_yaw_trim = ap_state->command_yaw_trim;
#if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
    // LOST-RC: do NOT go to autonomous
    // auto = stay in auto
    // manual = stay in manual
#else
    if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
      fbw_mode = FBW_MODE_AUTO;
    }
#endif
    if (fbw_mode == FBW_MODE_AUTO) {
      SetCommands(ap_state->commands);
    }
#ifdef SetApOnlyCommands
    else {
      SetApOnlyCommands(ap_state->commands);
    }
#endif
    fbw_new_actuators = 1;

#ifdef SINGLE_MCU
    inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
  }

#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
#warning DANGER DANGER DANGER DANGER: Outback Challenge Rule FORCE-CRASH-RULE: DANGER DANGER: AP is now capable to FORCE your FBW in failsafe mode EVEN IF RC IS NOT LOST: Consider the consequences.
  // OUTBACK: JURY REQUEST FLIGHT TERMINATION
  int crash = 0;
  if (commands[COMMAND_FORCECRASH] >= 8000) {
    set_failsafe_mode();
    crash = 1;
  }

#endif
#ifdef ACTUATORS
  if (fbw_new_actuators > 0) {
    pprz_t trimmed_commands[COMMANDS_NB];
    int i;
    for (i = 0; i < COMMANDS_NB; i++) { trimmed_commands[i] = commands[i]; }

#ifdef COMMAND_ROLL
    trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ / 10);
#endif
#ifdef COMMAND_PITCH
    trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ / 10);
#endif
#ifdef COMMAND_YAW
    trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ);
#endif

    SetActuatorsFromCommands(trimmed_commands, autopilot_mode);
    fbw_new_actuators = 0;
#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
    if (crash == 1) {
      for (;;) {
#if FBW_DATALINK
        fbw_datalink_event();
#endif
      }
    }
#endif

  }
#endif


#ifdef MCU_SPI_LINK
  if (link_mcu_received) {
    link_mcu_received = FALSE;
    inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
    link_mcu_restart(); /** Prepares the next SPI communication */
  }
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */

#ifdef FBW_DATALINK
  fbw_datalink_event();
#endif
}

예제 #10

파일: main_fbw.c 프로젝트: arizonamav/Old-paparazzi-designs

void event_task_fbw( void) {
#ifdef RADIO_CONTROL
  if (ppm_valid) {
    ppm_valid = FALSE;
    radio_control_event_task();
    if (rc_values_contains_avg_channels) {
      fbw_mode = FBW_MODE_OF_PPRZ(rc_values[RADIO_MODE]);
    }
    if (fbw_mode == FBW_MODE_MANUAL)
      SetCommandsFromRC(commands);
#ifdef CTL_GRZ
    if (fbw_mode == FBW_MODE_MANUAL)
      ctl_grz_set_setpoints_rate();
/*     else if (fbw_mode == FBW_MODE_AUTO) { */
/*       SetCommandsFromRC(commands); */
/*     } */
#endif /* CTL_GRZ */
  }
#endif


#ifdef INTER_MCU
#ifdef MCU_SPI_LINK
  if (spi_message_received) {
    /* Got a message on SPI. */
    spi_message_received = FALSE;

    /* Sets link_mcu_received */
    /* Sets inter_mcu_received_ap if checksum is ok */
    link_mcu_event_task();
  }
#endif /* MCU_SPI_LINK */

  if (inter_mcu_received_ap) {
    inter_mcu_received_ap = FALSE;
    inter_mcu_event_task();
    if (fbw_mode == FBW_MODE_AUTO) {
      SetCommands(ap_state->commands);
    }
#ifdef SINGLE_MCU
    inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
  }

#ifdef MCU_SPI_LINK
  if (link_mcu_received) {
    link_mcu_received = FALSE;
    inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
    link_mcu_restart(); /** Prepares the next SPI communication */
  }
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */

#ifdef LINK_IMU
  if (spi_message_received) {
    /* Got a message on SPI. */
    spi_message_received = FALSE;
    link_imu_event_task();
    EstimatorSetAtt(link_imu_state.eulers[AXIS_X], link_imu_state.eulers[AXIS_Z],  link_imu_state.eulers[AXIS_Y]);
#ifdef CTL_GRZ
    ctl_grz_set_measures(); 
#endif /* CTL_GRZ */
  }
#endif /* LINK_IMU */
}