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