void nps_autopilot_run_step(double time)
{
nps_electrical_run_step(time);
#if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK
if (nps_radio_control_available(time)) {
radio_control_feed();
main_event();
}
#endif
if (nps_sensors_gyro_available()) {
imu_feed_gyro_accel();
main_event();
}
if (nps_sensors_mag_available()) {
imu_feed_mag();
main_event();
}
if (nps_sensors_baro_available()) {
float pressure = (float) sensors.baro.value;
AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, pressure);
main_event();
}
#if USE_SONAR
if (nps_sensors_sonar_available()) {
float dist = (float) sensors.sonar.value;
AbiSendMsgAGL(AGL_SONAR_NPS_ID, dist);
uint16_t foo = 0;
DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);
main_event();
}
#endif
if (nps_sensors_gps_available()) {
gps_feed_value();
main_event();
}
if (nps_bypass_ahrs) {
sim_overwrite_ahrs();
}
if (nps_bypass_ins) {
sim_overwrite_ins();
}
handle_periodic_tasks();
/* scale final motor commands to 0-1 for feeding the fdm */
for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
autopilot.commands[i] = (double)motor_mixing.commands[i] / MAX_PPRZ;
}
}
/**
* 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);
}
}
/** Read ADC value to update sonar measurement
*/
void sonar_adc_read(void)
{
#ifndef SITL
sonar_adc.meas = sonar_adc_buf.sum / sonar_adc_buf.av_nb_sample;
sonar_adc.distance = (float)(sonar_adc.meas - sonar_adc.offset) * SONAR_SCALE;
#else // SITL
sonar_adc.distance = stateGetPositionEnu_f()->z;
Bound(sonar_adc.distance, 0.1f, 7.0f);
#endif // SITL
// Send ABI message
AbiSendMsgAGL(AGL_SONAR_ADC_ID, sonar_adc.distance);
#ifdef SENSOR_SYNC_SEND_SONAR
// Send Telemetry report
DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_adc.meas, &sonar_adc.distance);
#endif
}
/**
* Poll lidar for data
* run at max 50Hz
*/
void lidar_lite_periodic(void)
{
switch (lidar.status) {
case LIDAR_INIT_RANGING:
// ask for i2c frame
lidar.trans.buf[0] = LIDAR_LITE_REG_ADDR; // sets register pointer to (0x00)
lidar.trans.buf[1] = LIDAR_LITE_REG_VAL; // take acquisition & correlation processing with DC correction
if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar.trans, lidar.addr, 2)) {
// transaction OK, increment status
lidar.status = LIDAR_REQ_READ;
}
break;
case LIDAR_REQ_READ:
lidar.trans.buf[0] = LIDAR_LITE_READ_ADDR; // sets register pointer to results register
lidar.trans.buf[1] = 0;
if (i2c_transceive(&LIDAR_LITE_I2C_DEV, &lidar.trans, lidar.addr, 1,2)) {
// transaction OK, increment status
lidar.status = LIDAR_READ_DISTANCE;
}
break;
case LIDAR_READ_DISTANCE:
// filter data
/*
lidar.distance_raw = (uint16_t)((lidar.trans.buf[0] << 8) + lidar.trans.buf[1]);
lidar.distance = update_median_filter(&lidar_filter, (int32_t)lidar.distance_raw);
*/
//lidar.distance_raw = (uint32_t)((lidar.trans.buf[0] << 8) | lidar.trans.buf[1]);
lidar.distance_raw = update_median_filter(&lidar_filter, (uint32_t)((lidar.trans.buf[0] << 8) | lidar.trans.buf[1]));
lidar.distance = ((float)lidar.distance_raw)/100.0;
// send message (if requested)
if (lidar.update_agl) {
AbiSendMsgAGL(AGL_LIDAR_LITE_ID, lidar.distance);
}
// increment status
lidar.status = LIDAR_INIT_RANGING;
break;
default:
break;
}
}
/**
* Poll lidar for data
*/
void lidar_sf11_periodic(void)
{
switch (lidar_sf11.status) {
case LIDAR_SF11_REQ_READ:
// read two bytes
lidar_sf11.trans.buf[0] = 0; // set tx to zero
i2c_transceive(&LIDAR_SF11_I2C_DEV, &lidar_sf11.trans, lidar_sf11.addr, 1, 2);
break;
case LIDAR_SF11_READ_OK:
// process results
// filter data
lidar_sf11.distance_raw = update_median_filter(
&lidar_sf11_filter,
(uint32_t)((lidar_sf11.trans.buf[0] << 8) | lidar_sf11.trans.buf[1]));
lidar_sf11.distance = ((float)lidar_sf11.distance_raw)/100.0;
// compensate AGL measurement for body rotation
if (lidar_sf11.compensate_rotation) {
float phi = stateGetNedToBodyEulers_f()->phi;
float theta = stateGetNedToBodyEulers_f()->theta;
float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
lidar_sf11.distance = lidar_sf11.distance / gain;
}
// send message (if requested)
if (lidar_sf11.update_agl) {
AbiSendMsgAGL(AGL_LIDAR_SF11_ID, lidar_sf11.distance);
}
// reset status
lidar_sf11.status = LIDAR_SF11_REQ_READ;
break;
default:
break;
}
}
/**
* Poll lidar for data
* for altitude hold 50Hz-100Hz should be enough,
* in theory can go faster (see the datasheet for Lidar Lite v3
*/
void lidar_lite_periodic(void)
{
switch (lidar_lite.status) {
case LIDAR_LITE_INIT_RANGING:
if (lidar_lite.trans.status == I2CTransDone) {
// ask for i2c frame
lidar_lite.trans.buf[0] = LIDAR_LITE_REG_ADDR; // sets register pointer to (0x00)
lidar_lite.trans.buf[1] = LIDAR_LITE_REG_VAL; // take acquisition & correlation processing with DC correction
if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 2)){
// transaction OK, increment status
lidar_lite.status = LIDAR_LITE_REQ_READ;
}
}
break;
case LIDAR_LITE_REQ_READ:
if (lidar_lite.trans.status == I2CTransDone) {
lidar_lite.trans.buf[0] = LIDAR_LITE_READ_ADDR; // sets register pointer to results register
if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 1)){
// transaction OK, increment status
lidar_lite.status = LIDAR_LITE_READ_DISTANCE;
}
}
break;
case LIDAR_LITE_READ_DISTANCE:
if (lidar_lite.trans.status == I2CTransDone) {
// clear buffer
lidar_lite.trans.buf[0] = 0;
lidar_lite.trans.buf[1] = 0;
if (i2c_receive(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 2)){
// transaction OK, increment status
lidar_lite.status = LIDAR_LITE_PARSE;
}
}
break;
case LIDAR_LITE_PARSE: {
// filter data
uint32_t now_ts = get_sys_time_usec();
lidar_lite.distance_raw = update_median_filter_i(
&lidar_lite_filter,
(uint32_t)((lidar_lite.trans.buf[0] << 8) | lidar_lite.trans.buf[1]));
lidar_lite.distance = ((float)lidar_lite.distance_raw)/100.0;
// compensate AGL measurement for body rotation
if (lidar_lite.compensate_rotation) {
float phi = stateGetNedToBodyEulers_f()->phi;
float theta = stateGetNedToBodyEulers_f()->theta;
float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
lidar_lite.distance = lidar_lite.distance / gain;
}
// send message (if requested)
if (lidar_lite.update_agl) {
AbiSendMsgAGL(AGL_LIDAR_LITE_ID, now_ts, lidar_lite.distance);
}
// increment status
lidar_lite.status = LIDAR_LITE_INIT_RANGING;
break;
}
default:
break;
}
}
