Ejemplo n.º 1
0
void sensors_begin(bool flipped) {

    hal.console->printf("init AP_InertialSensor: ");
    if (flipped) {
        g_ahrs.set_orientation(ROTATION_ROLL_180);
    }
    g_ins.init(AP_InertialSensor::COLD_START, INS_SAMPLE_RATE, flash_leds);
    g_ins.init_accel(flash_leds);
    hal.console->println();
    led_set(0, false);
    hal.console->printf("done\r\n");

    hal.console->printf("init AP_Compass: "******"done\r\n");

    hal.console->printf("init AP_Baro: ");
    g_baro.init();
    g_baro.calibrate();
    hal.console->printf("done\r\n");

    hal.console->printf("init AP_AHRS: ");
    g_ahrs.init();
    g_ahrs.set_compass(&g_compass);
    hal.console->printf("sensors init done\r\n");

}
void main_task(void *args)
{
  vTaskSetApplicationTaskTag(xTaskGetIdleTaskHandle(), (pdTASK_HOOK_CODE)1);
  vTaskSetApplicationTaskTag(NULL, (pdTASK_HOOK_CODE)2);

  led_init();

  hal.init(0, NULL);
  hal.console->printf("AP_HAL Sensor Test\r\n");
  hal.scheduler->register_timer_failsafe(failsafe, 1000);

  hal.console->printf("init AP_InertialSensor: ");
  g_ins.init(AP_InertialSensor::COLD_START, INS_SAMPLE_RATE, flash_leds);
  g_ins.init_accel(flash_leds);
  hal.console->println();
  led_set(0, false);
  hal.console->printf("done\r\n");

  hal.console->printf("init AP_Compass: "******"done\r\n");

  hal.console->printf("init AP_Baro: ");
  g_baro.init();
  g_baro.calibrate();
  hal.console->printf("done\r\n");

  hal.console->printf("init AP_AHRS: ");
  g_ahrs.init();
  g_ahrs.set_compass(&g_compass);
  g_ahrs.set_barometer(&g_baro);
  hal.console->printf("done\r\n");

  portTickType last_print = 0;
  portTickType last_compass = 0;
  portTickType last_wake = 0;
  float heading = 0.0f;

  last_wake = xTaskGetTickCount();

  for (;;) {
    // Delay to run this loop at 100Hz.
    vTaskDelayUntil(&last_wake, 10);

    portTickType now = xTaskGetTickCount();

    if (last_compass == 0 || now - last_compass > 100) {
      last_compass = now;
      g_compass.read();
      g_baro.read();
      heading = g_compass.calculate_heading(g_ahrs.get_dcm_matrix());
    }

    g_ahrs.update();

    if (last_print == 0 || now - last_print > 100) {
      last_print = now;

      hal.console->write("\r\n");
      hal.console->printf("ahrs:    roll %4.1f  pitch %4.1f  yaw %4.1f hdg %.1f\r\n",
                          ToDeg(g_ahrs.roll), ToDeg(g_ahrs.pitch),
                          ToDeg(g_ahrs.yaw),
                          g_compass.use_for_yaw() ? ToDeg(heading) : 0.0f);

      Vector3f accel(g_ins.get_accel());
      Vector3f gyro(g_ins.get_gyro());
      hal.console->printf("mpu6000: accel %.2f %.2f %.2f  "
                          "gyro %.2f %.2f %.2f\r\n",
                          accel.x, accel.y, accel.z,
                          gyro.x, gyro.y, gyro.z);

      hal.console->printf("compass: heading %.2f deg\r\n",
                          ToDeg(g_compass.calculate_heading(0, 0)));
      g_compass.null_offsets();

      if (hal.rcin->valid()) {
        uint16_t ppm[PPM_MAX_CHANNELS];
        size_t count;

        count = hal.rcin->read(ppm, PPM_MAX_CHANNELS);

        hal.console->write("ppm:     ");
        for (size_t i = 0; i < count; ++i)
          hal.console->printf("%u ", ppm[i]);
        hal.console->write("\r\n");
      }
    }
  }
}