Exemplo n.º 1
0
void solution_send_sbp(gnss_solution *soln, dops_t *dops, bool clock_jump)
{
  if (soln) {
    /* Send GPS_TIME message first. */
    msg_gps_time_t gps_time;
    sbp_make_gps_time(&gps_time, &soln->time, 0);
    sbp_send_msg(SBP_MSG_GPS_TIME, sizeof(gps_time), (u8 *) &gps_time);
    if (chVTTimeElapsedSinceX(last_dgnss) > DGNSS_TIMEOUT(soln_freq)) {
      /* Position in LLH. */
      msg_pos_llh_t pos_llh;
      sbp_make_pos_llh(&pos_llh, soln, 0);
      sbp_send_msg(SBP_MSG_POS_LLH, sizeof(pos_llh), (u8 *) &pos_llh);

      /* Position in ECEF. */
      msg_pos_ecef_t pos_ecef;
      sbp_make_pos_ecef(&pos_ecef, soln, 0);
      sbp_send_msg(SBP_MSG_POS_ECEF, sizeof(pos_ecef), (u8 *) &pos_ecef);
    }
    /* Velocity in NED. */
    /* Do not send if there has been a clock jump. Velocity may be unreliable.*/
    if (!clock_jump) {
      msg_vel_ned_t vel_ned;
      sbp_make_vel_ned(&vel_ned, soln, 0);
      sbp_send_msg(SBP_MSG_VEL_NED, sizeof(vel_ned), (u8 *) &vel_ned);

      /* Velocity in ECEF. */
      msg_vel_ecef_t vel_ecef;
      sbp_make_vel_ecef(&vel_ecef, soln, 0);
      sbp_send_msg(SBP_MSG_VEL_ECEF, sizeof(vel_ecef), (u8 *) &vel_ecef);
    }
  }

  if (dops) {
    DO_EVERY(10,
      msg_dops_t sbp_dops;
      sbp_make_dops(&sbp_dops, dops, &(soln->time));
      sbp_send_msg(SBP_MSG_DOPS, sizeof(msg_dops_t), (u8 *) &sbp_dops);
    );
  }
Exemplo n.º 2
0
void solution_send_sbp(gnss_solution *soln, dops_t *dops)
{
  if (soln) {
    /* Send GPS_TIME message first. */
    sbp_gps_time_t gps_time;
    sbp_make_gps_time(&gps_time, &soln->time, 0);
    sbp_send_msg(SBP_GPS_TIME, sizeof(gps_time), (u8 *) &gps_time);

    /* Position in LLH. */
    sbp_pos_llh_t pos_llh;
    sbp_make_pos_llh(&pos_llh, soln, 0);
    sbp_send_msg(SBP_POS_LLH, sizeof(pos_llh), (u8 *) &pos_llh);

    /* Position in ECEF. */
    sbp_pos_ecef_t pos_ecef;
    sbp_make_pos_ecef(&pos_ecef, soln, 0);
    sbp_send_msg(SBP_POS_ECEF, sizeof(pos_ecef), (u8 *) &pos_ecef);

    /* Velocity in NED. */
    sbp_vel_ned_t vel_ned;
    sbp_make_vel_ned(&vel_ned, soln, 0);
    sbp_send_msg(SBP_VEL_NED, sizeof(vel_ned), (u8 *) &vel_ned);

    /* Velocity in ECEF. */
    sbp_vel_ecef_t vel_ecef;
    sbp_make_vel_ecef(&vel_ecef, soln, 0);
    sbp_send_msg(SBP_VEL_ECEF, sizeof(vel_ecef), (u8 *) &vel_ecef);
  }

  if (dops) {
    DO_EVERY(10,
      sbp_dops_t sbp_dops;
      sbp_make_dops(&sbp_dops, dops);
      sbp_send_msg(SBP_DOPS, sizeof(sbp_dops_t), (u8 *) &sbp_dops);
    );
  }