static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up)
{

  if (msg_up->valid.vane && vane_callback)
    vane_callback(0, msg_up->vane_angle1, msg_up->vane_angle2);

  // Fill pressure data
  if (msg_up->valid.pressure_absolute && pressure_absolute_callback)
    pressure_absolute_callback(0, msg_up->pressure_absolute);

  if (msg_up->valid.pressure_differential && pressure_differential_callback)
    pressure_differential_callback(0, (32768 + msg_up->pressure_differential));

	if (msg_up->valid.adc) {
		if(adc_callback) {
			adc_callback(msg_up->adc.channels);
		}
	}

  // Fill radio data
  if (msg_up->valid.rc && radio_control_callback) {
    radio_control.values[RADIO_ROLL] = msg_up->rc_roll;
    radio_control.values[RADIO_PITCH] = msg_up->rc_pitch;
    radio_control.values[RADIO_YAW] = msg_up->rc_yaw;
    radio_control.values[RADIO_THROTTLE] = msg_up->rc_thrust;
    radio_control.values[RADIO_MODE] = msg_up->rc_mode;
    radio_control.values[RADIO_KILL] = msg_up->rc_kill;
    radio_control.values[RADIO_GEAR] = msg_up->rc_gear;
    radio_control.values[RADIO_AUX2] = msg_up->rc_aux2;
    radio_control.values[RADIO_AUX3] = msg_up->rc_aux3;
    radio_control_callback();
  }
  // always fill status, it may change even when in the case when there is no new data
  radio_control.status = msg_up->rc_status;

  // Fill IMU data
  imuFloat.gyro.p = RATE_FLOAT_OF_BFP(msg_up->gyro.p);
  imuFloat.gyro.q = RATE_FLOAT_OF_BFP(msg_up->gyro.q);
  imuFloat.gyro.r = RATE_FLOAT_OF_BFP(msg_up->gyro.r);

  imuFloat.accel.x = ACCEL_FLOAT_OF_BFP(msg_up->accel.x);
  imuFloat.accel.y = ACCEL_FLOAT_OF_BFP(msg_up->accel.y);
  imuFloat.accel.z = ACCEL_FLOAT_OF_BFP(msg_up->accel.z);

  imuFloat.mag.x = MAG_FLOAT_OF_BFP(msg_up->mag.x);
  imuFloat.mag.y = MAG_FLOAT_OF_BFP(msg_up->mag.y);
  imuFloat.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z);

	imuFloat.sample_count = msg_up->imu_tick;

  if (msg_up->valid.imu)
    rdyb_booz_imu_update(&imuFloat);
}
예제 #2
0
/**
  * @brief  Main program.
  * @param  None 
  * @retval None
  */
int main(void)
{  

  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
  // Initialize library
  ge_init();

  // Initialize LEDs
  setup_led_gpio();
  led_state = false;
  led_speed = false;

  // Initialize the USER button as an input
  gpio_setup_pin(GE_PBTN2, GPIO_INPUT, false, false);

  // Initialize PBTN1
  gpio_setup_pin(GE_PBTN1, GPIO_INPUT, false, false);

  // Print to serial port
  printf("Hello, World!\n");

  // Print Hello World
  lcd_clear();
  lcd_goto(0, 0);
  lcd_puts("Hello, World!");

  // Setup timer library
  // Set minimum timestep to 1ms (number of counts referecned to 
  // a 72MHz clock)
  timer_set_timestep(72000);
  // register callback for toggling LEDs every 500ms
  led_timer = timer_register(500, &toggle_led, GE_PERIODIC);
  timer_start(led_timer);

  // set mode to the LED demo
  ui_state = LED_DEMO;

  // set pwm level for PWM demo
  float pwm_level = 0.0;

  // setup PWM library
  pwm_freq(10000.0);

  // setup ADC library
  // set sampling rate to 10kHz
  adc_set_fs(10000);
  // register callback method
  adc_callback(&my_adc_callback);
  // enable ADC channels
  adc_enable_channels(chan_to_conv, NUM_ADC);
  adc_initialize_channels();
  adc_start();

  // keep track of how many times the UI has looped
  num_refresh = 0;


  /* Infinite loop */
  /**
   * Handles the user interface state machine
   */
  while (1) {   
    switch (ui_state) {
      /**
       * User can toggle the flashing speed of the Discovery board
       * LEDs. This demos how the timer library can be used.
       */
      case LED_DEMO:
        //check if button depressed
        if (!gpio_read_pin(GE_PBTN2)) {
          if (led_speed) {
            timer_set_period(led_timer, 500);
            led_speed = false;
          } else {
            timer_set_period(led_timer, 100);
            led_speed = true;
          }

          // wait for button to be released
          while (!gpio_read_pin(GE_PBTN2));
        }
        break;

      case PWM_DEMO:
        pwm_level = pwm_level + .05;

        if (pwm_level > 1.0) pwm_level = 0.0;

        pwm_set(PWM_CHAN1, pwm_level);
        pwm_set(PWM_CHAN2, pwm_level);
        pwm_set(PWM_CHAN3, pwm_level);
        pwm_set(PWM_CHAN4, pwm_level);
        break;

      case ADC_DEMO:
        // print results every 10 refreshes
        num_refresh++;
        if (num_refresh >= 10) {
          num_refresh = 0;
          printf("%u\t%u\t%u\t%u\n", val[0], val[1], val[2], val[3]);
          // printf("%u\t%u\n", val[0], val[1]);
        }
        break;

      case USART_DEMO: ;
        if (ge_uart_available()) {
          uint8_t c = ge_uart_get();
          printf("%c\n", c);
          lcd_putc(c);
        }
        break;

      default:
        break;
    }

    // check whether to change state
    if (!gpio_read_pin(GE_PBTN1)) {
      // stop LED timer if necessary
      if (ui_state == LED_DEMO) timer_stop(led_timer);

      ui_state++;
      if (ui_state >= NUM_STATES) ui_state = LED_DEMO;

      change_state();

      // wait for button to be released
      while (!gpio_read_pin(GE_PBTN1));
    }

    delay_ms(50);
  }
}