void PX4RCOutput::force_safety_pending_requests(void)
{
// check if there is a pending saftey_state change. If so (timer != 0) then set it.
uint32_t now = AP_HAL::millis();
if (_safety_state_request_last_ms != 0 &&
now - _safety_state_request_last_ms >= 100) {
if (hal.util->safety_switch_state() == _safety_state_request &&
_safety_state_request_last_ms != 1) {
_safety_state_request_last_ms = 0;
} else if (_safety_state_request == AP_HAL::Util::SAFETY_DISARMED) {
// current != requested, set it
ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
_safety_state_request_last_ms = now;
} else if (_safety_state_request == AP_HAL::Util::SAFETY_ARMED) {
// current != requested, set it
ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
_safety_state_request_last_ms = now;
}
}
// also update safety button options if needed
if (now - _last_safety_options_check_ms > 1000) {
_last_safety_options_check_ms = now;
AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance();
if (boardconfig) {
uint16_t desired_options = 0;
uint16_t options = boardconfig->get_safety_button_options();
if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) {
desired_options |= PWM_SERVO_SET_SAFETY_OPTION_DISABLE_BUTTON_OFF;
}
if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) {
desired_options |= PWM_SERVO_SET_SAFETY_OPTION_DISABLE_BUTTON_ON;
}
if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) && hal.util->get_soft_armed()) {
desired_options |= PWM_SERVO_SET_SAFETY_OPTION_DISABLE_BUTTON_OFF | PWM_SERVO_SET_SAFETY_OPTION_DISABLE_BUTTON_ON;
}
if (_last_safety_options != desired_options) {
if (ioctl(_pwm_fd, PWM_SERVO_SET_SAFETY_OPTIONS, desired_options) == OK) {
_last_safety_options = desired_options;
}
}
}
}
}
//to be called only once on boot for initializing objects
void setup()
{
hal.console->printf("GPS AUTO library test\n");
board_config.init();
// Initialise the leds
board_led.init();
// Initialize the UART for GPS system
serial_manager.init();
gps.init(serial_manager);
}
void setup()
{
hal.console->printf("ArduPilot Airspeed library test\n");
set_object_value(&airspeed, airspeed.var_info, "PIN", 65);
set_object_value(&airspeed, airspeed.var_info, "ENABLE", 1);
set_object_value(&airspeed, airspeed.var_info, "USE", 1);
board_config.init();
airspeed.init();
airspeed.calibrate(false);
}
// to be called only once on boot for initializing objects
static void setup()
{
hal.console->printf("Compass library test\n");
board_config.init();
vehicle.ahrs.init();
compass.init();
hal.console->printf("init done - %u compasses detected\n", compass.get_count());
// set offsets to account for surrounding interference
compass.set_and_save_offsets(0, Vector3f(0, 0, 0));
// set local difference between magnetic north and true north
compass.set_declination(ToRad(0.0f));
hal.scheduler->delay(1000);
timer = AP_HAL::micros();
}
void setup(void)
{
// setup any board specific drivers
BoardConfig.init();
hal.console->printf("AP_InertialSensor startup...\n");
ins.init(100);
// display initial values
display_offsets_and_scaling();
// display number of detected accels/gyros
hal.console->printf("\n");
hal.console->printf("Number of detected accels : %u\n", ins.get_accel_count());
hal.console->printf("Number of detected gyros : %u\n\n", ins.get_gyro_count());
hal.console->printf("Complete. Reading:\n");
}
void setup()
{
hal.console->printf("SmartRTL test\n");
board_config.init();
smart_rtl.init();
}
