/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AltitudeInitialize() { BaroSensorInitialize(); RevoSettingsInitialize(); RevoSettingsConnectCallback(&SettingsUpdatedCb); SettingsUpdatedCb(NULL); #if defined(PIOS_INCLUDE_HCSR04) SonarAltitudeInitialize(); #endif return 0; }
static int32_t init(stateFilter *self) { struct data *this = (struct data *)self->localdata; this->baroOffset = 0.0f; this->first_run = INIT_CYCLES; RevoSettingsInitialize(); RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha); return 0; }
/** * Initialise the module. Called before the start function * \returns 0 on success or -1 if initialisation failed */ int32_t AttitudeInitialize(void) { AttitudeActualInitialize(); AirspeedActualInitialize(); AirspeedSensorInitialize(); AttitudeSettingsInitialize(); PositionActualInitialize(); VelocityActualInitialize(); RevoSettingsInitialize(); RevoCalibrationInitialize(); EKFConfigurationInitialize(); EKFStateVarianceInitialize(); FlightStatusInitialize(); // Initialize this here while we aren't setting the homelocation in GPS HomeLocationInitialize(); // Initialize quaternion AttitudeActualData attitude; AttitudeActualGet(&attitude); attitude.q1 = 1.0f; attitude.q2 = 0.0f; attitude.q3 = 0.0f; attitude.q4 = 0.0f; AttitudeActualSet(&attitude); // Cannot trust the values to init right above if BL runs GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); gyrosBias.x = 0.0f; gyrosBias.y = 0.0f; gyrosBias.z = 0.0f; GyrosBiasSet(&gyrosBias); AttitudeSettingsConnectCallback(&settingsUpdatedCb); RevoSettingsConnectCallback(&settingsUpdatedCb); RevoCalibrationConnectCallback(&settingsUpdatedCb); HomeLocationConnectCallback(&settingsUpdatedCb); EKFConfigurationConnectCallback(&settingsUpdatedCb); FlightStatusConnectCallback(&settingsUpdatedCb); return 0; }
/** * Initialise the module. Called before the start function * \returns 0 on success or -1 if initialisation failed */ int32_t SensorsInitialize(void) { source_data = (sensor_data *)pios_malloc(MAX_SENSOR_DATA_SIZE); GyroSensorInitialize(); AccelSensorInitialize(); MagSensorInitialize(); BaroSensorInitialize(); RevoCalibrationInitialize(); RevoSettingsInitialize(); AttitudeSettingsInitialize(); AccelGyroSettingsInitialize(); rotate = 0; RevoSettingsConnectCallback(&settingsUpdatedCb); RevoCalibrationConnectCallback(&settingsUpdatedCb); AttitudeSettingsConnectCallback(&settingsUpdatedCb); AccelGyroSettingsConnectCallback(&settingsUpdatedCb); return 0; }
/** * Run a preflight check over the hardware configuration * and currently active modules */ int32_t configuration_check() { int32_t severity = SYSTEMALARMS_ALARM_OK; SystemAlarmsExtendedAlarmStatusOptions alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; uint8_t alarmsubstatus = 0; // Get board type const struct pios_board_info *bdinfo = &pios_board_info_blob; bool coptercontrol = bdinfo->board_type == 0x04; // Classify navigation capability #ifdef REVOLUTION RevoSettingsInitialize(); uint8_t revoFusion; RevoSettingsFusionAlgorithmGet(&revoFusion); bool navCapableFusion; switch (revoFusion) { case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR: case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13: navCapableFusion = true; break; default: navCapableFusion = false; // check for hitl. hitl allows to feed position and velocity state via // telemetry, this makes nav possible even with an unsuited algorithm if (PositionStateHandle()) { if (PositionStateReadOnly()) { navCapableFusion = true; } } } #else const bool navCapableFusion = false; #endif /* ifdef REVOLUTION */ // Classify airframe type bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // For each available flight mode position sanity check the available // modes uint8_t num_modes; uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM]; ManualControlSettingsFlightModeNumberGet(&num_modes); FlightModeSettingsFlightModePositionGet(modes); for (uint32_t i = 0; i < num_modes; i++) { switch (modes[i]) { case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL: ADDSEVERITY(!multirotor); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1: ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2: ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3: ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4: ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5: ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6: ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: { // Revo supports PathPlanner and that must be OK or we are not sane // PathPlan alarm is uninitialized if not running // PathPlan alarm is warning or error if the flightplan is invalid SystemAlarmsAlarmData alarms; SystemAlarmsAlarmGet(&alarms); ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK); } // intentionally no break as this also needs pathfollower case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOFPV: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOLOS: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIONSEW: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE: ADDSEVERITY(!coptercontrol); ADDSEVERITY(navCapableFusion); break; default: // Uncovered modes are automatically an error ADDSEVERITY(false); } // mark the first encountered erroneous setting in status and substatus if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) { alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE; alarmsubstatus = i; } } uint8_t checks_disabled; FlightModeSettingsDisableSanityChecksGet(&checks_disabled); if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) { severity = SYSTEMALARMS_ALARM_WARNING; } if (severity != SYSTEMALARMS_ALARM_OK) { ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus); } else { AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION); } return 0; }