void nps_autopilot_run_step(double time __attribute__ ((unused))) {
if (nps_radio_control_available(time)) {
radio_control_feed();
main_event();
}
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()) {
baro_feed_value(sensors.baro.value);
main_event();
}
if (nps_sensors_gps_available()) {
booz_gps_feed_value();
main_event();
}
if (nps_bypass_ahrs) {
sim_overwrite_ahrs();
}
main_periodic();
if (time < 8) { /* start with a little bit of hovering */
int32_t init_cmd[4];
init_cmd[COMMAND_THRUST] = 0.253*SUPERVISION_MAX_MOTOR;
init_cmd[COMMAND_ROLL] = 0;
init_cmd[COMMAND_PITCH] = 0;
init_cmd[COMMAND_YAW] = 0;
supervision_run(TRUE, FALSE, init_cmd);
}
for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++)
autopilot.commands[i] = (double)supervision.commands[i] / SUPERVISION_MAX_MOTOR;
}
void nps_autopilot_run_step(double time __attribute__ ((unused))) {
if (nps_radio_control_available(time)) {
radio_control_feed();
main_event();
}
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()) {
baro_feed_value(sensors.baro.value);
main_event();
}
if (nps_sensors_gps_available()) {
gps_feed_value();
main_event();
}
if (nps_bypass_ahrs) {
sim_overwrite_ahrs();
}
handle_periodic_tasks();
/* scale final motor commands to 0-1 for feeding the fdm */
/* FIXME: autopilot.commands is of length NB_COMMANDS instead of number of motors */
for (uint8_t i=0; i<SUPERVISION_NB_MOTOR; i++)
autopilot.commands[i] = (double)(supervision.commands[i] - SUPERVISION_MIN_MOTOR) / SUPERVISION_MAX_MOTOR;
}
