int main(void)
{
main_init();
#if LIMIT_EVENT_POLLING
/* Limit main loop frequency to 1kHz.
* This is a kludge until we can better leverage threads and have real events.
* Without this limit the event flags will constantly polled as fast as possible,
* resulting on 100% cpu load on boards with an (RT)OS.
* On bare metal boards this is not an issue, as you have nothing else running anyway.
*/
uint32_t t_begin = 0;
uint32_t t_diff = 0;
while (1) {
t_begin = get_sys_time_usec();
handle_periodic_tasks();
main_event();
/* sleep remaining time to limit to 1kHz */
t_diff = get_sys_time_usec() - t_begin;
if (t_diff < 1000) {
sys_time_usleep(1000 - t_diff);
}
}
#else
while (1) {
handle_periodic_tasks();
main_event();
}
#endif
return 0;
}
static void gps_cb(uint8_t sender_id,
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{
/* ignore callback from own AbiSendMsgGPS */
if (sender_id == GPS_MULTI_ID) {
return;
}
uint32_t now_ts = get_sys_time_usec();
#ifdef SECONDARY_GPS
current_gps_id = gps_multi_switch(gps_s);
if (gps_s->comp_id == current_gps_id) {
gps = *gps_s;
AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s);
}
#else
gps = *gps_s;
AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s);
#endif
if (gps.tow != gps_time_sync.t0_tow)
{
gps_time_sync.t0_ticks = sys_time.nb_tick;
gps_time_sync.t0_tow = gps.tow;
}
}
/**
* Update the optical flow state for the calculation thread
* and update the stabilization loops with the newest result
*/
void opticflow_module_run(void)
{
pthread_mutex_lock(&opticflow_mutex);
// Update the stabilization loops on the current calculation
if (opticflow_got_result) {
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgOPTICAL_FLOW(OPTICFLOW_SENDER_ID, now_ts,
opticflow_result.flow_x,
opticflow_result.flow_y,
opticflow_result.flow_der_x,
opticflow_result.flow_der_y,
opticflow_result.noise_measurement,// FIXME, scale to some quality measure 0-255
opticflow_result.div_size,
opticflow_state.agl);
//TODO Find an appropiate quality measure for the noise model in the state filter, for now it is tracked_cnt
if (opticflow_result.noise_measurement < 0.8) {
AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
opticflow_result.vel_body_x,
opticflow_result.vel_body_y,
0.0f,
opticflow_result.noise_measurement
);
}
opticflow_got_result = false;
}
pthread_mutex_unlock(&opticflow_mutex);
}
void mag_hmc58xx_module_event(void)
{
hmc58xx_event(&mag_hmc58xx);
if (mag_hmc58xx.data_available) {
#if MODULE_HMC58XX_UPDATE_AHRS
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// set channel order
struct Int32Vect3 mag = {
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
};
// unscaled vector
VECT3_COPY(imu.mag_unscaled, mag);
// scale vector
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, now_ts, &imu.mag);
#endif
#if MODULE_HMC58XX_SYNC_SEND
mag_hmc58xx_report();
#endif
#if MODULE_HMC58XX_UPDATE_AHRS || MODULE_HMC58XX_SYNC_SEND
mag_hmc58xx.data_available = FALSE;
#endif
}
}
void apogee_baro_event(void)
{
mpl3115_event(&apogee_baro);
if (apogee_baro.data_available && startup_cnt == 0) {
uint32_t now_ts = get_sys_time_usec();
float pressure = ((float)apogee_baro.pressure / (1 << 2));
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
float temp = apogee_baro.temperature / 16.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
apogee_baro.data_available = false;
#if APOGEE_BARO_SDLOG
if (pprzLogFile != -1) {
if (!log_apogee_baro_started) {
sdLogWriteLog(pprzLogFile, "APOGEE_BARO: Pres(Pa) Temp(degC) GPS_fix TOW(ms) Week Lat(1e7deg) Lon(1e7deg) HMSL(mm) gpseed(cm/s) course(1e7deg) climb(cm/s)\n");
log_apogee_baro_started = true;
}
sdLogWriteLog(pprzLogFile, "apogee_baro: %9.4f %9.4f %d %d %d %d %d %d %d %d %d\n",
pressure,temp,
gps.fix, gps.tow, gps.week,
gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl,
gps.gspeed, gps.course, -gps.ned_vel.z);
}
#endif
}
}
void imu_hbmini_event(void)
{
uint32_t now_ts = get_sys_time_usec();
max1168_event();
if (max1168_status == MAX1168_DATA_AVAILABLE) {
imu.gyro_unscaled.p = max1168_values[IMU_GYRO_P_CHAN];
imu.gyro_unscaled.q = max1168_values[IMU_GYRO_Q_CHAN];
imu.gyro_unscaled.r = max1168_values[IMU_GYRO_R_CHAN];
imu.accel_unscaled.x = max1168_values[IMU_ACCEL_X_CHAN];
imu.accel_unscaled.y = max1168_values[IMU_ACCEL_Y_CHAN];
imu.accel_unscaled.z = max1168_values[IMU_ACCEL_Z_CHAN];
max1168_status = MAX1168_IDLE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
// HMC58XX event task
hmc58xx_event(&imu_hbmini.hmc);
if (imu_hbmini.hmc.data_available) {
imu.mag_unscaled.z = imu_hbmini.hmc.data.value[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = imu_hbmini.hmc.data.value[IMU_MAG_Y_CHAN];
imu.mag_unscaled.x = imu_hbmini.hmc.data.value[IMU_MAG_Z_CHAN];
imu_hbmini.hmc.data_available = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
}
}
void imu_krooz_event(void)
{
if (imu_krooz.mpu_eoc) {
mpu60x0_i2c_read(&imu_krooz.mpu);
imu_krooz.mpu_eoc = false;
}
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
imu_krooz.meas_nb++;
imu_krooz.mpu.data_available = false;
}
if (imu_krooz.hmc_eoc) {
hmc58xx_read(&imu_krooz.hmc);
imu_krooz.hmc_eoc = false;
}
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
imu_krooz.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, get_sys_time_usec(), &imu.mag);
}
}
void gps_mtk_msg(void)
{
// current timestamp
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;
gps_mtk_read_message();
if (gps_mtk.msg_class == MTK_DIY14_ID &&
gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
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_MTK_ID, now_ts, &gps);
}
if (gps_mtk.msg_class == MTK_DIY16_ID &&
gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
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_MTK_ID, now_ts, &gps);
}
gps_mtk.msg_available = FALSE;
}
void imu_px4fmu_event(void)
{
uint32_t now_ts = get_sys_time_usec();
mpu60x0_spi_event(&imu_px4fmu.mpu);
if (imu_px4fmu.mpu.data_available) {
RATES_ASSIGN(imu.gyro_unscaled,
imu_px4fmu.mpu.data_rates.rates.q,
imu_px4fmu.mpu.data_rates.rates.p,
-imu_px4fmu.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
imu_px4fmu.mpu.data_accel.vect.y,
imu_px4fmu.mpu.data_accel.vect.x,
-imu_px4fmu.mpu.data_accel.vect.z);
imu_px4fmu.mpu.data_available = FALSE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
hmc58xx_event(&imu_px4fmu.hmc);
if (imu_px4fmu.hmc.data_available) {
imu.mag_unscaled.x = imu_px4fmu.hmc.data.vect.y;
imu.mag_unscaled.y = imu_px4fmu.hmc.data.vect.x;
imu.mag_unscaled.z = -imu_px4fmu.hmc.data.vect.z;
imu_px4fmu.hmc.data_available = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
}
}
/**
* Handle all the events of the Navstik IMU components.
* When there is data available convert it to the correct axis and save it in the imu structure.
*/
void imu_navstik_event(void)
{
uint32_t now_ts = get_sys_time_usec();
/* MPU-60x0 event taks */
mpu60x0_i2c_event(&imu_navstik.mpu);
if (imu_navstik.mpu.data_available) {
/* default orientation as should be printed on the pcb, z-down, ICs down */
RATES_COPY(imu.gyro_unscaled, imu_navstik.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_navstik.mpu.data_accel.vect);
imu_navstik.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
hmc58xx_event(&imu_navstik.hmc);
if (imu_navstik.hmc.data_available) {
imu.mag_unscaled.x = imu_navstik.hmc.data.vect.y;
imu.mag_unscaled.y = -imu_navstik.hmc.data.vect.x;
imu.mag_unscaled.z = imu_navstik.hmc.data.vect.z;
imu_navstik.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
}
}
void imu_mpu_hmc_event(void)
{
uint32_t now_ts = get_sys_time_usec();
mpu60x0_spi_event(&imu_mpu_hmc.mpu);
if (imu_mpu_hmc.mpu.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_mpu_hmc.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_mpu_hmc.mpu.data_accel.vect);
imu_mpu_hmc.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
hmc58xx_event(&imu_mpu_hmc.hmc);
if (imu_mpu_hmc.hmc.data_available) {
/* mag rotated by 90deg around z axis relative to MPU */
imu.mag_unscaled.x = imu_mpu_hmc.hmc.data.vect.y;
imu.mag_unscaled.y = -imu_mpu_hmc.hmc.data.vect.x;
imu.mag_unscaled.z = imu_mpu_hmc.hmc.data.vect.z;
imu_mpu_hmc.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
}
}
/**
* Update the optical flow state for the calculation thread
* and update the stabilization loops with the newest result
*/
void opticflow_module_run(void)
{
// Send Updated data to thread
pthread_mutex_lock(&opticflow_mutex);
opticflow_state.phi = stateGetNedToBodyEulers_f()->phi;
opticflow_state.theta = stateGetNedToBodyEulers_f()->theta;
// Update the stabilization loops on the current calculation
if (opticflow_got_result) {
uint32_t now_ts = get_sys_time_usec();
uint8_t quality = opticflow_result.divergence; // FIXME, scale to some quality measure 0-255
AbiSendMsgOPTICAL_FLOW(OPTICFLOW_SENDER_ID, now_ts,
opticflow_result.flow_x,
opticflow_result.flow_y,
opticflow_result.flow_der_x,
opticflow_result.flow_der_y,
quality,
opticflow_result.div_size,
opticflow_state.agl);
//TODO Find an appropiate quality measure for the noise model in the state filter, for now it is tracked_cnt
if (opticflow_result.tracked_cnt > 0) {
AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
opticflow_result.vel_body_x,
opticflow_result.vel_body_y,
0.0f,
opticflow_result.noise_measurement
);
}
opticflow_got_result = false;
}
pthread_mutex_unlock(&opticflow_mutex);
}
/* Parse the InterMCU message */
static inline void mag_pitot_parse_msg(void)
{
uint32_t now_ts = get_sys_time_usec();
/* Parse the mag-pitot message */
uint8_t msg_id = pprzlink_get_msg_id(mp_msg_buf);
#if PPRZLINK_DEFAULT_VER == 2
// Check that the message is really a intermcu message
if (pprzlink_get_msg_class_id(mp_msg_buf) == DL_intermcu_CLASS_ID) {
#endif
switch (msg_id) {
/* Got a magneto message */
case DL_IMCU_REMOTE_MAG: {
struct Int32Vect3 raw_mag;
// Read the raw magneto information
raw_mag.x = DL_IMCU_REMOTE_MAG_mag_x(mp_msg_buf);
raw_mag.y = DL_IMCU_REMOTE_MAG_mag_y(mp_msg_buf);
raw_mag.z = DL_IMCU_REMOTE_MAG_mag_z(mp_msg_buf);
// Rotate the magneto
struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&mag_pitot.imu_to_mag);
int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &raw_mag);
// Send and set the magneto IMU data
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MAG_PITOT_ID, now_ts, &imu.mag);
break;
}
/* Got a barometer message */
case DL_IMCU_REMOTE_BARO: {
float pitot_stat = DL_IMCU_REMOTE_BARO_pitot_stat(mp_msg_buf);
float pitot_temp = DL_IMCU_REMOTE_BARO_pitot_temp(mp_msg_buf);
AbiSendMsgBARO_ABS(IMU_MAG_PITOT_ID, pitot_stat);
AbiSendMsgTEMPERATURE(IMU_MAG_PITOT_ID, pitot_temp);
break;
}
/* Got an airspeed message */
case DL_IMCU_REMOTE_AIRSPEED: {
// Should be updated to differential pressure
float pitot_ias = DL_IMCU_REMOTE_AIRSPEED_pitot_IAS(mp_msg_buf);
AbiSendMsgAIRSPEED(IMU_MAG_PITOT_ID, pitot_ias);
break;
}
default:
break;
}
#if PPRZLINK_DEFAULT_VER == 2
}
#endif
}
void gps_parse(void)
{
if (gps_network == NULL) { return; }
//Read from the network
int size = network_read(gps_network, &gps_udp_read_buffer[0], 256);
if (size > 0) {
// Correct MSG
if ((size == GPS_UDP_MSG_LEN) && (gps_udp_read_buffer[0] == STX)) {
gps.lla_pos.lat = UDP_GPS_INT(gps_udp_read_buffer + 4);
gps.lla_pos.lon = UDP_GPS_INT(gps_udp_read_buffer + 8);
gps.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer + 12);
gps.hmsl = UDP_GPS_INT(gps_udp_read_buffer + 16);
gps.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer + 20);
gps.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer + 24);
gps.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer + 28);
gps.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer + 32);
gps.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer + 36);
gps.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer + 40);
gps.fix = GPS_FIX_3D;
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
struct LlaCoor_f lla_f;
LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
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 = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
// publish new 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_UDP_ID, now_ts, &gps);
} else {
printf("gps_udp error: msg len invalid %d bytes\n", size);
}
memset(&gps_udp_read_buffer[0], 0, sizeof(gps_udp_read_buffer));
}
}
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
{
uint8_t mde = 3;
uint16_t val = 0;
if (!ahrs_dcm.is_aligned) { mde = 2; }
uint32_t t_diff = get_sys_time_usec() - ahrs_dcm_last_stamp;
/* set lost if no new gyro measurements for 50ms */
if (t_diff > 50000) { mde = 5; }
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_dcm_id, &mde, &val);
}
void gps_sim_publish(void)
{
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);
}
/**
* Propagate optical flow information
*/
static inline void px4flow_i2c_frame_cb(void)
{
static float quality = 0;
static float noise = 0;
uint32_t now_ts = get_sys_time_usec();
quality = ((float)px4flow.i2c_frame.qual) / 255.0;
noise = px4flow.stddev + (1 - quality) * px4flow.stddev * 10;
noise = noise * noise; // square the noise to get variance of the measurement
static float timestamp = 0;
static uint32_t time_usec = 0;
static float flow_comp_m_x = 0.0;
static float flow_comp_m_y = 0.0;
if (quality > PX4FLOW_QUALITY_THRESHOLD) {
timestamp = get_sys_time_float();
time_usec = (uint32_t)(timestamp * 1e6);
flow_comp_m_x = ((float)px4flow.i2c_frame.flow_comp_m_x) / 1000.0;
flow_comp_m_y = ((float)px4flow.i2c_frame.flow_comp_m_y) / 1000.0;
// flip the axis (if the PX4FLOW is mounted as shown in
// https://pixhawk.org/modules/px4flow
AbiSendMsgVELOCITY_ESTIMATE(VEL_PX4FLOW_ID,
time_usec,
flow_comp_m_y,
flow_comp_m_x,
0.0f,
noise,
noise,
-1.f);
}
// distance is always positive - use median filter to remove outliers
static int32_t ground_distance = 0;
static float ground_distance_float = 0.0;
// update filter
ground_distance = update_median_filter_i(&sonar_filter, (int32_t)px4flow.i2c_frame.ground_distance);
ground_distance_float = ((float)ground_distance) / 1000.0;
// compensate AGL measurement for body rotation
if (px4flow.compensate_rotation) {
float phi = stateGetNedToBodyEulers_f()->phi;
float theta = stateGetNedToBodyEulers_f()->theta;
float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
ground_distance_float = ground_distance_float / gain;
}
if (px4flow.update_agl) {
AbiSendMsgAGL(AGL_SONAR_PX4FLOW_ID, now_ts, ground_distance_float);
}
}
static void gps_xsens_publish(void)
{
// 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_XSENS_ID, now_ts, &gps);
}
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 imu_mpu_spi_event(void)
{
mpu60x0_spi_event(&imu_mpu_spi.mpu);
if (imu_mpu_spi.mpu.data_available) {
uint32_t now_ts = get_sys_time_usec();
RATES_COPY(imu.gyro_unscaled, imu_mpu_spi.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_mpu_spi.mpu.data_accel.vect);
imu_mpu_spi.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_ID, now_ts, &imu.accel);
}
}
/** Parse the REMOTE_GPS datalink packet */
void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon,
int32_t alt,
int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course)
{
gps.lla_pos.lat = lat;
gps.lla_pos.lon = lon;
gps.lla_pos.alt = alt;
gps.hmsl = hmsl;
gps.ecef_pos.x = ecef_x;
gps.ecef_pos.y = ecef_y;
gps.ecef_pos.z = ecef_z;
gps.ecef_vel.x = ecef_xd;
gps.ecef_vel.y = ecef_yd;
gps.ecef_vel.z = ecef_zd;
gps.course = course;
gps.num_sv = numsv;
gps.tow = tow;
gps.fix = GPS_FIX_3D;
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
struct LlaCoor_f lla_f;
LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
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 = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
// publish new 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_DATALINK_ID, now_ts, &gps);
}
void imu_mpu9250_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the MPU9250 I2C transaction has succeeded: convert the data
mpu9250_i2c_event(&imu_mpu9250.mpu);
if (imu_mpu9250.mpu.data_available) {
// set channel order
struct Int32Vect3 accel = {
IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]),
IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z])
};
struct Int32Rates rates = {
IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]),
IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
};
// unscaled vector
VECT3_COPY(imu.accel_unscaled, accel);
RATES_COPY(imu.gyro_unscaled, rates);
imu_mpu9250.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel);
}
#if IMU_MPU9250_READ_MAG
// Test if mag data are updated
if (imu_mpu9250.mpu.akm.data_available) {
struct Int32Vect3 mag = {
IMU_MPU9250_X_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]),
-IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z])
};
VECT3_COPY(imu.mag_unscaled, mag);
imu_mpu9250.mpu.akm.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);
}
#endif
}
void nmea_gps_msg(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
gps_nmea.state.last_msg_ticks = sys_time.nb_sec_rem;
gps_nmea.state.last_msg_time = sys_time.nb_sec;
nmea_parse_msg();
if (gps_nmea.pos_available) {
if (gps_nmea.state.fix == GPS_FIX_3D) {
gps_nmea.state.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_nmea.state.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_NMEA_ID, now_ts, &gps);
}
gps_nmea.msg_available = FALSE;
}
void gps_udp_parse(void)
{
if (gps_network == NULL) { return; }
//Read from the network
int size = network_read(gps_network, &gps_udp_read_buffer[0], 256);
if (size > 0) {
// Correct MSG
if ((size == GPS_UDP_MSG_LEN) && (gps_udp_read_buffer[0] == STX)) {
gps_udp.lla_pos.lat = UDP_GPS_INT(gps_udp_read_buffer + 4);
gps_udp.lla_pos.lon = UDP_GPS_INT(gps_udp_read_buffer + 8);
gps_udp.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer + 12);
SetBit(gps_udp.valid_fields, GPS_VALID_POS_LLA_BIT);
gps_udp.hmsl = UDP_GPS_INT(gps_udp_read_buffer + 16);
SetBit(gps_udp.valid_fields, GPS_VALID_HMSL_BIT);
gps_udp.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer + 20);
gps_udp.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer + 24);
gps_udp.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer + 28);
SetBit(gps_udp.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps_udp.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer + 32);
gps_udp.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer + 36);
gps_udp.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer + 40);
SetBit(gps_udp.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps_udp.fix = GPS_FIX_3D;
// publish new GPS data
uint32_t now_ts = get_sys_time_usec();
gps_udp.last_msg_ticks = sys_time.nb_sec_rem;
gps_udp.last_msg_time = sys_time.nb_sec;
if (gps_udp.fix == GPS_FIX_3D) {
gps_udp.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_udp.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_UDP_ID, now_ts, &gps_udp);
} else {
printf("gps_udp error: msg len invalid %d bytes\n", size);
}
memset(&gps_udp_read_buffer[0], 0, sizeof(gps_udp_read_buffer));
}
}
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
{
uint8_t mde = 3; // OK
uint16_t val = 0;
if (!ins_mekf_wind.is_aligned) {
mde = 2; // ALIGN
} else if (!ins_mekf_wind.baro_initialized || !ins_mekf_wind.gps_initialized) {
mde = 1; // INIT
}
uint32_t t_diff = get_sys_time_usec() - last_imu_stamp;
/* set lost if no new gyro measurements for 50ms */
if (t_diff > 50000) {
mde = 5; // IMU_LOST
}
uint8_t id = INS_MEKF_WIND_FILTER_ID;
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val);
}
void imu_mpu9250_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the MPU9250 SPI transaction has succeeded: convert the data
mpu9250_spi_event(&imu_mpu9250.mpu);
if (imu_mpu9250.mpu.data_available) {
// set channel order
struct Int32Vect3 accel = {
IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]),
IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z])
};
struct Int32Rates rates = {
IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]),
IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
};
// unscaled vector
VECT3_COPY(imu.accel_unscaled, accel);
RATES_COPY(imu.gyro_unscaled, rates);
#if IMU_MPU9250_READ_MAG
if (!bit_is_set(imu_mpu9250.mpu.data_ext[6], 3)) { //mag valid just HOFL == 0
/** FIXME: assumes that we get new mag data each time instead of reading drdy bit */
struct Int32Vect3 mag;
mag.x = (IMU_MPU9250_X_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Y);
mag.y = (IMU_MPU9250_Y_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_X);
mag.z = -(IMU_MPU9250_Z_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Z);
VECT3_COPY(imu.mag_unscaled, mag);
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);
}
#endif
imu_mpu9250.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel);
}
}
void imu_periodic(void)
{
// Start reading the latest gyroscope data
if (!imu_krooz.mpu.config.initialized) {
mpu60x0_i2c_start_configure(&imu_krooz.mpu);
}
if (!imu_krooz.hmc.initialized) {
hmc58xx_start_configure(&imu_krooz.hmc);
}
uint32_t now_ts = get_sys_time_usec();
if (imu_krooz.meas_nb) {
RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb,
imu_krooz.rates_sum.p / imu_krooz.meas_nb,
imu_krooz.rates_sum.r / imu_krooz.meas_nb);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
}
if (imu_krooz.meas_nb_acc.x && imu_krooz.meas_nb_acc.y && imu_krooz.meas_nb_acc.z) {
imu.accel_unscaled.x = 65536 - imu_krooz.accel_sum.x / imu_krooz.meas_nb_acc.x;
imu.accel_unscaled.y = 65536 - imu_krooz.accel_sum.y / imu_krooz.meas_nb_acc.y;
imu.accel_unscaled.z = imu_krooz.accel_sum.z / imu_krooz.meas_nb_acc.z;
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);
INT_VECT3_ZERO(imu_krooz.accel_sum);
INT_VECT3_ZERO(imu_krooz.meas_nb_acc);
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
RunOnceEvery(128, {axis_nb = 5;});
void imu_drotek2_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_drotek2.mpu);
if (imu_drotek2.mpu.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
/* change orientation, so if ICs face up, z-axis is down */
imu.gyro_unscaled.p = imu_drotek2.mpu.data_rates.rates.p;
imu.gyro_unscaled.q = -imu_drotek2.mpu.data_rates.rates.q;
imu.gyro_unscaled.r = -imu_drotek2.mpu.data_rates.rates.r;
imu.accel_unscaled.x = imu_drotek2.mpu.data_accel.vect.x;
imu.accel_unscaled.y = -imu_drotek2.mpu.data_accel.vect.y;
imu.accel_unscaled.z = -imu_drotek2.mpu.data_accel.vect.z;
#else
/* default orientation as should be printed on the pcb, z-down, ICs down */
RATES_COPY(imu.gyro_unscaled, imu_drotek2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_drotek2.mpu.data_accel.vect);
#endif
imu_drotek2.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_DROTEK_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_DROTEK_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
hmc58xx_event(&imu_drotek2.hmc);
if (imu_drotek2.hmc.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
imu.mag_unscaled.x = imu_drotek2.hmc.data.vect.x;
imu.mag_unscaled.y = -imu_drotek2.hmc.data.vect.y;
imu.mag_unscaled.z = -imu_drotek2.hmc.data.vect.z;
#else
VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect);
#endif
imu_drotek2.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_DROTEK_ID, now_ts, &imu.mag);
}
}
static void gps_cb(uint8_t sender_id,
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{
if (sender_id == GPS_MULTI_ID) {
return;
}
uint32_t now_ts = get_sys_time_usec();
#ifdef SECONDARY_GPS
current_gps_id = gps_multi_switch(gps_s);
if (gps_s->comp_id == current_gps_id) {
gps = *gps_s;
AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s);
}
#else
gps = *gps_s;
AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s);
#endif
}
void imu_nps_event(void)
{
uint32_t now_ts = get_sys_time_usec();
if (imu_nps.gyro_available) {
imu_nps.gyro_available = FALSE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
}
if (imu_nps.accel_available) {
imu_nps.accel_available = FALSE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
if (imu_nps.mag_available) {
imu_nps.mag_available = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
}
}