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 }
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 }
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); } }
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(); }
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 } }
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); }
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); }
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 }
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 } } }
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 } } }
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); } } }