/** Service an external event interrupt
*
* When an event occurs (i.e. pin edge) that matches the NAP's trigger
* condition, the NAP will latch the time, pin number and trigger direction.
* It will also set an IRQ bit which will lead to an EXTI. The firmware
* EXTI handling routine handle_nap_exti() will call this function, which
* reads out the details and spits them out as an SBP message to our host.
*
*/
void ext_event_service(void)
{
u8 event_pin;
ext_event_trigger_t event_trig;
/* Read the details, and also clear IRQ + set up for next time */
u32 event_nap_time = nap_rw_ext_event(&event_pin, &event_trig, trigger);
/* We have to infer the most sig word (i.e. # of 262-second rollovers) */
union {
u32 half[2];
u64 full;
} tc;
tc.full = nap_timing_count();
if (tc.half[0] < event_nap_time) /* Rollover occurred since event */
tc.half[1]--;
tc.half[0] = event_nap_time;
/* Prepare the MSG_EXT_EVENT */
msg_ext_event_t msg;
msg.flags = (event_trig == TRIG_RISING) ? (1<<0) : (0<<0);
if (time_quality == TIME_FINE)
msg.flags |= (1 << 1);
msg.pin = event_pin;
/* Convert to the SBP convention of rounded ms + signed ns residual */
gps_time_t gpst = rx2gpstime(tc.full);
msg_gps_time_t mgt;
sbp_make_gps_time(&mgt, &gpst, 0);
msg.wn = mgt.wn;
msg.tow = mgt.tow;
msg.ns = mgt.ns;
sbp_send_msg(SBP_MSG_EXT_EVENT, sizeof(msg), (u8 *)&msg);
}
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);
);
}
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);
);
}
