void VtolFlyController::UpdateAutoPilot() { if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { mManualThrust = true; } uint8_t result = RunAutoPilot(); if (result) { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); } else { pathStatus->Status = PATHSTATUS_STATUS_CRITICAL; AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); } PathStatusSet(pathStatus); // If rtbl, detect arrival at the endpoint and then triggers a change // to the pathDesired to initiate a Landing sequence. This is the simpliest approach. plans.c // can't manage this. And pathplanner whilst similar does not manage this as it is not a // waypoint traversal and is not aware of flight modes other than path plan. if (flightStatus->FlightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) { if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) { if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) { if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) { plan_setup_land(); } } } } }
void FixedWingFlyController::UpdateAutoPilot() { uint8_t result = updateAutoPilotFixedWing(); if (result) { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); } else { pathStatus->Status = PATHSTATUS_STATUS_CRITICAL; AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); } PathStatusSet(pathStatus); }
/** * Module thread, should not return. */ static void ahrscommsTask(void* parameters) { portTickType lastSysTime; AhrsStatusData data; AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL); /*Until AHRS connects, assume it doesn't know home */ AhrsStatusGet(&data); data.HomeSet = AHRSSTATUS_HOMESET_FALSE; //data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE; data.AlgorithmSet = AHRSSTATUS_CALIBRATIONSET_FALSE; AhrsStatusSet(&data); // Main task loop while (1) { AHRSSettingsData settings; AHRSSettingsGet(&settings); AhrsSendObjects(); AhrsCommStatus stat = AhrsGetStatus(); if(stat.linkOk) { AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS); }else { AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING); } AhrsStatusData sData; AhrsStatusGet(&sData); sData.LinkRunning = stat.linkOk; sData.AhrsKickstarts = stat.remote.kickStarts; sData.AhrsCrcErrors = stat.remote.crcErrors; sData.AhrsRetries = stat.remote.retries; sData.AhrsInvalidPackets = stat.remote.invalidPacket; sData.OpCrcErrors = stat.local.crcErrors; sData.OpRetries = stat.local.retries; sData.OpInvalidPackets = stat.local.invalidPacket; AhrsStatusSet(&sData); /* Wait for the next update interval */ vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS ); } }
/** * Module task */ static void stabilizationTask(void* parameters) { portTickType lastSysTime; portTickType thisSysTime; UAVObjEvent ev; QuadMotorsDesiredData actuatorDesired; FlightStatusData flightStatus; //SettingsUpdatedCb((UAVObjEvent *) NULL); // Main task loop lastSysTime = xTaskGetTickCount(); while(1) { PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); continue; } // Check how long since last update thisSysTime = xTaskGetTickCount(); if(thisSysTime > lastSysTime) // reuse dt in case of wraparound dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; lastSysTime = thisSysTime; FlightStatusGet(&flightStatus); QuadMotorsDesiredGet(&actuatorDesired); //if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) //{ // ZERO MOTORS //} //else //{ //} //ActuatorSettingsData settings; //ActuatorSettingsGet(&settings); //PIOS_SetMKSpeed(settings.ChannelAddr[mixer_channel],value); PIOS_SetMKSpeed(0,actuatorDesired.motorFront_NW); PIOS_SetMKSpeed(1,actuatorDesired.motorRight_NE); PIOS_SetMKSpeed(2,actuatorDesired.motorBack_SE); PIOS_SetMKSpeed(3,actuatorDesired.motorLeft_SW); // Clear alarms AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } }
/* stub: module has no module thread */ int32_t GeofenceStart(void) { if (geofenceSettings == NULL) { return -1; } // Schedule periodic task to check position UAVObjEvent ev = { .obj = PositionActualHandle(), .instId = 0, .event = 0, }; EventPeriodicCallbackCreate(&ev, checkPosition, SAMPLE_PERIOD_MS); return 0; } MODULE_INITCALL(GeofenceInitialize, GeofenceStart); /** * Periodic callback that processes changes in position and * sets the alarm. */ static void checkPosition(UAVObjEvent* ev, void *ctx, void *obj, int len) { (void) ev; (void) ctx; (void) obj; (void) len; if (PositionActualHandle()) { PositionActualData positionActual; PositionActualGet(&positionActual); const float distance2 = powf(positionActual.North, 2) + powf(positionActual.East, 2); // ErrorRadius is squared when it is fetched, so this is correct if (distance2 > geofenceSettings->ErrorRadius) { AlarmsSet(SYSTEMALARMS_ALARM_GEOFENCE, SYSTEMALARMS_ALARM_ERROR); } else if (distance2 > geofenceSettings->WarningRadius) { AlarmsSet(SYSTEMALARMS_ALARM_GEOFENCE, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_GEOFENCE); } } }
int32_t GPSStart(void) { if (module_enabled) { if (gpsPort) { // Start gps task xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_GPS, gpsTaskHandle); return 0; } AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); } return -1; }
int32_t GPSStart(void) { if (module_enabled) { if (gpsPort) { // Start gps task gpsTaskHandle = PIOS_Thread_Create(gpsTask, "GPS", STACK_SIZE_BYTES, NULL, TASK_PRIORITY); TaskMonitorAdd(TASKINFO_RUNNING_GPS, gpsTaskHandle); return 0; } AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); } return -1; }
/** * Update system alarms */ static void updateSystemAlarms() { SystemStatsData stats; UAVObjStats objStats; EventStats evStats; SystemStatsGet(&stats); // Check heap if (stats.HeapRemaining < HEAP_LIMIT_CRITICAL) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL); } else if (stats.HeapRemaining < HEAP_LIMIT_WARNING) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY); } // Check CPU load if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) { AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL); } else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) { AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD); } // Check for stack overflow if (stackOverflow == 1) { AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL); } else { AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW); } #if defined(PIOS_INCLUDE_SDCARD) // Check for SD card if (PIOS_SDCARD_IsMounted() == 0) { AlarmsSet(SYSTEMALARMS_ALARM_SDCARD, SYSTEMALARMS_ALARM_ERROR); } else { AlarmsClear(SYSTEMALARMS_ALARM_SDCARD); } #endif // Check for event errors UAVObjGetStats(&objStats); EventGetStats(&evStats); UAVObjClearStats(); EventClearStats(); if (objStats.eventErrors > 0 || evStats.eventErrors > 0) { AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM); } }
/** * Module thread, should not return. */ static void ahrscommsTask(void *parameters) { portTickType lastSysTime; AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL); // Main task loop while (1) { PIOS_WDG_UpdateFlag(PIOS_WDG_AHRS); AhrsCommStatus stat; AhrsSendObjects(); AhrsGetStatus(&stat); if (stat.linkOk) { AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS); } else { AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING); } InsStatusData sData; InsStatusGet(&sData); sData.LinkRunning = stat.linkOk; sData.AhrsKickstarts = stat.remote.kickStarts; sData.AhrsCrcErrors = stat.remote.crcErrors; sData.AhrsRetries = stat.remote.retries; sData.AhrsInvalidPackets = stat.remote.invalidPacket; sData.OpCrcErrors = stat.local.crcErrors; sData.OpRetries = stat.local.retries; sData.OpInvalidPackets = stat.local.invalidPacket; InsStatusSet(&sData); /* Wait for the next update interval */ vTaskDelayUntil(&lastSysTime, 2 / portTICK_RATE_MS); } }
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) { StabilizationDesiredData stabilization; StabilizationDesiredGet(&stabilization); StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); uint8_t * stab_settings; switch(cmd->FlightMode) { case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1: stab_settings = settings->Stabilization1Settings; break; case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2: stab_settings = settings->Stabilization2Settings; break; case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED3: stab_settings = settings->Stabilization3Settings; break; default: // Major error, this should not occur because only enter this block when one of these is true AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); break; } // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode ; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_PITCH] : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; StabilizationDesiredSet(&stabilization); }
/** * Module thread, should not return. */ static void AttitudeTask(void *parameters) { bool first_run = true; uint32_t last_algorithm; AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); // Force settings update to make sure rotation loaded settingsUpdatedCb(NULL); // Wait for all the sensors be to read vTaskDelay(100); // Invalidate previous algorithm to trigger a first run last_algorithm = 0xfffffff; // Main task loop while (1) { int32_t ret_val = -1; if (last_algorithm != revoSettings.FusionAlgorithm) { last_algorithm = revoSettings.FusionAlgorithm; first_run = true; } // This function blocks on data queue switch (revoSettings.FusionAlgorithm ) { case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: ret_val = updateAttitudeComplementary(first_run); break; case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: ret_val = updateAttitudeINSGPS(first_run, true); break; case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: ret_val = updateAttitudeINSGPS(first_run, false); break; default: AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL); break; } if(ret_val == 0) first_run = false; PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); } }
/** * @brief Main module task */ static void actuatorTask(void* parameters) { // UAVObjEvent ev; portTickType lastSysTime; ActuatorCommandData command; ActuatorSettingsData settings; // Set servo update frequency (done only on start-up) ActuatorSettingsGet(&settings); PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]); // Go to the neutral (failsafe) values until an ActuatorDesired update is received setFailsafe(); // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { ActuatorCommandGet(&command); ActuatorSettingsGet(&settings); if ( RunMixers(&command, &settings) == -1 ) { AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); } else { AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); } // Update output object ActuatorCommandSet(&command); // Update in case read only (eg. during servo configuration) ActuatorCommandGet(&command); // Update servo outputs for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { PIOS_Servo_Set( n, command.Channel[n] ); } // Wait until next update vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS ); } }
/** * Set the error code and alarm state * @param[in] error code */ void set_config_error(SystemAlarmsConfigErrorOptions error_code) { // Get the severity of the alarm given the error code SystemAlarmsAlarmOptions severity; static bool sticky = false; /* Once a sticky error occurs, never change the error code */ if (sticky) return; switch (error_code) { case SYSTEMALARMS_CONFIGERROR_NONE: severity = SYSTEMALARMS_ALARM_OK; break; default: error_code = SYSTEMALARMS_CONFIGERROR_UNDEFINED; /* and fall through */ case SYSTEMALARMS_CONFIGERROR_DUPLICATEPORTCFG: sticky = true; /* and fall through */ case SYSTEMALARMS_CONFIGERROR_AUTOTUNE: case SYSTEMALARMS_CONFIGERROR_ALTITUDEHOLD: case SYSTEMALARMS_CONFIGERROR_MULTIROTOR: case SYSTEMALARMS_CONFIGERROR_NAVFILTER: case SYSTEMALARMS_CONFIGERROR_PATHPLANNER: case SYSTEMALARMS_CONFIGERROR_POSITIONHOLD: case SYSTEMALARMS_CONFIGERROR_STABILIZATION: case SYSTEMALARMS_CONFIGERROR_UNDEFINED: case SYSTEMALARMS_CONFIGERROR_UNSAFETOARM: severity = SYSTEMALARMS_ALARM_ERROR; break; } // Make sure not to set the error code if it didn't change SystemAlarmsConfigErrorOptions current_error_code; SystemAlarmsConfigErrorGet((uint8_t *) ¤t_error_code); if (current_error_code != error_code) { SystemAlarmsConfigErrorSet((uint8_t *) &error_code); } // AlarmSet checks only updates on toggle AlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, (uint8_t) severity); }
/** * Module thread, should not return. */ static void altitudeHoldTask(void *parameters) { bool engaged = false; AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; AltitudeHoldSettingsData altitudeHoldSettings; UAVObjEvent ev; struct pid velocity_pid; // Listen for object updates. AltitudeHoldDesiredConnectQueue(queue); AltitudeHoldSettingsConnectQueue(queue); FlightStatusConnectQueue(queue); AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi, 0.0f, 1.0f); AlarmsSet(SYSTEMALARMS_ALARM_ALTITUDEHOLD, SYSTEMALARMS_ALARM_OK); // Main task loop const uint32_t dt_ms = 5; const float dt_s = dt_ms * 0.001f; uint32_t timeout = dt_ms; while (1) { if (PIOS_Queue_Receive(queue, &ev, timeout) != true) { } else if (ev.obj == FlightStatusHandle()) { uint8_t flight_mode; FlightStatusFlightModeGet(&flight_mode); if (flight_mode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !engaged) { // Copy the current throttle as a starting point for integral StabilizationDesiredThrottleGet(&velocity_pid.iAccumulator); velocity_pid.iAccumulator *= 1000.0f; // pid library scales up accumulator by 1000 engaged = true; } else if (flight_mode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) engaged = false; // Run loop at 20 Hz when engaged otherwise just slowly wait for it to be engaged timeout = engaged ? dt_ms : 100; } else if (ev.obj == AltitudeHoldDesiredHandle()) { AltitudeHoldDesiredGet(&altitudeHoldDesired); } else if (ev.obj == AltitudeHoldSettingsHandle()) { AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi, 0.0f, 1.0f); } bool landing = altitudeHoldDesired.Land == ALTITUDEHOLDDESIRED_LAND_TRUE; // For landing mode allow throttle to go negative to allow the integrals // to stop winding up const float min_throttle = landing ? -0.1f : 0.0f; // When engaged compute altitude controller output if (engaged) { float position_z, velocity_z, altitude_error; PositionActualDownGet(&position_z); VelocityActualDownGet(&velocity_z); position_z = -position_z; // Use positive up convention velocity_z = -velocity_z; // Use positive up convention // Compute the altitude error altitude_error = altitudeHoldDesired.Altitude - position_z; // Velocity desired is from the outer controller plus the set point float velocity_desired = altitude_error * altitudeHoldSettings.PositionKp + altitudeHoldDesired.ClimbRate; float throttle_desired = pid_apply_antiwindup(&velocity_pid, velocity_desired - velocity_z, min_throttle, 1.0f, // positive limits since this is throttle dt_s); if (altitudeHoldSettings.AttitudeComp > 0) { // Throttle desired is at this point the mount desired in the up direction, we can // account for the attitude if desired AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); // Project a unit vector pointing up into the body frame and // get the z component float fraction = attitudeActual.q1 * attitudeActual.q1 - attitudeActual.q2 * attitudeActual.q2 - attitudeActual.q3 * attitudeActual.q3 + attitudeActual.q4 * attitudeActual.q4; // Add ability to scale up the amount of compensation to achieve // level forward flight fraction = powf(fraction, (float) altitudeHoldSettings.AttitudeComp / 100.0f); // Dividing by the fraction remaining in the vertical projection will // attempt to compensate for tilt. This acts like the thrust is linear // with the output which isn't really true. If the fraction is starting // to go negative we are inverted and should shut off throttle throttle_desired = (fraction > 0.1f) ? (throttle_desired / fraction) : 0.0f; } StabilizationDesiredGet(&stabilizationDesired); stabilizationDesired.Throttle = bound_min_max(throttle_desired, min_throttle, 1.0f); if (landing) { stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.Roll = 0; stabilizationDesired.Pitch = 0; stabilizationDesired.Yaw = 0; } else { stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.Roll = altitudeHoldDesired.Roll; stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; } StabilizationDesiredSet(&stabilizationDesired); } } }
/** * Module task */ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; ManualControlCommandData cmd; portTickType lastSysTime; float flightMode = 0; uint8_t disconnected_count = 0; uint8_t connected_count = 0; enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED; // Make sure unarmed on power up ManualControlCommandGet(&cmd); cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; ManualControlCommandSet(&cmd); armState = ARM_STATE_DISARMED; // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); // Read settings ManualControlSettingsGet(&settings); if (ManualControlCommandReadOnly(&cmd)) { FlightTelemetryStatsData flightTelemStats; FlightTelemetryStatsGet(&flightTelemStats); if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { /* trying to fly via GCS and lost connection. fall back to transmitter */ UAVObjMetadata metadata; UAVObjGetMetadata(&cmd, &metadata); metadata.access = ACCESS_READWRITE; UAVObjSetMetadata(&cmd, &metadata); } } if (!ManualControlCommandReadOnly(&cmd)) { // Check settings, if error raise alarm if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); continue; } // Read channel values in us // TODO: settings.InputMode is currently ignored because PIOS will not allow runtime // selection of PWM and PPM. The configuration is currently done at compile time in // the pios_config.h file. for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { #if defined(PIOS_INCLUDE_PWM) cmd.Channel[n] = PIOS_PWM_Get(n); #elif defined(PIOS_INCLUDE_PPM) cmd.Channel[n] = PIOS_PPM_Get(n); #elif defined(PIOS_INCLUDE_SPEKTRUM) cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); #endif scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n], 0); } // Scale channels to -1 -> +1 range cmd.Roll = scaledChannel[settings.Roll]; cmd.Pitch = scaledChannel[settings.Pitch]; cmd.Yaw = scaledChannel[settings.Yaw]; cmd.Throttle = scaledChannel[settings.Throttle]; flightMode = scaledChannel[settings.FlightMode]; if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) cmd.Accessory1 = scaledChannel[settings.Accessory1]; else cmd.Accessory1 = 0; if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) cmd.Accessory2 = scaledChannel[settings.Accessory2]; else cmd.Accessory2 = 0; if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) cmd.Accessory3 = scaledChannel[settings.Accessory3]; else cmd.Accessory3 = 0; // Note here the code is ass if (flightMode < -FLIGHT_MODE_LIMIT) cmd.FlightMode = settings.FlightModePosition[0]; else if (flightMode > FLIGHT_MODE_LIMIT) cmd.FlightMode = settings.FlightModePosition[2]; else cmd.FlightMode = settings.FlightModePosition[1]; // Update the ManualControlCommand object ManualControlCommandSet(&cmd); // This seems silly to set then get, but the reason is if the GCS is // the control input, the set command will be blocked by the read only // setting and the get command will pull the right values from telemetry } else ManualControlCommandGet(&cmd); /* Under GCS control */ // Implement hysteresis loop on connection status // Must check both Max and Min in case they reversed if (!ManualControlCommandReadOnly(&cmd) && cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET && cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) { if (disconnected_count++ > 10) { connection_state = DISCONNECTED; connected_count = 0; disconnected_count = 0; } else disconnected_count++; } else { if (connected_count++ > 10) { connection_state = CONNECTED; connected_count = 0; disconnected_count = 0; } else connected_count++; } if (connection_state == DISCONNECTED) { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; cmd.Throttle = -1; // Shut down engine with no control cmd.Roll = 0; cmd.Yaw = 0; cmd.Pitch = 0; //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); ManualControlCommandSet(&cmd); } else { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); ManualControlCommandSet(&cmd); } // // Arming and Disarming mechanism // // Look for state changes and write in newArmState uint8_t newCmdArmed = cmd.Armed; // By default, keep the arming state the same if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { // In this configuration we always disarm newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE; } else { // In all other cases, we will not change the arm state when disconnected if (connection_state == CONNECTED) { if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { // In this configuration, we go into armed state as soon as the throttle is low, never disarm if (cmd.Throttle < 0) { newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; } } else { // When the configuration is not "Always armed" and no "Always disarmed", // the state will not be changed when the throttle is not low if (cmd.Throttle < 0) { static portTickType armedDisarmStart; float armingInputLevel = 0; // Calc channel see assumptions7 switch ( (settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { case ARMING_CHANNEL_ROLL: armingInputLevel = cmd.Roll; break; case ARMING_CHANNEL_PITCH: armingInputLevel = cmd.Pitch; break; case ARMING_CHANNEL_YAW: armingInputLevel = cmd.Yaw; break; } bool manualArm = false; bool manualDisarm = false; if (connection_state == CONNECTED) { // Should use RC input only if RX is connected if (armingInputLevel <= -0.90) manualArm = true; else if (armingInputLevel >= +0.90) manualDisarm = true; } // Swap arm-disarming see assumptions8 if ((settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) { bool temp = manualArm; manualArm = manualDisarm; manualDisarm = temp; } switch(armState) { case ARM_STATE_DISARMED: newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE; if (manualArm) { if (okToArm()) // only allow arming if it's OK too { armedDisarmStart = lastSysTime; armState = ARM_STATE_ARMING_MANUAL; } } break; case ARM_STATE_ARMING_MANUAL: if (manualArm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_ARMED; } else armState = ARM_STATE_DISARMED; break; case ARM_STATE_ARMED: // When we get here, the throttle is low, // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_TIMEOUT; newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; break; case ARM_STATE_DISARMING_TIMEOUT: // We get here when armed while throttle low, even when the arming timeout is not enabled if (settings.ArmedTimeout != 0) if (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.ArmedTimeout) armState = ARM_STATE_DISARMED; // Switch to disarming due to manual control when needed if (manualDisarm) { armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_MANUAL; } break; case ARM_STATE_DISARMING_MANUAL: if (manualDisarm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_DISARMED; } else armState = ARM_STATE_ARMED; break; } // End Switch } else { // The throttle is not low, in case we where arming or disarming, abort switch(armState) { case ARM_STATE_DISARMING_MANUAL: case ARM_STATE_DISARMING_TIMEOUT: armState = ARM_STATE_ARMED; break; case ARM_STATE_ARMING_MANUAL: armState = ARM_STATE_DISARMED; break; default: // Nothing needs to be done in the other states break; } } } } } // Update cmd object when needed if (newCmdArmed != cmd.Armed) { cmd.Armed = newCmdArmed; ManualControlCommandSet(&cmd); } // // End of arming/disarming // // Depending on the mode update the Stabilization or Actuator objects switch(PARSE_FLIGHT_MODE(cmd.FlightMode)) { case FLIGHTMODE_UNDEFINED: // This reflects a bug in the code architecture! AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); break; case FLIGHTMODE_MANUAL: updateActuatorDesired(&cmd); break; case FLIGHTMODE_STABILIZED: updateStabilizationDesired(&cmd, &settings); break; case FLIGHTMODE_GUIDANCE: // TODO: Implement break; } } }
/** * Module thread, should not return. */ static void vtolPathFollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; portTickType lastUpdateTime; VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); VtolPathFollowerSettingsGet(&guidanceSettings); PathDesiredGet(&pathDesired); // Main task loop lastUpdateTime = xTaskGetTickCount(); while (1) { // Conditions when this runs: // 1. Must have VTOL type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) ) { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING); vTaskDelay(1000); continue; } // Continue collecting data if not enough time vTaskDelayUntil(&lastUpdateTime, MS2TICKS(guidanceSettings.UpdatePeriod)); // Convert the accels into the NED frame updateNedAccel(); FlightStatusGet(&flightStatus); // Check the combinations of flightmode and pathdesired mode switch(flightStatus.FlightMode) { /* This combination of RETURNTOHOME and HOLDPOSITION looks strange but * is correct. RETURNTOHOME mode uses HOLDPOSITION with the position * set to home */ case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT || pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) { updatePathVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; default: for (uint32_t i = 0; i < VTOL_PID_NUM; i++) pid_zero(&vtol_pids[i]); // Track throttle before engaging this mode. Cheap system ident StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); throttleOffset = stabDesired.Throttle; break; } AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); } }
/** * Module task */ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; StabilizationSettingsData stabSettings; ManualControlCommandData cmd; ActuatorDesiredData actuator; AttitudeDesiredData attitude; RateDesiredData rate; portTickType lastSysTime; float flightMode; uint8_t disconnected_count = 0; uint8_t connected_count = 0; enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED; // Make sure unarmed on power up ManualControlCommandGet(&cmd); cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; ManualControlCommandSet(&cmd); armState = ARM_STATE_DISARMED; // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); // Read settings ManualControlSettingsGet(&settings); StabilizationSettingsGet(&stabSettings); if (ManualControlCommandReadOnly(&cmd)) { FlightTelemetryStatsData flightTelemStats; FlightTelemetryStatsGet(&flightTelemStats); if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { /* trying to fly via GCS and lost connection. fall back to transmitter */ UAVObjMetadata metadata; UAVObjGetMetadata(&cmd, &metadata); metadata.access = ACCESS_READWRITE; UAVObjSetMetadata(&cmd, &metadata); } } if (!ManualControlCommandReadOnly(&cmd)) { // Check settings, if error raise alarm if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); continue; } // Read channel values in us // TODO: settings.InputMode is currently ignored because PIOS will not allow runtime // selection of PWM and PPM. The configuration is currently done at compile time in // the pios_config.h file. for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { #if defined(PIOS_INCLUDE_PWM) cmd.Channel[n] = PIOS_PWM_Get(n); #elif defined(PIOS_INCLUDE_PPM) cmd.Channel[n] = PIOS_PPM_Get(n); #elif defined(PIOS_INCLUDE_SPEKTRUM) cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); #endif scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } // Scale channels to -1 -> +1 range cmd.Roll = scaledChannel[settings.Roll]; cmd.Pitch = scaledChannel[settings.Pitch]; cmd.Yaw = scaledChannel[settings.Yaw]; cmd.Throttle = scaledChannel[settings.Throttle]; flightMode = scaledChannel[settings.FlightMode]; if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) cmd.Accessory1 = scaledChannel[settings.Accessory1]; else cmd.Accessory1 = 0; if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) cmd.Accessory2 = scaledChannel[settings.Accessory2]; else cmd.Accessory2 = 0; if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) cmd.Accessory3 = scaledChannel[settings.Accessory3]; else cmd.Accessory3 = 0; if (flightMode < -FLIGHT_MODE_LIMIT) { // Position 1 for(int i = 0; i < 3; i++) { cmd.StabilizationSettings[i] = settings.Pos1StabilizationSettings[i]; // See assumptions1 } cmd.FlightMode = settings.Pos1FlightMode; // See assumptions2 } else if (flightMode > FLIGHT_MODE_LIMIT) { // Position 3 for(int i = 0; i < 3; i++) { cmd.StabilizationSettings[i] = settings.Pos3StabilizationSettings[i]; // See assumptions5 } cmd.FlightMode = settings.Pos3FlightMode; // See assumptions6 } else { // Position 2 for(int i = 0; i < 3; i++) { cmd.StabilizationSettings[i] = settings.Pos2StabilizationSettings[i]; // See assumptions3 } cmd.FlightMode = settings.Pos2FlightMode; // See assumptions4 } // Update the ManualControlCommand object ManualControlCommandSet(&cmd); // This seems silly to set then get, but the reason is if the GCS is // the control input, the set command will be blocked by the read only // setting and the get command will pull the right values from telemetry } else ManualControlCommandGet(&cmd); /* Under GCS control */ // Implement hysteresis loop on connection status // Must check both Max and Min in case they reversed if (!ManualControlCommandReadOnly(&cmd) && cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET && cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) { if (disconnected_count++ > 10) { connection_state = DISCONNECTED; connected_count = 0; disconnected_count = 0; } else disconnected_count++; } else { if (connected_count++ > 10) { connection_state = CONNECTED; connected_count = 0; disconnected_count = 0; } else connected_count++; } if (connection_state == DISCONNECTED) { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; cmd.Throttle = -1; // Shut down engine with no control cmd.Roll = 0; cmd.Yaw = 0; cmd.Pitch = 0; //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); ManualControlCommandSet(&cmd); } else { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); ManualControlCommandSet(&cmd); } // Arming and Disarming mechanism if (cmd.Throttle < 0) { // Throttle is low, in this condition the arming state could change uint8_t newCmdArmed = cmd.Armed; static portTickType armedDisarmStart; // Look for state changes and write in newArmState if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_NONE) { // No channel assigned to arming -> arm immediately when throttle is low newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; } else { float armStickLevel; uint8_t channel = settings.Arming/2; // 0=Channel1, 1=Channel1_Rev, 2=Channel2, .... bool reverse = (settings.Arming%2)==1; bool manualArm = false; bool manualDisarm = false; if (connection_state == CONNECTED) { // Should use RC input only if RX is connected armStickLevel = scaledChannel[channel]; if (reverse) armStickLevel =-armStickLevel; if (armStickLevel <= -0.90) manualArm = true; else if (armStickLevel >= +0.90) manualDisarm = true; } switch(armState) { case ARM_STATE_DISARMED: newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE; if (manualArm) { armedDisarmStart = lastSysTime; armState = ARM_STATE_ARMING_MANUAL; } break; case ARM_STATE_ARMING_MANUAL: if (manualArm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_ARMED; } else armState = ARM_STATE_DISARMED; break; case ARM_STATE_ARMED: // When we get here, the throttle is low, // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_TIMEOUT; newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; break; case ARM_STATE_DISARMING_TIMEOUT: // We get here when armed while throttle low, even when the arming timeout is not enabled if (settings.ArmedTimeout != 0) if (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.ArmedTimeout) armState = ARM_STATE_DISARMED; // Switch to disarming due to manual control when needed if (manualDisarm) { armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_MANUAL; } break; case ARM_STATE_DISARMING_MANUAL: if (manualDisarm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_DISARMED; } else armState = ARM_STATE_ARMED; break; } } // Update cmd object when needed if (newCmdArmed != cmd.Armed) { cmd.Armed = newCmdArmed; ManualControlCommandSet(&cmd); } } else { // The throttle is not low, in case we where arming or disarming, abort switch(armState) { case ARM_STATE_DISARMING_MANUAL: case ARM_STATE_DISARMING_TIMEOUT: armState = ARM_STATE_ARMED; break; case ARM_STATE_ARMING_MANUAL: armState = ARM_STATE_DISARMED; break; default: // Nothing needs to be done in the other states break; } } // End of arming/disarming // Depending on the mode update the Stabilization or Actuator objects if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) { actuator.Roll = cmd.Roll; actuator.Pitch = cmd.Pitch; actuator.Yaw = cmd.Yaw; actuator.Throttle = cmd.Throttle; ActuatorDesiredSet(&actuator); } else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) { attitude.Roll = cmd.Roll * stabSettings.RollMax; attitude.Pitch = cmd.Pitch * stabSettings.PitchMax; attitude.Yaw = fmod(cmd.Yaw * 180.0, 360); attitude.Throttle = (cmd.Throttle < 0) ? -1 : cmd.Throttle; rate.Roll = cmd.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; rate.Pitch = cmd.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; rate.Yaw = cmd.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; AttitudeDesiredSet(&attitude); RateDesiredSet(&rate); } } }
/** * Module thread, should not return. */ static void pathfollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; uint32_t lastUpdateTime; AirspeedActualConnectCallback(airspeedActualUpdatedCb); FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); FixedWingAirspeedsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); // Force update of all the settings SettingsUpdatedCb(NULL); FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); path_desired_updated = false; PathDesiredGet(&pathDesired); PathDesiredConnectCallback(pathDesiredUpdated); // Main task loop lastUpdateTime = PIOS_Thread_Systime(); while (1) { // Conditions when this runs: // 1. Must have FixedWing type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) ) { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL); PIOS_Thread_Sleep(1000); continue; } // Continue collecting data if not enough time PIOS_Thread_Sleep_Until(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod); static uint8_t last_flight_mode; FlightStatusGet(&flightStatus); PathStatusGet(&pathStatus); PositionActualData positionActual; static enum {FW_FOLLOWER_IDLE, FW_FOLLOWER_RUNNING, FW_FOLLOWER_ERR} state = FW_FOLLOWER_IDLE; // Check whether an update to the path desired occured and we should // process it. This makes sure that the follower alarm state is // updated. bool process_path_desired_update = (last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER || last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && path_desired_updated; path_desired_updated = false; // Process most of these when the flight mode changes // except when in path following mode in which case // each iteration must make sure this has the latest // PathDesired if (flightStatus.FlightMode != last_flight_mode || process_path_desired_update) { last_flight_mode = flightStatus.FlightMode; switch(flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: state = FW_FOLLOWER_RUNNING; PositionActualGet(&positionActual); pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT; pathDesired.Start[0] = positionActual.North; pathDesired.Start[1] = positionActual.East; pathDesired.Start[2] = positionActual.Down; pathDesired.End[0] = 0; pathDesired.End[1] = 0; pathDesired.End[2] = -30.0f; pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius; pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed; pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed; PathDesiredSet(&pathDesired); break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: state = FW_FOLLOWER_RUNNING; PositionActualGet(&positionActual); pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT; pathDesired.Start[0] = positionActual.North; pathDesired.Start[1] = positionActual.East; pathDesired.Start[2] = positionActual.Down; pathDesired.End[0] = positionActual.North; pathDesired.End[1] = positionActual.East; pathDesired.End[2] = positionActual.Down; pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius; pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed; pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed; PathDesiredSet(&pathDesired); break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL: state = FW_FOLLOWER_RUNNING; PathDesiredGet(&pathDesired); switch(pathDesired.Mode) { case PATHDESIRED_MODE_ENDPOINT: case PATHDESIRED_MODE_VECTOR: case PATHDESIRED_MODE_CIRCLERIGHT: case PATHDESIRED_MODE_CIRCLELEFT: break; default: state = FW_FOLLOWER_ERR; pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; PathStatusSet(&pathStatus); AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL); break; } break; default: state = FW_FOLLOWER_IDLE; break; } } switch(state) { case FW_FOLLOWER_RUNNING: { updatePathVelocity(); uint8_t result = updateFixedDesiredAttitude(); if (result) { AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING); } PathStatusSet(&pathStatus); break; } case FW_FOLLOWER_IDLE: // Be cleaner and get rid of global variables northVelIntegral = 0; eastVelIntegral = 0; downVelIntegral = 0; bearingIntegral = 0; speedIntegral = 0; accelIntegral = 0; powerIntegral = 0; airspeedErrorInt = 0; AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); break; case FW_FOLLOWER_ERR: default: // Leave alarms set above break; } } }
/** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); const struct pios_board_info * bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) PIOS_LED_Init(&pios_led_cfg); #endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_FLASH) /* Inititialize all flash drivers */ if (PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg) != 0) panic(1); /* Register the partition table */ const struct pios_flash_partition * flash_partition_table; uint32_t num_partitions; flash_partition_table = PIOS_BOARD_HW_DEFS_GetPartitionTable(bdinfo->board_rev, &num_partitions); PIOS_FLASH_register_partition_table(flash_partition_table, num_partitions); /* Mount all filesystems */ if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS) != 0) panic(1); #endif /* PIOS_INCLUDE_FLASH */ /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); /* Initialize the alarms library. Reads RCC reset flags */ AlarmsInitialize(); PIOS_RESET_Clear(); // Clear the RCC reset flags after use. /* Initialize the hardware UAVOs */ HwDiscoveryF4Initialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); #endif #ifndef ERASE_FLASH /* Initialize watchdog as early as possible to catch faults during init */ #ifndef DEBUG //PIOS_WDG_Init(); #endif #endif /* Check for repeated boot failures */ PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hw config to defaults */ HwDiscoveryF4SetDefaults(HwDiscoveryF4Handle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uintptr_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwDiscoveryF4USB_VCPPortGet(&hw_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hw_usb_vcpport = HWDISCOVERYF4_USB_VCPPORT_DISABLED; } uintptr_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } switch (hw_usb_vcpport) { case HWDISCOVERYF4_USB_VCPPORT_DISABLED: break; case HWDISCOVERYF4_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWDISCOVERYF4_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWDISCOVERYF4_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwDiscoveryF4USB_HIDPortGet(&hw_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hw_usb_hidport = HWDISCOVERYF4_USB_HIDPORT_DISABLED; } uintptr_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } switch (hw_usb_hidport) { case HWDISCOVERYF4_USB_HIDPORT_DISABLED: break; case HWDISCOVERYF4_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_HID */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ /* Configure the main IO port */ uint8_t hw_mainport; HwDiscoveryF4MainPortGet(&hw_mainport); switch (hw_mainport) { case HWDISCOVERYF4_MAINPORT_DISABLED: break; case HWDISCOVERYF4_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); #endif /* PIOS_INCLUDE_TELEMETRY_RF */ case HWDISCOVERYF4_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; } #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uintptr_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uintptr_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ #if defined(PIOS_INCLUDE_GPIO) PIOS_GPIO_Init(); #endif /* Make sure we have at least one telemetry link configured or else fail initialization */ PIOS_Assert(pios_com_telem_usb_id); }
void PIOS_Board_Init(void) { check_bor(); const struct pios_board_info * bdinfo = &pios_board_info_blob; // Make sure all the PWM outputs are low const struct pios_servo_cfg * servo_cfg = get_servo_cfg(bdinfo->board_rev); const struct pios_tim_channel * channels = servo_cfg->channels; uint8_t num_channels = servo_cfg->num_channels; for (int i = 0; i < num_channels; i++) { GPIO_Init(channels[i].pin.gpio, (GPIO_InitTypeDef*) &channels[i].pin.init); } /* Delay system */ PIOS_DELAY_Init(); #if defined(PIOS_INCLUDE_LED) const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); PIOS_Assert(led_cfg); PIOS_LED_Init(led_cfg); #endif /* PIOS_INCLUDE_LED */ /* Set up the SPI interface to the gyro/acelerometer */ if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the flash and rfm22b */ if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_FLASH) /* Inititialize all flash drivers */ #if defined(PIOS_INCLUDE_FLASH_JEDEC) if (get_external_flash(bdinfo->board_rev)) { if (PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_telem_flash_id, 1, &flash_m25p_cfg) != 0) panic(1); } #endif /* PIOS_INCLUDE_FLASH_JEDEC */ PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg); /* Register the partition table */ const struct pios_flash_partition * flash_partition_table; uint32_t num_partitions; flash_partition_table = PIOS_BOARD_HW_DEFS_GetPartitionTable(bdinfo->board_rev, &num_partitions); PIOS_FLASH_register_partition_table(flash_partition_table, num_partitions); /* Mount all filesystems */ if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, get_flashfs_settings_cfg(bdinfo->board_rev), FLASH_PARTITION_LABEL_SETTINGS)) panic(1); #if defined(PIOS_INCLUDE_FLASH_JEDEC) if (get_external_flash(bdinfo->board_rev)) { if (PIOS_FLASHFS_Logfs_Init(&pios_waypoints_settings_fs_id, &flashfs_waypoints_cfg, FLASH_PARTITION_LABEL_WAYPOINTS) != 0) panic(1); } #endif /* PIOS_INCLUDE_FLASH_JEDEC */ #if defined(ERASE_FLASH) PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); #endif #endif /* PIOS_INCLUDE_FLASH */ /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); HwSparky2Initialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif #ifndef ERASE_FLASH /* Initialize watchdog as early as possible to catch faults during init * but do it only if there is no debugger connected */ if ((CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) == 0) { PIOS_WDG_Init(); } #endif /* Initialize the alarms library */ AlarmsInitialize(); /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_3_cfg); PIOS_TIM_InitClock(&tim_5_cfg); PIOS_TIM_InitClock(&tim_8_cfg); PIOS_TIM_InitClock(&tim_9_cfg); PIOS_TIM_InitClock(&tim_12_cfg); NVIC_InitTypeDef tim_8_up_irq = { .NVIC_IRQChannel = TIM8_UP_TIM13_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelCmd = ENABLE, }; NVIC_Init(&tim_8_up_irq); /* IAP System Setup */ PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hw config to defaults */ HwSparky2SetDefaults(HwSparky2Handle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } //PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uintptr_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwSparky2USB_VCPPortGet(&hw_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hw_usb_vcpport = HWSPARKY2_USB_VCPPORT_DISABLED; } PIOS_HAL_ConfigureCDC(hw_usb_vcpport, pios_usb_id, &pios_usb_cdc_cfg); #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwSparky2USB_HIDPortGet(&hw_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hw_usb_hidport = HWSPARKY2_USB_HIDPORT_DISABLED; } PIOS_HAL_ConfigureHID(hw_usb_hidport, pios_usb_id, &pios_usb_hid_cfg); #endif /* PIOS_INCLUDE_USB_HID */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ /* Configure IO ports */ HwSparky2DSMxModeOptions hw_DSMxMode; HwSparky2DSMxModeGet(&hw_DSMxMode); /* Configure main USART port */ uint8_t hw_mainport; HwSparky2MainPortGet(&hw_mainport); PIOS_HAL_ConfigurePort(hw_mainport, &pios_usart_main_cfg, &pios_usart_com_driver, NULL, NULL, NULL, PIOS_LED_ALARM, &pios_usart_dsm_hsum_main_cfg, &pios_dsm_main_cfg, hw_DSMxMode, NULL, NULL, false); /* Configure FlexiPort */ uint8_t hw_flexiport; HwSparky2FlexiPortGet(&hw_flexiport); PIOS_HAL_ConfigurePort(hw_flexiport, &pios_usart_flexi_cfg, &pios_usart_com_driver, &pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg, NULL, PIOS_LED_ALARM, &pios_usart_dsm_hsum_flexi_cfg, &pios_dsm_flexi_cfg, hw_DSMxMode, NULL, NULL, false); #if defined(PIOS_INCLUDE_RFM22B) HwSparky2Data hwSparky2; HwSparky2Get(&hwSparky2); const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(bdinfo->board_rev); PIOS_HAL_ConfigureRFM22B(hwSparky2.Radio, bdinfo->board_type, bdinfo->board_rev, hwSparky2.MaxRfPower, hwSparky2.MaxRfSpeed, openlrs_cfg, rfm22b_cfg, hwSparky2.MinChannel, hwSparky2.MaxChannel, hwSparky2.CoordID, 1); #endif /* PIOS_INCLUDE_RFM22B */ /* Configure the receiver port*/ uint8_t hw_rcvrport; HwSparky2RcvrPortGet(&hw_rcvrport); if (bdinfo->board_rev != BRUSHEDSPARKY_V0_2 && hw_DSMxMode >= HWSPARKY2_DSMXMODE_BIND3PULSES) { hw_DSMxMode = HWSPARKY2_DSMXMODE_AUTODETECT; /* Do not try to bind through XOR */ } PIOS_HAL_ConfigurePort(hw_rcvrport, NULL, /* XXX TODO: fix as part of DSM refactor */ &pios_usart_com_driver, NULL, NULL, &pios_ppm_cfg, PIOS_LED_ALARM, &pios_usart_dsm_hsum_rcvr_cfg, &pios_dsm_rcvr_cfg, hw_DSMxMode, get_sbus_rcvr_cfg(bdinfo->board_rev), &pios_sbus_cfg, get_sbus_toggle(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uintptr_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uintptr_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS /* Set up the servo outputs */ PIOS_Servo_Init(&pios_servo_cfg); #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_CAN) if(get_use_can(bdinfo->board_rev)) { if (PIOS_CAN_Init(&pios_can_id, &pios_can_cfg) != 0) panic(6); uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_CAN_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_CAN_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_can_id, &pios_can_com_driver, pios_can_id, rx_buffer, PIOS_COM_CAN_RX_BUF_LEN, tx_buffer, PIOS_COM_CAN_TX_BUF_LEN)) panic(6); /* pios_com_bridge_id = pios_com_can_id; */ } #endif PIOS_DELAY_WaitmS(50); PIOS_SENSORS_Init(); #if defined(PIOS_INCLUDE_ADC) uint32_t internal_adc_id; PIOS_INTERNAL_ADC_Init(&internal_adc_id, &pios_adc_cfg); PIOS_ADC_Init(&pios_internal_adc_id, &pios_internal_adc_driver, internal_adc_id); // configure the pullup for PA8 (inhibit pullups from current/sonar shared pin) GPIO_Init(pios_current_sonar_pin.gpio, &pios_current_sonar_pin.init); #endif #if defined(PIOS_INCLUDE_MS5611) if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id) != 0) panic(4); if (PIOS_MS5611_Test() != 0) panic(4); #endif uint8_t Magnetometer; HwSparky2MagnetometerGet(&Magnetometer); if (Magnetometer != HWSPARKY2_MAGNETOMETER_INTERNAL) pios_mpu9250_cfg.use_magnetometer = false; #if defined(PIOS_INCLUDE_MPU9250_SPI) if (PIOS_MPU9250_SPI_Init(pios_spi_gyro_id, 0, &pios_mpu9250_cfg) != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwSparky2GyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWSPARKY2_GYRORANGE_250: PIOS_MPU9250_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWSPARKY2_GYRORANGE_500: PIOS_MPU9250_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWSPARKY2_GYRORANGE_1000: PIOS_MPU9250_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWSPARKY2_GYRORANGE_2000: PIOS_MPU9250_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwSparky2AccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWSPARKY2_ACCELRANGE_2G: PIOS_MPU9250_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWSPARKY2_ACCELRANGE_4G: PIOS_MPU9250_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWSPARKY2_ACCELRANGE_8G: PIOS_MPU9250_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWSPARKY2_ACCELRANGE_16G: PIOS_MPU9250_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } // the filter has to be set before rate else divisor calculation will fail uint8_t hw_mpu9250_dlpf; HwSparky2MPU9250GyroLPFGet(&hw_mpu9250_dlpf); enum pios_mpu9250_gyro_filter mpu9250_gyro_lpf = \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_184) ? PIOS_MPU9250_GYRO_LOWPASS_184_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_92) ? PIOS_MPU9250_GYRO_LOWPASS_92_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_41) ? PIOS_MPU9250_GYRO_LOWPASS_41_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_20) ? PIOS_MPU9250_GYRO_LOWPASS_20_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_10) ? PIOS_MPU9250_GYRO_LOWPASS_10_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_5) ? PIOS_MPU9250_GYRO_LOWPASS_5_HZ : \ pios_mpu9250_cfg.default_gyro_filter; PIOS_MPU9250_SetGyroLPF(mpu9250_gyro_lpf); HwSparky2MPU9250AccelLPFGet(&hw_mpu9250_dlpf); enum pios_mpu9250_accel_filter mpu9250_accel_lpf = \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_460) ? PIOS_MPU9250_ACCEL_LOWPASS_460_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_184) ? PIOS_MPU9250_ACCEL_LOWPASS_184_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_92) ? PIOS_MPU9250_ACCEL_LOWPASS_92_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_41) ? PIOS_MPU9250_ACCEL_LOWPASS_41_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_20) ? PIOS_MPU9250_ACCEL_LOWPASS_20_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_10) ? PIOS_MPU9250_ACCEL_LOWPASS_10_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_5) ? PIOS_MPU9250_ACCEL_LOWPASS_5_HZ : \ pios_mpu9250_cfg.default_accel_filter; PIOS_MPU9250_SetAccelLPF(mpu9250_accel_lpf); uint8_t hw_mpu9250_samplerate; HwSparky2MPU9250RateGet(&hw_mpu9250_samplerate); uint16_t mpu9250_samplerate = \ (hw_mpu9250_samplerate == HWSPARKY2_MPU9250RATE_200) ? 200 : \ (hw_mpu9250_samplerate == HWSPARKY2_MPU9250RATE_250) ? 250 : \ (hw_mpu9250_samplerate == HWSPARKY2_MPU9250RATE_333) ? 333 : \ (hw_mpu9250_samplerate == HWSPARKY2_MPU9250RATE_500) ? 500 : \ (hw_mpu9250_samplerate == HWSPARKY2_MPU9250RATE_1000) ? 1000 : \ pios_mpu9250_cfg.default_samplerate; PIOS_MPU9250_SetSampleRate(mpu9250_samplerate); #endif /* PIOS_INCLUDE_MPU9250_SPI */ PIOS_WDG_Clear(); #if defined(PIOS_INCLUDE_HMC5883) { uint8_t Magnetometer; HwSparky2MagnetometerGet(&Magnetometer); if (Magnetometer == HWSPARKY2_MAGNETOMETER_EXTERNALI2CFLEXIPORT) { if (PIOS_HMC5883_Init(pios_i2c_flexiport_adapter_id, &pios_hmc5883_external_cfg) != 0) panic(6); if (PIOS_HMC5883_Test() != 0) panic(6); } else if (Magnetometer == HWSPARKY2_MAGNETOMETER_EXTERNALAUXI2C) { if (PIOS_HMC5883_Init(pios_i2c_mag_pressure_adapter_id, &pios_hmc5883_external_cfg) != 0) panic(6); if (PIOS_HMC5883_Test() != 0) panic(6); } if (Magnetometer != HWSPARKY2_MAGNETOMETER_INTERNAL) { // setup sensor orientation uint8_t ExtMagOrientation; HwSparky2ExtMagOrientationGet(&ExtMagOrientation); enum pios_hmc5883_orientation hmc5883_orientation = \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_TOP0DEGCW) ? PIOS_HMC5883_TOP_0DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_TOP90DEGCW) ? PIOS_HMC5883_TOP_90DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_TOP180DEGCW) ? PIOS_HMC5883_TOP_180DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_TOP270DEGCW) ? PIOS_HMC5883_TOP_270DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_BOTTOM0DEGCW) ? PIOS_HMC5883_BOTTOM_0DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_BOTTOM90DEGCW) ? PIOS_HMC5883_BOTTOM_90DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_BOTTOM180DEGCW) ? PIOS_HMC5883_BOTTOM_180DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_BOTTOM270DEGCW) ? PIOS_HMC5883_BOTTOM_270DEG : \ pios_hmc5883_external_cfg.Default_Orientation; PIOS_HMC5883_SetOrientation(hmc5883_orientation); } } #endif /* PIOS_INCLUDE_HMC5883 */ #if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) if (get_external_flash(bdinfo->board_rev)) { if ( PIOS_STREAMFS_Init(&streamfs_id, &streamfs_settings, FLASH_PARTITION_LABEL_LOG) != 0) panic(8); const uint32_t LOG_BUF_LEN = 256; uint8_t *log_rx_buffer = PIOS_malloc(LOG_BUF_LEN); uint8_t *log_tx_buffer = PIOS_malloc(LOG_BUF_LEN); if (PIOS_COM_Init(&pios_com_logging_id, &pios_streamfs_com_driver, streamfs_id, log_rx_buffer, LOG_BUF_LEN, log_tx_buffer, LOG_BUF_LEN) != 0) panic(9); } #endif /* PIOS_INCLUDE_FLASH */ switch (bdinfo->board_rev) { case BRUSHEDSPARKY_V0_2: { HwSparky2VTX_ChOptions channel; HwSparky2VTX_ChGet(&channel); set_vtx_channel(channel); } break; } }
/** * Module thread, should not return. */ static void groundPathFollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; uint32_t lastUpdateTime; GroundPathFollowerSettingsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); GroundPathFollowerSettingsGet(&guidanceSettings); PathDesiredGet(&pathDesired); // Main task loop lastUpdateTime = PIOS_Thread_Systime(); while (1) { // Conditions when this runs: // 1. Must have GROUND type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE) ) { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING); PIOS_Thread_Sleep(1000); continue; } // Continue collecting data if not enough time PIOS_Thread_Sleep_Until(&lastUpdateTime, guidanceSettings.UpdatePeriod); // Convert the accels into the NED frame updateNedAccel(); FlightStatusGet(&flightStatus); // Check the combinations of flightmode and pathdesired mode switch(flightStatus.FlightMode) { /* This combination of RETURNTOHOME and HOLDPOSITION looks strange but * is correct. RETURNTOHOME mode uses HOLDPOSITION with the position * set to home */ case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateGroundDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateGroundDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT || pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateGroundDesiredAttitude(); } else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) { updatePathVelocity(); updateGroundDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; default: // Be cleaner and get rid of global variables northVelIntegral = 0; eastVelIntegral = 0; northPosIntegral = 0; eastPosIntegral = 0; // Track throttle before engaging this mode. Cheap system ident StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); throttleOffset = stabDesired.Throttle; break; } AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); } }
/** * Update system alarms */ static void updateSystemAlarms() { SystemStatsData stats; UAVObjStats objStats; EventStats evStats; SystemStatsGet(&stats); // Check heap, IRQ stack and malloc failures if (PIOS_heap_malloc_failed_p() || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL) #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL) #endif ) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL); } else if ( (stats.HeapRemaining < HEAP_LIMIT_WARNING) #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING) #endif ) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY); } // Check CPU load if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) { AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL); } else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) { AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD); } // Check for stack overflow if (stackOverflow) { AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL); } else { AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW); } // Check for event errors UAVObjGetStats(&objStats); EventGetStats(&evStats); UAVObjClearStats(); EventClearStats(); if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) { AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM); } if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) { SystemStatsData sysStats; SystemStatsGet(&sysStats); sysStats.EventSystemWarningID = evStats.lastErrorID; sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID; sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID; SystemStatsSet(&sysStats); } }
/** * Set an alarm to it's default value * @param alarm The system alarm to be modified * @return 0 if success, -1 if an error */ int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm) { return AlarmsSet(alarm, SYSTEMALARMS_ALARM_DEFAULT); }
void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); const struct pios_board_info * bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); PIOS_Assert(led_cfg); PIOS_LED_Init(led_cfg); #endif /* PIOS_INCLUDE_LED */ /* Set up the SPI interface to the gyro/acelerometer */ if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the flash and rfm22b */ if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_FLASH) /* Connect flash to the approrpiate interface and configure it */ uintptr_t flash_id; if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1, &flash_m25p_cfg) != 0) panic(1); uintptr_t fs_id; if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id) != 0) panic(1); #endif /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); HwFreedomInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* Initialize the alarms library */ AlarmsInitialize(); /* Initialize the task monitor library */ TaskMonitorInitialize(); // /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); /* IAP System Setup */ PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hw config to defaults */ HwFreedomSetDefaults(HwFreedomHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uint32_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwFreedomUSB_VCPPortGet(&hw_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hw_usb_vcpport = HWFREEDOM_USB_VCPPORT_DISABLED; } switch (hw_usb_vcpport) { case HWFREEDOM_USB_VCPPORT_DISABLED: break; case HWFREEDOM_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uint32_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwFreedomUSB_HIDPortGet(&hw_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hw_usb_hidport = HWFREEDOM_USB_HIDPORT_DISABLED; } switch (hw_usb_hidport) { case HWFREEDOM_USB_HIDPORT_DISABLED: break; case HWFREEDOM_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_HID */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ /* Configure IO ports */ uint8_t hw_DSMxBind; HwFreedomDSMxBindGet(&hw_DSMxBind); /* Configure FlexiPort */ uint8_t hw_mainport; HwFreedomMainPortGet(&hw_mainport); switch (hw_mainport) { case HWFREEDOM_MAINPORT_DISABLED: break; case HWFREEDOM_MAINPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; break; case HWFREEDOM_MAINPORT_GPS: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_MAINPORT_DSM2: case HWFREEDOM_MAINPORT_DSMX10BIT: case HWFREEDOM_MAINPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_mainport) { case HWFREEDOM_MAINPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_MAINPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } break; case HWFREEDOM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_MAINPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_freedom_mainport */ /* Configure flexi USART port */ uint8_t hw_flexiport; HwFreedomFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_DISABLED: break; case HWFREEDOM_FLEXIPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWFREEDOM_FLEXIPORT_GPS: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_FLEXIPORT_DSM2: case HWFREEDOM_FLEXIPORT_DSMX10BIT: case HWFREEDOM_FLEXIPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_FLEXIPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_FLEXIPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hw_DSMxBind); } break; case HWFREEDOM_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ case HWFREEDOM_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_freedom_flexiport */ /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) uint8_t hwsettings_radioport; HwFreedomRadioPortGet(&hwsettings_radioport); switch (hwsettings_radioport) { case HWFREEDOM_RADIOPORT_DISABLED: break; case HWFREEDOM_RADIOPORT_TELEMETRY: { const struct pios_board_info * bdinfo = &pios_board_info_blob; const struct pios_rfm22b_cfg *pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) { PIOS_Assert(0); } uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } break; } } #endif /* PIOS_INCLUDE_RFM22B */ /* Configure input receiver USART port */ uint8_t hw_rcvrport; HwFreedomRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_DISABLED: break; case HWFREEDOM_RCVRPORT_PPM: { uint32_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); uint32_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } break; case HWFREEDOM_RCVRPORT_DSM2: case HWFREEDOM_RCVRPORT_DSMX10BIT: case HWFREEDOM_RCVRPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_RCVRPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_RCVRPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_rcvr_cfg, &pios_dsm_rcvr_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hw_DSMxBind); } break; case HWFREEDOM_RCVRPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uint32_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_rcvr_cfg)) { PIOS_Assert(0); } uint32_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } uint32_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; } #endif break; } if (hw_rcvrport != HWFREEDOM_RCVRPORT_SBUS) { GPIO_Init(pios_sbus_cfg.inv.gpio, (GPIO_InitTypeDef*)&pios_sbus_cfg.inv.init); GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); } #if defined(PIOS_OVERO_SPI) /* Set up the SPI based PIOS_COM interface to the overo */ { bool overo_enabled = false; #ifdef MODULE_OveroSync_BUILTIN overo_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_OVEROSYNC] == MODULESETTINGS_ADMINSTATE_ENABLED) { overo_enabled = true; } else { overo_enabled = false; } #endif if (overo_enabled) { if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { PIOS_DEBUG_Assert(0); } const uint32_t PACKET_SIZE = 1024; uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, rx_buffer, PACKET_SIZE, tx_buffer, PACKET_SIZE)) { PIOS_Assert(0); } } } #endif #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uint32_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uint32_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS uint8_t hw_output_port; HwFreedomOutputGet(&hw_output_port); switch (hw_output_port) { case HWFREEDOM_OUTPUT_DISABLED: break; case HWFREEDOM_OUTPUT_PWM: /* Set up the servo outputs */ PIOS_Servo_Init(&pios_servo_cfg); break; default: break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif PIOS_DELAY_WaitmS(200); PIOS_SENSORS_Init(); #if defined(PIOS_INCLUDE_MPU6000) if (PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg) != 0) panic(2); if (PIOS_MPU6000_Test() != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwFreedomGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWFREEDOM_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWFREEDOM_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWFREEDOM_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWFREEDOM_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwFreedomAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWFREEDOM_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWFREEDOM_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWFREEDOM_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWFREEDOM_ACCELRANGE_16G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } uint8_t hw_mpu6000_rate; HwFreedomMPU6000RateGet(&hw_mpu6000_rate); uint16_t hw_mpu6000_divisor = \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_500) ? 15 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_666) ? 11 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_1000) ? 7 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_2000) ? 3 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_4000) ? 1 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_8000) ? 0 : \ 15; PIOS_MPU6000_SetDivisor(hw_mpu6000_divisor); uint8_t hw_dlpf; HwFreedomMPU6000DLPFGet(&hw_dlpf); uint16_t hw_mpu6000_dlpf = \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ PIOS_MPU60X0_LOWPASS_256_HZ; PIOS_MPU6000_SetLPF(hw_mpu6000_dlpf); #endif if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_mag_pressure_adapter_id) != 0) panic(5); PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) PIOS_ADC_Init(&pios_adc_cfg); #endif PIOS_LED_On(0); #if defined(PIOS_INCLUDE_HMC5883) if (PIOS_HMC5883_Init(pios_i2c_mag_pressure_adapter_id, &pios_hmc5883_cfg) != 0) panic(3); #endif PIOS_LED_On(1); #if defined(PIOS_INCLUDE_MS5611) if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id) != 0) panic(4); #endif PIOS_LED_On(2); }
void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); const struct pios_board_info * bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); PIOS_Assert(led_cfg); PIOS_LED_Init(led_cfg); #endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the serial flash */ switch(bdinfo->board_rev) { case BOARD_REVISION_CC: if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) { PIOS_Assert(0); } break; case BOARD_REVISION_CC3D: if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { PIOS_Assert(0); } break; default: PIOS_Assert(0); } #endif switch(bdinfo->board_rev) { case BOARD_REVISION_CC: PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_flash_accel_id, 1, &flash_w25x_cfg); break; case BOARD_REVISION_CC3D: PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_flash_accel_id, 0, &flash_m25p_cfg); break; default: PIOS_DEBUG_Assert(0); } PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg); /* Register the partition table */ const struct pios_flash_partition * flash_partition_table; uint32_t num_partitions; flash_partition_table = PIOS_BOARD_HW_DEFS_GetPartitionTable(bdinfo->board_rev, &num_partitions); PIOS_FLASH_register_partition_table(flash_partition_table, num_partitions); /* Mount all filesystems */ PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS); /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); /* Initialize the alarms library */ AlarmsInitialize(); HwCopterControlInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); #endif #ifndef ERASE_FLASH /* Initialize watchdog as early as possible to catch faults during init */ PIOS_WDG_Init(); #endif /* Check for repeated boot failures */ PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hw configuration to defaults */ HwCopterControlSetDefaults(HwCopterControlHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); PIOS_TIM_InitClock(&tim_4_cfg); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uintptr_t pios_usb_id; switch(bdinfo->board_rev) { case BOARD_REVISION_CC: PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); break; case BOARD_REVISION_CC3D: PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); break; default: PIOS_Assert(0); } #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwCopterControlUSB_VCPPortGet(&hw_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hw_usb_vcpport = HWCOPTERCONTROL_USB_VCPPORT_DISABLED; } switch (hw_usb_vcpport) { case HWCOPTERCONTROL_USB_VCPPORT_DISABLED: break; case HWCOPTERCONTROL_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uintptr_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWCOPTERCONTROL_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { uintptr_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWCOPTERCONTROL_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uintptr_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwCopterControlUSB_HIDPortGet(&hw_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hw_usb_hidport = HWCOPTERCONTROL_USB_HIDPORT_DISABLED; } switch (hw_usb_hidport) { case HWCOPTERCONTROL_USB_HIDPORT_DISABLED: break; case HWCOPTERCONTROL_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uintptr_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWCOPTERCONTROL_USB_HIDPORT_RCTRANSMITTER: #if defined(PIOS_INCLUDE_USB_RCTX) { if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_USB_RCTX */ break; } #endif /* PIOS_INCLUDE_USB_HID */ #endif /* PIOS_INCLUDE_USB */ /* Configure the main IO port */ HwCopterControlDSMxModeOptions hw_DSMxMode; HwCopterControlDSMxModeGet(&hw_DSMxMode); uint8_t hw_mainport; HwCopterControlMainPortGet(&hw_mainport); switch (hw_mainport) { case HWCOPTERCONTROL_MAINPORT_DISABLED: break; case HWCOPTERCONTROL_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWCOPTERCONTROL_MAINPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } uintptr_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; } #endif /* PIOS_INCLUDE_SBUS */ break; case HWCOPTERCONTROL_MAINPORT_GPS: #if defined(PIOS_INCLUDE_GPS) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(rx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_GPS */ break; case HWCOPTERCONTROL_MAINPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { uintptr_t pios_usart_dsm_id; if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_hsum_main_cfg)) { PIOS_Assert(0); } if (hw_DSMxMode >= HWCOPTERCONTROL_DSMXMODE_BIND3PULSES) { hw_DSMxMode = HWCOPTERCONTROL_DSMXMODE_AUTODETECT; /* Do not try to bind through XOR */ } uintptr_t pios_dsm_id; if (PIOS_DSM_Init(&pios_dsm_id, &pios_dsm_main_cfg, &pios_usart_com_driver, pios_usart_dsm_id, hw_DSMxMode)) { PIOS_Assert(0); } uintptr_t pios_dsm_rcvr_id; if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM] = pios_dsm_rcvr_id; } #endif /* PIOS_INCLUDE_DSM */ break; case HWCOPTERCONTROL_MAINPORT_HOTTSUMD: case HWCOPTERCONTROL_MAINPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; proto = (hw_mainport == HWCOPTERCONTROL_MAINPORT_HOTTSUMD) ? PIOS_HSUM_PROTO_SUMD : PIOS_HSUM_PROTO_SUMH; uintptr_t pios_usart_hsum_id; if (PIOS_USART_Init(&pios_usart_hsum_id, &pios_usart_dsm_hsum_main_cfg)) { PIOS_Assert(0); } uintptr_t pios_hsum_id; if (PIOS_HSUM_Init(&pios_hsum_id, &pios_usart_com_driver, pios_usart_hsum_id, proto)) { PIOS_Assert(0); } uintptr_t pios_hsum_rcvr_id; if (PIOS_RCVR_Init(&pios_hsum_rcvr_id, &pios_hsum_rcvr_driver, pios_hsum_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM] = pios_hsum_rcvr_id; } #endif /* PIOS_INCLUDE_HSUM */ break; case HWCOPTERCONTROL_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; case HWCOPTERCONTROL_MAINPORT_COMBRIDGE: { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); PIOS_Assert(rx_buffer); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } break; case HWCOPTERCONTROL_MAINPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_MAVLINK_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWCOPTERCONTROL_MAINPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_MAVLINK) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_GPS_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, tx_buffer, PIOS_COM_MAVLINK_TX_BUF_LEN)) { PIOS_Assert(0); } pios_com_mavlink_id = pios_com_gps_id; } #endif /* PIOS_INCLUDE_MAVLINK */ #endif /* PIOS_INCLUDE_GPS */ break; case HWCOPTERCONTROL_MAINPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_frsky_sensor_hub_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_FRSKYSENSORHUB */ break; case HWCOPTERCONTROL_MAINPORT_LIGHTTELEMETRYTX: { #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_lighttelemetry_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN)) { PIOS_Assert(0); } #endif } break; } /* Configure the flexi port */ uint8_t hw_flexiport; HwCopterControlFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWCOPTERCONTROL_FLEXIPORT_DISABLED: break; case HWCOPTERCONTROL_FLEXIPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWCOPTERCONTROL_FLEXIPORT_COMBRIDGE: { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } break; case HWCOPTERCONTROL_FLEXIPORT_GPS: #if defined(PIOS_INCLUDE_GPS) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(rx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_GPS */ break; case HWCOPTERCONTROL_FLEXIPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { uintptr_t pios_usart_dsm_id; if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_hsum_flexi_cfg)) { PIOS_Assert(0); } uintptr_t pios_dsm_id; if (PIOS_DSM_Init(&pios_dsm_id, &pios_dsm_flexi_cfg, &pios_usart_com_driver, pios_usart_dsm_id, hw_DSMxMode)) { PIOS_Assert(0); } uintptr_t pios_dsm_rcvr_id; if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM] = pios_dsm_rcvr_id; } #endif /* PIOS_INCLUDE_DSM */ break; case HWCOPTERCONTROL_FLEXIPORT_HOTTSUMD: case HWCOPTERCONTROL_FLEXIPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; proto = (hw_flexiport == HWCOPTERCONTROL_FLEXIPORT_HOTTSUMD) ? PIOS_HSUM_PROTO_SUMD : PIOS_HSUM_PROTO_SUMH; uintptr_t pios_usart_hsum_id; if (PIOS_USART_Init(&pios_usart_hsum_id, &pios_usart_dsm_hsum_flexi_cfg)) { PIOS_Assert(0); } uintptr_t pios_hsum_id; if (PIOS_HSUM_Init(&pios_hsum_id, &pios_usart_com_driver, pios_usart_hsum_id, proto)) { PIOS_Assert(0); } uintptr_t pios_hsum_rcvr_id; if (PIOS_RCVR_Init(&pios_hsum_rcvr_id, &pios_hsum_rcvr_driver, pios_hsum_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM] = pios_hsum_rcvr_id; } #endif /* PIOS_INCLUDE_HSUM */ break; case HWCOPTERCONTROL_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; case HWCOPTERCONTROL_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ #if defined(PIOS_INCLUDE_PCF8591) { uintptr_t pcf8591_adc_id; if(PIOS_PCF8591_ADC_Init(&pcf8591_adc_id, &pios_8591_cfg) < 0) PIOS_Assert(0); PIOS_ADC_Init(&pios_pcf8591_adc_id, &pios_pcf8591_adc_driver, pcf8591_adc_id); } #endif break; case HWCOPTERCONTROL_FLEXIPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_MAVLINK_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWCOPTERCONTROL_FLEXIPORT_LIGHTTELEMETRYTX: { #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_lighttelemetry_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN)) { PIOS_Assert(0); } #endif case HWCOPTERCONTROL_FLEXIPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_frsky_sensor_hub_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_FRSKYSENSORHUB */ break; } break; } /* Configure the rcvr port */ uint8_t hw_rcvrport; HwCopterControlRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWCOPTERCONTROL_RCVRPORT_DISABLED: break; case HWCOPTERCONTROL_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) { uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); uintptr_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; case HWCOPTERCONTROL_RCVRPORT_PPM: case HWCOPTERCONTROL_RCVRPORT_PPMONPIN8: case HWCOPTERCONTROL_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_PPM) { uintptr_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, (hw_rcvrport == HWCOPTERCONTROL_RCVRPORT_PPMONPIN8) ? &pios_ppm_pin8_cfg : &pios_ppm_cfg); uintptr_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } #endif /* PIOS_INCLUDE_PPM */ break; case HWCOPTERCONTROL_RCVRPORT_PPMPWM: /* This is a combination of PPM and PWM inputs */ #if defined(PIOS_INCLUDE_PPM) { uintptr_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); uintptr_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } #endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PWM) { uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); uintptr_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; } #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uintptr_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uintptr_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ /* Remap AFIO pin for PB4 (Servo 5 Out)*/ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS switch (hw_rcvrport) { case HWCOPTERCONTROL_RCVRPORT_DISABLED: case HWCOPTERCONTROL_RCVRPORT_PWM: case HWCOPTERCONTROL_RCVRPORT_PPM: case HWCOPTERCONTROL_RCVRPORT_PPMONPIN8: case HWCOPTERCONTROL_RCVRPORT_PPMPWM: PIOS_Servo_Init(&pios_servo_cfg); break; case HWCOPTERCONTROL_RCVRPORT_PPMOUTPUTS: case HWCOPTERCONTROL_RCVRPORT_OUTPUTS: PIOS_Servo_Init(&pios_servo_rcvr_cfg); break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ PIOS_SENSORS_Init(); switch(bdinfo->board_rev) { case BOARD_REVISION_CC: // Revision 1 with invensense gyros, start the ADC #if defined(PIOS_INCLUDE_ADC) { uint32_t internal_adc_id; PIOS_INTERNAL_ADC_Init(&internal_adc_id, &internal_adc_cfg); PIOS_ADC_Init(&pios_internal_adc_id, &pios_internal_adc_driver, internal_adc_id); } #endif #if defined(PIOS_INCLUDE_ADXL345) PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); #endif break; case BOARD_REVISION_CC3D: // Revision 2 with L3GD20 gyros, start a SPI interface and connect to it GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); #if defined(PIOS_INCLUDE_MPU6000) // Set up the SPI interface to the serial flash if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_Assert(0); } PIOS_MPU6000_Init(pios_spi_gyro_id,0,&pios_mpu6000_cfg); init_test = PIOS_MPU6000_Test(); uint8_t hw_gyro_range; HwCopterControlGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWCOPTERCONTROL_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWCOPTERCONTROL_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWCOPTERCONTROL_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWCOPTERCONTROL_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwCopterControlAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWCOPTERCONTROL_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWCOPTERCONTROL_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWCOPTERCONTROL_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWCOPTERCONTROL_ACCELRANGE_16G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } // the filter has to be set before rate else divisor calculation will fail uint8_t hw_mpu6000_dlpf; HwCopterControlMPU6000DLPFGet(&hw_mpu6000_dlpf); enum pios_mpu60x0_filter mpu6000_dlpf = \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ pios_mpu6000_cfg.default_filter; PIOS_MPU6000_SetLPF(mpu6000_dlpf); uint8_t hw_mpu6000_samplerate; HwCopterControlMPU6000RateGet(&hw_mpu6000_samplerate); uint16_t mpu6000_samplerate = \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_200) ? 200 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_333) ? 333 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_500) ? 500 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_666) ? 666 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_1000) ? 1000 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_2000) ? 2000 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_4000) ? 4000 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_8000) ? 8000 : \ pios_mpu6000_cfg.default_samplerate; PIOS_MPU6000_SetSampleRate(mpu6000_samplerate); #endif /* PIOS_INCLUDE_MPU6000 */ break; default: PIOS_Assert(0); } PIOS_GPIO_Init(); /* Make sure we have at least one telemetry link configured or else fail initialization */ PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); }
/** * Module task */ static void stabilizationTask(void* parameters) { portTickType lastSysTime; portTickType thisSysTime; UAVObjEvent ev; ActuatorDesiredData actuatorDesired; StabilizationDesiredData stabDesired; RateDesiredData rateDesired; AttitudeActualData attitudeActual; AttitudeRawData attitudeRaw; SystemSettingsData systemSettings; FlightStatusData flightStatus; SettingsUpdatedCb((UAVObjEvent *) NULL); // Main task loop lastSysTime = xTaskGetTickCount(); ZeroPids(); while(1) { PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); continue; } // Check how long since last update thisSysTime = xTaskGetTickCount(); if(thisSysTime > lastSysTime) // reuse dt in case of wraparound dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; lastSysTime = thisSysTime; FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); AttitudeRawGet(&attitudeRaw); RateDesiredGet(&rateDesired); SystemSettingsGet(&systemSettings); #if defined(PIOS_QUATERNION_STABILIZATION) // Quaternion calculation of error in each axis. Uses more memory. float rpy_desired[3]; float q_desired[4]; float q_error[4]; float local_error[3]; // Essentially zero errors for anything in rate or none if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[0] = stabDesired.Roll; else rpy_desired[0] = attitudeActual.Roll; if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[1] = stabDesired.Pitch; else rpy_desired[1] = attitudeActual.Pitch; if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[2] = stabDesired.Yaw; else rpy_desired[2] = attitudeActual.Yaw; RPY2Quaternion(rpy_desired, q_desired); quat_inverse(q_desired); quat_mult(q_desired, &attitudeActual.q1, q_error); quat_inverse(q_error); Quaternion2RPY(q_error, local_error); #else // Simpler algorithm for CC, less memory float local_error[3] = {stabDesired.Roll - attitudeActual.Roll, stabDesired.Pitch - attitudeActual.Pitch, stabDesired.Yaw - attitudeActual.Yaw}; local_error[2] = fmod(local_error[2] + 180, 360) - 180; #endif for(uint8_t i = 0; i < MAX_AXES; i++) { gyro_filtered[i] = gyro_filtered[i] * gyro_alpha + attitudeRaw.gyros[i] * (1 - gyro_alpha); } float *attitudeDesiredAxis = &stabDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; //Calculate desired rate for(uint8_t i=0; i< MAX_AXES; i++) { switch(stabDesired.StabilizationMode[i]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: rateDesiredAxis[i] = attitudeDesiredAxis[i]; axis_lock_accum[i] = 0; break; case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { float weak_leveling = local_error[i] * weak_leveling_kp; if(weak_leveling > weak_leveling_max) weak_leveling = weak_leveling_max; if(weak_leveling < -weak_leveling_max) weak_leveling = -weak_leveling_max; rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; axis_lock_accum[i] = 0; break; } case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]); axis_lock_accum[i] = 0; break; case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) { // While getting strong commands act like rate mode rateDesiredAxis[i] = attitudeDesiredAxis[i]; axis_lock_accum[i] = 0; } else { // For weaker commands or no command simply attitude lock (almost) on no gyro change axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT; if(axis_lock_accum[i] > max_axis_lock) axis_lock_accum[i] = max_axis_lock; else if(axis_lock_accum[i] < -max_axis_lock) axis_lock_accum[i] = -max_axis_lock; rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i]); } break; } } uint8_t shouldUpdate = 1; RateDesiredSet(&rateDesired); ActuatorDesiredGet(&actuatorDesired); //Calculate desired command for(int8_t ct=0; ct< MAX_AXES; ct++) { if(rateDesiredAxis[ct] > settings.MaximumRate[ct]) rateDesiredAxis[ct] = settings.MaximumRate[ct]; else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct]) rateDesiredAxis[ct] = -settings.MaximumRate[ct]; switch(stabDesired.StabilizationMode[ct]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct] - gyro_filtered[ct]); actuatorDesiredAxis[ct] = bound(command); break; } case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: switch (ct) { case ROLL: actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); shouldUpdate = 1; break; case PITCH: actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); shouldUpdate = 1; break; case YAW: actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); shouldUpdate = 1; break; } break; } } // Save dT actuatorDesired.UpdateTime = dT * 1000; if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL) shouldUpdate = 0; if(shouldUpdate) { actuatorDesired.Throttle = stabDesired.Throttle; if(dT > 15) actuatorDesired.NumLongUpdates++; ActuatorDesiredSet(&actuatorDesired); } if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || (lowThrottleZeroIntegral && stabDesired.Throttle < 0) || !shouldUpdate) { ZeroPids(); } // Clear alarms AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } }
static void onTimer(UAVObjEvent* ev) { static FlightBatteryStateData flightBatteryData; FlightBatterySettingsData batterySettings; FlightBatterySettingsGet(&batterySettings); static float dT = SAMPLE_PERIOD_MS / 1000.0f; float energyRemaining; //calculate the battery parameters if (voltageADCPin >=0) { flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(voltageADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts } else { flightBatteryData.Voltage=1234; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } if (currentADCPin >=0) { flightBatteryData.Current = ((float)PIOS_ADC_PinGet(currentADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in Amps if (flightBatteryData.Current > flightBatteryData.PeakCurrent) flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps } else { //If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm flightBatteryData.Current=-0.1234f; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f) ;//in mAh //Apply a 2 second rise time low-pass filter to average the current float alpha = 1.0f-dT/(dT+2.0f); flightBatteryData.AvgCurrent=alpha*flightBatteryData.AvgCurrent+(1-alpha)*flightBatteryData.Current; //in Amps /*The motor could regenerate power. Or we could have solar cells. In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple sign check doesn't catch this.*/ // //sanity checks // if (flightBatteryData.AvgCurrent<0) flightBatteryData.AvgCurrent=0.0f; // if (flightBatteryData.PeakCurrent<0) flightBatteryData.PeakCurrent=0.0f; // if (flightBatteryData.ConsumedEnergy<0) flightBatteryData.ConsumedEnergy=0.0f; energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh if (flightBatteryData.AvgCurrent > 0) flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent*1000.0f))*3600.0f;//in Sec else flightBatteryData.EstimatedFlightTime = 9999; //generate alarms where needed... if ((flightBatteryData.Voltage<=0) && (flightBatteryData.Current<=0)) { //FIXME: There's no guarantee that a floating ADC will give 0. So this // check might fail, even when there's nothing attached. AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR); } else { // FIXME: should make the timer alarms user configurable if (flightBatteryData.EstimatedFlightTime < 30) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.EstimatedFlightTime < 120) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME); // FIXME: should make the battery voltage detection dependent on battery type. /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); } FlightBatteryStateSet(&flightBatteryData); }
static void SensorsTask(void *parameters) { portTickType lastSysTime; uint32_t accel_samples = 0; uint32_t gyro_samples = 0; int32_t accel_accum[3] = {0, 0, 0}; int32_t gyro_accum[3] = {0,0,0}; float gyro_scaling = 0; float accel_scaling = 0; static int32_t timeval; AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); UAVObjEvent ev; settingsUpdatedCb(&ev); const struct pios_board_info * bdinfo = &pios_board_info_blob; switch(bdinfo->board_rev) { case 0x01: #if defined(PIOS_INCLUDE_L3GD20) gyro_test = PIOS_L3GD20_Test(); #endif #if defined(PIOS_INCLUDE_BMA180) accel_test = PIOS_BMA180_Test(); #endif break; case 0x02: #if defined(PIOS_INCLUDE_MPU6000) gyro_test = PIOS_MPU6000_Test(); accel_test = gyro_test; #endif break; default: PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_HMC5883) mag_test = PIOS_HMC5883_Test(); #else mag_test = 0; #endif if(accel_test < 0 || gyro_test < 0 || mag_test < 0) { AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); while(1) { PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); vTaskDelay(10); } } // Main task loop lastSysTime = xTaskGetTickCount(); bool error = false; uint32_t mag_update_time = PIOS_DELAY_GetRaw(); while (1) { // TODO: add timeouts to the sensor reads and set an error if the fail sensor_dt_us = PIOS_DELAY_DiffuS(timeval); timeval = PIOS_DELAY_GetRaw(); if (error) { PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); lastSysTime = xTaskGetTickCount(); vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); error = false; } else { AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); } for (int i = 0; i < 3; i++) { accel_accum[i] = 0; gyro_accum[i] = 0; } accel_samples = 0; gyro_samples = 0; AccelsData accelsData; GyrosData gyrosData; switch(bdinfo->board_rev) { case 0x01: // L3GD20 + BMA180 board #if defined(PIOS_INCLUDE_BMA180) { struct pios_bma180_data accel; int32_t read_good; int32_t count; count = 0; while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; if (error) { // Unfortunately if the BMA180 ever misses getting read, then it will not // trigger more interrupts. In this case we must force a read to kickstarts // it. struct pios_bma180_data data; PIOS_BMA180_ReadAccels(&data); continue; } while(read_good == 0) { count++; accel_accum[1] += accel.x; accel_accum[0] += accel.y; accel_accum[2] -= accel.z; read_good = PIOS_BMA180_ReadFifo(&accel); } accel_samples = count; accel_scaling = PIOS_BMA180_GetScale(); // Get temp from last reading accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f; } #endif #if defined(PIOS_INCLUDE_L3GD20) { struct pios_l3gd20_data gyro; gyro_samples = 0; xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue(); if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) { error = true; continue; } gyro_samples = 1; gyro_accum[1] += gyro.gyro_x; gyro_accum[0] += gyro.gyro_y; gyro_accum[2] -= gyro.gyro_z; gyro_scaling = PIOS_L3GD20_GetScale(); // Get temp from last reading gyrosData.temperature = gyro.temperature; } #endif break; case 0x02: // MPU6000 board case 0x03: // MPU6000 board #if defined(PIOS_INCLUDE_MPU6000) { struct pios_mpu6000_data mpu6000_data; xQueueHandle queue = PIOS_MPU6000_GetQueue(); while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) { gyro_accum[0] += mpu6000_data.gyro_x; gyro_accum[1] += mpu6000_data.gyro_y; gyro_accum[2] += mpu6000_data.gyro_z; accel_accum[0] += mpu6000_data.accel_x; accel_accum[1] += mpu6000_data.accel_y; accel_accum[2] += mpu6000_data.accel_z; gyro_samples ++; accel_samples ++; } if (gyro_samples == 0) { PIOS_MPU6000_ReadGyros(&mpu6000_data); error = true; continue; } gyro_scaling = PIOS_MPU6000_GetScale(); accel_scaling = PIOS_MPU6000_GetAccelScale(); gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; } #endif /* PIOS_INCLUDE_MPU6000 */ break; default: PIOS_DEBUG_Assert(0); } // Scale the accels float accels[3] = {(float) accel_accum[0] / accel_samples, (float) accel_accum[1] / accel_samples, (float) accel_accum[2] / accel_samples}; float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0], accels[1] * accel_scaling * accel_scale[1] - accel_bias[1], accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]}; if (rotate) { rot_mult(R, accels_out, accels); accelsData.x = accels[0]; accelsData.y = accels[1]; accelsData.z = accels[2]; } else { accelsData.x = accels_out[0]; accelsData.y = accels_out[1]; accelsData.z = accels_out[2]; } AccelsSet(&accelsData); // Scale the gyros float gyros[3] = {(float) gyro_accum[0] / gyro_samples, (float) gyro_accum[1] / gyro_samples, (float) gyro_accum[2] / gyro_samples}; float gyros_out[3] = {gyros[0] * gyro_scaling, gyros[1] * gyro_scaling, gyros[2] * gyro_scaling}; if (rotate) { rot_mult(R, gyros_out, gyros); gyrosData.x = gyros[0]; gyrosData.y = gyros[1]; gyrosData.z = gyros[2]; } else { gyrosData.x = gyros_out[0]; gyrosData.y = gyros_out[1]; gyrosData.z = gyros_out[2]; } if (bias_correct_gyro) { // Apply bias correction to the gyros from the state estimator GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); gyrosData.x -= gyrosBias.x; gyrosData.y -= gyrosBias.y; gyrosData.z -= gyrosBias.z; } GyrosSet(&gyrosData); // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) #if defined(PIOS_INCLUDE_HMC5883) MagnetometerData mag; if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { int16_t values[3]; PIOS_HMC5883_ReadMag(values); float mags[3] = {-(float) values[1] * mag_scale[0] - mag_bias[0], -(float) values[0] * mag_scale[1] - mag_bias[1], -(float) values[2] * mag_scale[2] - mag_bias[2]}; if (rotate) { float mag_out[3]; rot_mult(R, mags, mag_out); mag.x = mag_out[0]; mag.y = mag_out[1]; mag.z = mag_out[2]; } else { mag.x = mags[0]; mag.y = mags[1]; mag.z = mags[2]; } // Correct for mag bias and update if the rate is non zero if(cal.MagBiasNullingRate > 0) magOffsetEstimation(&mag); MagnetometerSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } #endif PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); lastSysTime = xTaskGetTickCount(); } }
/** * Clear an alarm * @param alarm The system alarm to be modified * @return 0 if success, -1 if an error */ int32_t AlarmsClear(SystemAlarmsAlarmElem alarm) { return AlarmsSet(alarm, SYSTEMALARMS_ALARM_OK); }
/** * Module thread, should not return. */ static void guidanceTask(void *parameters) { SystemSettingsData systemSettings; GuidanceSettingsData guidanceSettings; ManualControlCommandData manualControl; portTickType thisTime; portTickType lastUpdateTime; UAVObjEvent ev; float accel[3] = {0,0,0}; uint32_t accel_accum = 0; float q[4]; float Rbe[3][3]; float accel_ned[3]; // Main task loop lastUpdateTime = xTaskGetTickCount(); while (1) { GuidanceSettingsGet(&guidanceSettings); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(queue, &ev, guidanceSettings.UpdatePeriod / portTICK_RATE_MS) != pdTRUE ) { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); } // Collect downsampled attitude data AttitudeRawData attitudeRaw; AttitudeRawGet(&attitudeRaw); accel[0] += attitudeRaw.accels[0]; accel[1] += attitudeRaw.accels[1]; accel[2] += attitudeRaw.accels[2]; accel_accum++; // Continue collecting data if not enough time thisTime = xTaskGetTickCount(); if( (thisTime - lastUpdateTime) < (guidanceSettings.UpdatePeriod / portTICK_RATE_MS) ) continue; lastUpdateTime = xTaskGetTickCount(); accel[0] /= accel_accum; accel[1] /= accel_accum; accel[2] /= accel_accum; //rotate avg accels into earth frame and store it AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); q[0]=attitudeActual.q1; q[1]=attitudeActual.q2; q[2]=attitudeActual.q3; q[3]=attitudeActual.q4; Quaternion2R(q, Rbe); for (uint8_t i=0; i<3; i++){ accel_ned[i]=0; for (uint8_t j=0; j<3; j++) accel_ned[i] += Rbe[j][i]*accel[j]; } accel_ned[2] += 9.81; NedAccelData accelData; NedAccelGet(&accelData); // Convert from m/s to cm/s accelData.North = accel_ned[0] * 100; accelData.East = accel_ned[1] * 100; accelData.Down = accel_ned[2] * 100; NedAccelSet(&accelData); ManualControlCommandGet(&manualControl); SystemSettingsGet(&systemSettings); GuidanceSettingsGet(&guidanceSettings); if ((manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) && ((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) )) { if(positionHoldLast == 0) { /* When enter position hold mode save current position */ PositionDesiredData positionDesired; PositionActualData positionActual; PositionDesiredGet(&positionDesired); PositionActualGet(&positionActual); positionDesired.North = positionActual.North; positionDesired.East = positionActual.East; PositionDesiredSet(&positionDesired); positionHoldLast = 1; } if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_DUAL_LOOP) updateVtolDesiredVelocity(); else manualSetDesiredVelocity(); updateVtolDesiredAttitude(); } else { // Be cleaner and get rid of global variables northIntegral = 0; eastIntegral = 0; downIntegral = 0; positionHoldLast = 0; } accel[0] = accel[1] = accel[2] = 0; accel_accum = 0; } }