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;
}
}
/** Read ADC value to update sonar measurement
*/
void sonar_adc_read(void) {
#ifndef SITL
sonar_meas = sonar_adc.sum / sonar_adc.av_nb_sample;
sonar_data_available = TRUE;
sonar_distance = ((float)sonar_meas * sonar_scale) + sonar_offset;
#else // SITL
sonar_distance = stateGetPositionEnu_f()->z;
Bound(sonar_distance, 0.1f, 7.0f);
#endif // SITL
#ifdef SENSOR_SYNC_SEND_SONAR
DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_meas, &sonar_distance);
#endif
}
/** 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
}