Example #1
0
void actuators_init(void) {
  actuators_asctec.cmd = NONE;
  actuators_asctec.cur_addr = FRONT;
  actuators_asctec.new_addr = FRONT;
  actuators_asctec.i2c_trans.status = I2CTransSuccess;
  actuators_asctec.i2c_trans.type = I2CTransTx;
  actuators_asctec.i2c_trans.slave_addr = 0x02;
#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
  actuators_asctec.i2c_trans.len_w = 5;
#else
  actuators_asctec.i2c_trans.len_w = 4;
#endif
  actuators_asctec.nb_err = 0;

#if defined ACTUATORS_START_DELAY && ! defined SITL
  actuators_delay_done = FALSE;
  SysTimeTimerStart(actuators_delay_time);
#else
  actuators_delay_done = TRUE;
  actuators_delay_time = 0;
#endif

#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
  supervision_init();
#endif

}
Example #2
0
void airspeed_ets_init(void)
{
  int n;
  airspeed_ets_raw = 0;
  airspeed_ets = 0.0;
  airspeed_ets_offset = 0;
  airspeed_ets_offset_tmp = 0;
  airspeed_ets_i2c_done = true;
  airspeed_ets_valid = false;
  airspeed_ets_offset_init = false;
  airspeed_ets_cnt = AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT + AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG;

  airspeed_ets_buffer_idx = 0;
  for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) {
    airspeed_ets_buffer[n] = 0.0;
  }

  airspeed_ets_i2c_trans.status = I2CTransDone;

  airspeed_ets_delay_done = false;
  SysTimeTimerStart(airspeed_ets_delay_time);

#ifndef SITL
#if AIRSPEED_ETS_SDLOG
  log_airspeed_ets_started = false;
#endif
#endif
}
Example #3
0
void gps_sim_hitl_event(void)
{
  if (SysTimeTimer(gps_sim_hitl_timer) > 100000) {
    SysTimeTimerStart(gps_sim_hitl_timer);
    gps.last_msg_ticks = sys_time.nb_sec_rem;
    gps.last_msg_time = sys_time.nb_sec;
    if (state.ned_initialized_i) {
      if (!autopilot_in_flight) {
        struct Int32Vect2 zero_vector;
        INT_VECT2_ZERO(zero_vector);
        gh_set_ref(zero_vector, zero_vector, zero_vector);
        INT_VECT2_ZERO(guidance_h.ref.pos);
        INT_VECT2_ZERO(guidance_h.ref.speed);
        INT_VECT2_ZERO(guidance_h.ref.accel);
        gv_set_ref(0, 0, 0);
        guidance_v_z_ref = 0;
        guidance_v_zd_ref = 0;
        guidance_v_zdd_ref = 0;
      }
      struct NedCoor_i ned_c;
      ned_c.x = guidance_h.ref.pos.x * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
      ned_c.y = guidance_h.ref.pos.y * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
      ned_c.z = guidance_v_z_ref * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
      ecef_of_ned_point_i(&gps.ecef_pos, &state.ned_origin_i, &ned_c);
      gps.lla_pos.alt = state.ned_origin_i.lla.alt - ned_c.z;
      gps.hmsl = state.ned_origin_i.hmsl - ned_c.z;
      ned_c.x = guidance_h.ref.speed.x * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
      ned_c.y = guidance_h.ref.speed.y * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
      ned_c.z = guidance_v_zd_ref * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
      ecef_of_ned_vect_i(&gps.ecef_vel, &state.ned_origin_i, &ned_c);
      gps.fix = GPS_FIX_3D;
      gps.last_3dfix_ticks = sys_time.nb_sec_rem;
      gps.last_3dfix_time = sys_time.nb_sec;
      gps_available = true;
    }
    else {
      struct Int32Vect2 zero_vector;
      INT_VECT2_ZERO(zero_vector);
      gh_set_ref(zero_vector, zero_vector, zero_vector);
      gv_set_ref(0, 0, 0);
    }

    // publish gps data
    uint32_t now_ts = get_sys_time_usec();
    gps.last_msg_ticks = sys_time.nb_sec_rem;
    gps.last_msg_time = sys_time.nb_sec;
    if (gps.fix == GPS_FIX_3D) {
      gps.last_3dfix_ticks = sys_time.nb_sec_rem;
      gps.last_3dfix_time = sys_time.nb_sec;
    }
    AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps);
  }
}
Example #4
0
void actuators_init(void)
{

#if defined ACTUATORS_START_DELAY && ! defined SITL
  actuators_delay_done = FALSE;
  SysTimeTimerStart(actuators_delay_time);
#else
  actuators_delay_done = TRUE;
  actuators_delay_time = 0;
#endif

  // Init macro from generated airframe.h
  AllActuatorsInit();

}
Example #5
0
void i2c_event(void) {
  static uint32_t i2c_wd_timer;

  if (SysTimeTimer(i2c_wd_timer) > 2000) { // 2ms (500Hz) periodic watchdog check
    SysTimeTimerStart(i2c_wd_timer);
#if USE_I2C1
    i2c_wd_check(&i2c1);
#endif

#if USE_I2C2
    i2c_wd_check(&i2c2);
#endif
#if USE_I2C3
    i2c_wd_check(&i2c3);
#endif
  }
}
Example #6
0
void baro_ets_init( void ) {
  baro_ets_adc = 0;
  baro_ets_altitude = 0.0;
  baro_ets_offset = 0;
  baro_ets_offset_tmp = 0;
  baro_ets_valid = FALSE;
  baro_ets_offset_init = FALSE;
  baro_ets_enabled = TRUE;
  baro_ets_cnt = BARO_ETS_OFFSET_NBSAMPLES_INIT + BARO_ETS_OFFSET_NBSAMPLES_AVRG;
  baro_ets_r = BARO_ETS_R;
  baro_ets_sigma2 = BARO_ETS_SIGMA2;

  baro_ets_i2c_trans.status = I2CTransDone;

  baro_ets_delay_done = FALSE;
  SysTimeTimerStart(baro_ets_delay_time);
}
Example #7
0
void airspeed_ets_init( void ) {
  int n;
  airspeed_ets_raw = 0;
  airspeed_ets = 0.0;
  airspeed_ets_offset = 0;
  airspeed_ets_offset_tmp = 0;
  airspeed_ets_i2c_done = TRUE;
  airspeed_ets_valid = FALSE;
  airspeed_ets_offset_init = FALSE;
  airspeed_ets_cnt = AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT + AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG;

  airspeed_ets_buffer_idx = 0;
  for (n=0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n)
    airspeed_ets_buffer[n] = 0.0;

  airspeed_ets_i2c_trans.status = I2CTransDone;

  airspeed_ets_delay_done = FALSE;
  SysTimeTimerStart(airspeed_ets_delay_time);
}
Example #8
0
void actuators_init(void) {

  supervision_init();
  const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR;
  for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++) {
    actuators_mkk.trans[i].type = I2CTransTx;
    actuators_mkk.trans[i].len_w = 1;
    actuators_mkk.trans[i].slave_addr = actuators_addr[i];
    actuators_mkk.trans[i].status = I2CTransSuccess;
  }

#if defined ACTUATORS_START_DELAY && ! defined SITL
  actuators_delay_done = FALSE;
  SysTimeTimerStart(actuators_delay_time);
#else
  actuators_delay_done = TRUE;
  actuators_delay_time = 0;
#endif

}
void actuators_init(void) {

  supervision_init();
  actuators_skiron.trans.type = I2CTransTx;
  actuators_skiron.trans.len_w = ACTUATORS_SKIRON_NB;
  actuators_skiron.trans.slave_addr = ACTUATORS_SKIRON_I2C_ADDR;
  actuators_skiron.trans.status = I2CTransDone;
  const uint8_t actuators_idx[ACTUATORS_SKIRON_NB] = ACTUATORS_SKIRON_IDX;
  for (uint8_t i=0; i<ACTUATORS_SKIRON_NB; i++) {
    actuators_skiron.actuators_idx[i] = actuators_idx[i];
  }

#if defined ACTUATORS_START_DELAY && ! defined SITL
  actuators_delay_done = FALSE;
  SysTimeTimerStart(actuators_delay_time);
#else
  actuators_delay_done = TRUE;
  actuators_delay_time = 0;
#endif

}
Example #10
0
void gps_mtk_read_message(void) {
  if (gps_mtk.msg_class == MTK_DIY14_ID) {
    if (gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
#ifdef GPS_TIMESTAMP
      /* get hardware clock ticks */
      SysTimeTimerStart(gps.t0);
      gps.t0_tow      = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);
      gps.t0_tow_frac = 0;
#endif
      gps.lla_pos.lat = RadOfDeg(MTK_DIY14_NAV_LAT(gps_mtk.msg_buf))*10;
      gps.lla_pos.lon = RadOfDeg(MTK_DIY14_NAV_LON(gps_mtk.msg_buf))*10;
      // FIXME: with MTK you do not receive vertical speed
      if (cpu_time_sec - gps.last_fix_time < 2) {
        gps.ned_vel.z  = ((gps.hmsl -
            MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10;
      } else gps.ned_vel.z = 0;
      gps.hmsl        = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10;
      // FIXME: with MTK you do not receive ellipsoid altitude
      gps.lla_pos.alt = gps.hmsl;
      gps.gspeed      = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf);
      // FIXME: with MTK you do not receive speed 3D
      gps.speed_3d    = gps.gspeed;
      gps.course      = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf)))*10;
      gps.num_sv      = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf);
      switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) {
      case MTK_DIY_FIX_3D:
        gps.fix = GPS_FIX_3D;
        break;
      case MTK_DIY_FIX_2D:
        gps.fix = GPS_FIX_2D;
        break;
      default:
        gps.fix = GPS_FIX_NONE;
      }
      gps.tow         = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);;
      // FIXME: with MTK DIY 1.4 you do not receive GPS week
      gps.week        = 0;
      /* Computes from (lat, long) in the referenced UTM zone */
      struct LlaCoor_f lla_f;
      lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
      lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
      struct UtmCoor_f utm_f;
      utm_f.zone = nav_utm_zone0;
      /* convert to utm */
      utm_of_lla_f(&utm_f, &lla_f);
      /* copy results of utm conversion */
      gps.utm_pos.east = utm_f.east*100;
      gps.utm_pos.north = utm_f.north*100;
      gps.utm_pos.alt = utm_f.alt*1000;
      gps.utm_pos.zone = nav_utm_zone0;
#ifdef GPS_LED
      if (gps.fix == GPS_FIX_3D) {
        LED_ON(GPS_LED);
      }
      else {
        LED_TOGGLE(GPS_LED);
      }
#endif
    }
  }

  if (gps_mtk.msg_class == MTK_DIY16_ID) {
    if (gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
      uint32_t gps_date, gps_time;
      gps_date = MTK_DIY16_NAV_UTC_DATE(gps_mtk.msg_buf);
      gps_time = MTK_DIY16_NAV_UTC_TIME(gps_mtk.msg_buf);
      gps_mtk_time2itow(gps_date, gps_time, &gps.week, &gps.tow);
#ifdef GPS_TIMESTAMP
      /* get hardware clock ticks */
      SysTimeTimerStart(gps.t0);
      gps.t0_tow      = gps.tow;
      gps.t0_tow_frac = 0;
#endif
      gps.lla_pos.lat = RadOfDeg(MTK_DIY16_NAV_LAT(gps_mtk.msg_buf))*10;
      gps.lla_pos.lon = RadOfDeg(MTK_DIY16_NAV_LON(gps_mtk.msg_buf))*10;
      // FIXME: with MTK you do not receive vertical speed
      if (cpu_time_sec - gps.last_fix_time < 2) {
        gps.ned_vel.z  = ((gps.hmsl -
            MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10;
      } else gps.ned_vel.z = 0;
      gps.hmsl        = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10;
      // FIXME: with MTK you do not receive ellipsoid altitude
      gps.lla_pos.alt = gps.hmsl;
      gps.gspeed      = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf);
      // FIXME: with MTK you do not receive speed 3D
      gps.speed_3d    = gps.gspeed;
      gps.course      = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf)*10000)) * 10;
      gps.num_sv      = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf);
      switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) {
      case MTK_DIY_FIX_3D:
        gps.fix = GPS_FIX_3D;
        break;
      case MTK_DIY_FIX_2D:
        gps.fix = GPS_FIX_2D;
        break;
      default:
        gps.fix = GPS_FIX_NONE;
      }
      /* HDOP? */
      /* Computes from (lat, long) in the referenced UTM zone */
      struct LlaCoor_f lla_f;
      lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
      lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
      struct UtmCoor_f utm_f;
      utm_f.zone = nav_utm_zone0;
      /* convert to utm */
      utm_of_lla_f(&utm_f, &lla_f);
      /* copy results of utm conversion */
      gps.utm_pos.east = utm_f.east*100;
      gps.utm_pos.north = utm_f.north*100;
      gps.utm_pos.alt = utm_f.alt*1000;
      gps.utm_pos.zone = nav_utm_zone0;
#ifdef GPS_LED
      if (gps.fix == GPS_FIX_3D) {
        LED_ON(GPS_LED);
      }
      else {
        LED_TOGGLE(GPS_LED);
      }
#endif
    }
  }
}
Example #11
0
void gps_mtk_read_message(void)
{
  if (gps_mtk.msg_class == MTK_DIY14_ID) {
    if (gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
      /* get hardware clock ticks */
      gps_time_sync.t0_ticks      = sys_time.nb_tick;
      gps_time_sync.t0_tow      = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);
      gps_time_sync.t0_tow_frac = 0;
      gps.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf) * 10;
      gps.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf) * 10;
      SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
      // FIXME: with MTK you do not receive vertical speed
      if (sys_time.nb_sec - gps.last_3dfix_time < 2) {
        gps.ned_vel.z  = ((gps.hmsl -
                           MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10;
      } else { gps.ned_vel.z = 0; }
      gps.hmsl        = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10;
      SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
      // FIXME: with MTK you do not receive ellipsoid altitude
      gps.lla_pos.alt = gps.hmsl;
      gps.gspeed      = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf);
      // FIXME: with MTK you do not receive speed 3D
      gps.speed_3d    = gps.gspeed;
      gps.course      = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf))) * 10;
      SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
      gps.num_sv      = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf);
      switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) {
        case MTK_DIY_FIX_3D:
          gps.fix = GPS_FIX_3D;
          break;
        case MTK_DIY_FIX_2D:
          gps.fix = GPS_FIX_2D;
          break;
        default:
          gps.fix = GPS_FIX_NONE;
      }
      gps.tow         = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);;
      // FIXME: with MTK DIY 1.4 you do not receive GPS week
      gps.week        = 0;
#ifdef GPS_LED
      if (gps.fix == GPS_FIX_3D) {
        LED_ON(GPS_LED);
      } else {
        LED_TOGGLE(GPS_LED);
      }
#endif
    }
  }

  if (gps_mtk.msg_class == MTK_DIY16_ID) {
    if (gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
      uint32_t gps_date, gps_time;
      gps_date = MTK_DIY16_NAV_UTC_DATE(gps_mtk.msg_buf);
      gps_time = MTK_DIY16_NAV_UTC_TIME(gps_mtk.msg_buf);
      gps_mtk_time2itow(gps_date, gps_time, &gps.week, &gps.tow);
#ifdef GPS_TIMESTAMP
      /* get hardware clock ticks */
      SysTimeTimerStart(gps.t0);
      gps.t0_tow      = gps.tow;
      gps.t0_tow_frac = 0;
#endif
      gps.lla_pos.lat = MTK_DIY16_NAV_LAT(gps_mtk.msg_buf) * 10;
      gps.lla_pos.lon = MTK_DIY16_NAV_LON(gps_mtk.msg_buf) * 10;
      // FIXME: with MTK you do not receive vertical speed
      if (sys_time.nb_sec - gps.last_3dfix_time < 2) {
        gps.ned_vel.z  = ((gps.hmsl -
                           MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10;
      } else { gps.ned_vel.z = 0; }
      gps.hmsl        = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10;
      SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
      // FIXME: with MTK you do not receive ellipsoid altitude
      gps.lla_pos.alt = gps.hmsl;
      gps.gspeed      = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf);
      // FIXME: with MTK you do not receive speed 3D
      gps.speed_3d    = gps.gspeed;
      gps.course      = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf) * 10000)) * 10;
      SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
      gps.num_sv      = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf);
      switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) {
        case MTK_DIY_FIX_3D:
          gps.fix = GPS_FIX_3D;
          break;
        case MTK_DIY_FIX_2D:
          gps.fix = GPS_FIX_2D;
          break;
        default:
          gps.fix = GPS_FIX_NONE;
      }
      /* HDOP? */
#ifdef GPS_LED
      if (gps.fix == GPS_FIX_3D) {
        LED_ON(GPS_LED);
      } else {
        LED_TOGGLE(GPS_LED);
      }
#endif
    }
  }
}
Example #12
0
void gps_ubx_read_message(void) {

  if (gps_ubx.msg_class == UBX_NAV_ID) {
    if (gps_ubx.msg_id == UBX_NAV_SOL_ID) {
#ifdef GPS_TIMESTAMP
      /* get hardware clock ticks */
      SysTimeTimerStart(gps.t0);
      gps.t0_tow        = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
      gps.t0_tow_frac   = UBX_NAV_SOL_Frac(gps_ubx.msg_buf);
#endif
      gps.tow        = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
      gps.week       = UBX_NAV_SOL_week(gps_ubx.msg_buf);
      gps.fix        = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf);
      gps.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf);
      gps.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf);
      gps.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf);
      gps.pacc       = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf);
      gps.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf);
      gps.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf);
      gps.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf);
      gps.sacc       = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf);
      gps.pdop       = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf);
      gps.num_sv     = UBX_NAV_SOL_numSV(gps_ubx.msg_buf);
#ifdef GPS_LED
      if (gps.fix == GPS_FIX_3D) {
        LED_ON(GPS_LED);
      }
      else {
        LED_TOGGLE(GPS_LED);
      }
#endif
    } else if (gps_ubx.msg_id == UBX_NAV_POSLLH_ID) {
      gps.lla_pos.lat = RadOfDeg(UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf));
      gps.lla_pos.lon = RadOfDeg(UBX_NAV_POSLLH_LON(gps_ubx.msg_buf));
      gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf);
      gps.hmsl        = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf);
#ifdef GPS_USE_LATLONG
      /* Computes from (lat, long) in the referenced UTM zone */
      struct LlaCoor_f lla_f;
      lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
      lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
      struct UtmCoor_f utm_f;
      utm_f.zone = nav_utm_zone0;
      /* convert to utm */
      utm_of_lla_f(&utm_f, &lla_f);
      /* copy results of utm conversion */
      gps.utm_pos.east = utm_f.east*100;
      gps.utm_pos.north = utm_f.north*100;
      gps.utm_pos.alt = utm_f.alt*1000;
      gps.utm_pos.zone = nav_utm_zone0;
#else
    }
    else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) {
      gps.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf);
      gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf);
      uint8_t hem = UBX_NAV_POSUTM_HEM(gps_ubx.msg_buf);
      if (hem == UTM_HEM_SOUTH)
        gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */
      gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf)*10;
      gps.hmsl = gps.utm_pos.alt;
      gps.lla_pos.alt = gps.utm_pos.alt; // FIXME: with UTM only you do not receive ellipsoid altitude
      gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf);
#endif
    }
    else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) {
      gps.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf);
      gps.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf);
      gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf);
      gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf);
      gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf);
      // Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180)
      // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg
      // solution: First to radians, and then scale to 1e-7 radians
      // First x 10 for loosing less resolution, then to radians, then multiply x 10 again
      gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*10)) * 10;
      gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf);
      gps_ubx.have_velned = 1;
    }
    else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) {
      gps.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS);
      uint8_t i;
      for(i = 0; i < gps.nb_channels; i++) {
        gps.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i);
        gps.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i);
        gps.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i);
        gps.svinfos[i].cno = UBX_NAV_SVINFO_CNO(gps_ubx.msg_buf, i);
        gps.svinfos[i].elev = UBX_NAV_SVINFO_Elev(gps_ubx.msg_buf, i);
        gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i);
      }
    }
    else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) {
      gps.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf);
      gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf);
      gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf);
    }
  }
}