void plan_setup_AutoTakeoff() { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED; // We only allow takeoff if the state transition of disarmed to armed occurs // whilst in the autotake flight mode FlightStatusData flightStatus; FlightStatusGet(&flightStatus); StabilizationDesiredData stabiDesired; StabilizationDesiredGet(&stabiDesired); // Are we inflight? if (flightStatus.Armed && stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT) { // ok assume already in flight and just enter position hold // if we are not actually inflight this will just be a violent autotakeoff autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD; plan_setup_positionHold(); } else { if (flightStatus.Armed) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST; // Note that if this mode was invoked unintentionally whilst in flight, effectively // all inputs get ignored and the vtol continues to fly to its previous // stabi command. } PathDesiredData pathDesired; plan_setup_AutoTakeoff_helper(&pathDesired); PathDesiredSet(&pathDesired); } }
/** * If the system is disarmed, look for a variety of conditions that * make it unsafe to arm (that might not be dangerous to engage once * flying). */ static int32_t check_safe_to_arm() { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); // Only arm in traditional modes where pilot has control if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) { switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_MANUAL: case FLIGHTSTATUS_FLIGHTMODE_ACRO: case FLIGHTSTATUS_FLIGHTMODE_ACROPLUS: case FLIGHTSTATUS_FLIGHTMODE_LEVELING: case FLIGHTSTATUS_FLIGHTMODE_MWRATE: case FLIGHTSTATUS_FLIGHTMODE_HORIZON: case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK: case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: break; default: // Any mode not specifically allowed prevents arming return SYSTEMALARMS_CONFIGERROR_UNSAFETOARM; } } return SYSTEMALARMS_CONFIGERROR_NONE; }
/** * 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); } }
//! When the control system requests to disarm the FC static int32_t control_event_disarm() { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) { flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; FlightStatusSet(&flightStatus); } return 0; }
//! When the control system requests to start arming the FC static int32_t control_event_arming() { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMING) { flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMING; FlightStatusSet(&flightStatus); } return 0; }
/** * @brief Update the flightStatus object only if value changed. Reduces callbacks * @param[in] val The new value */ static void setArmedIfChanged(uint8_t val) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.Armed != val) { flightStatus.Armed = val; FlightStatusSet(&flightStatus); } }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { portTickType lastSysTime; /* create all modules thread */ MODULE_TASKCREATE_ALL // Initialize vars idleCounter = 0; idleCounterClear = 0; lastSysTime = xTaskGetTickCount(); // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectCallback(&objectUpdatedCb); // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); #endif // Update the task status object TaskMonitorUpdateAll(); // Flash the heartbeat LED PIOS_LED_Toggle(LED1); // Turn on the error LED if an alarm is set #if (PIOS_LED_NUM > 1) if (AlarmsHasErrors()) { PIOS_LED_Toggle(LED2); } else if (AlarmsHasWarnings()) { PIOS_LED_On(LED2); } else { PIOS_LED_Off(LED2); } #endif FlightStatusData flightStatus; FlightStatusGet(&flightStatus); // Wait until next period if(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) ); } else { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); } } }
/** * On changed waypoints or active waypoint update position desired * if we are in charge */ static void waypointsUpdated(UAVObjEvent * ev) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) return; WaypointActiveGet(&waypointActive); if(active_waypoint != waypointActive.Index) activateWaypoint(waypointActive.Index); }
/** * Module thread, should not return. */ static void AttitudeTask(void *parameters) { uint8_t init = 0; AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE); // Keep flash CS pin high while talking accel PIOS_FLASH_DISABLE; PIOS_ADXL345_Init(); // Force settings update to make sure rotation loaded settingsUpdatedCb(AttitudeSettingsHandle()); // Main task loop while (1) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { // For first 7 seconds use accels to get gyro bias accelKp = 1; accelKi = 0.9; yawBiasRate = 0.23; init = 0; } else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { accelKp = 1; accelKi = 0.9; yawBiasRate = 0.23; init = 0; } else if (init == 0) { // Reload settings (all the rates) AttitudeSettingsAccelKiGet(&accelKi); AttitudeSettingsAccelKpGet(&accelKp); AttitudeSettingsYawBiasRateGet(&yawBiasRate); init = 1; } PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); AttitudeRawData attitudeRaw; AttitudeRawGet(&attitudeRaw); updateSensors(&attitudeRaw); updateAttitude(&attitudeRaw); AttitudeRawSet(&attitudeRaw); } }
/** * Select and use transmitter control * @param [in] reset_controller True if previously another controller was used */ int32_t transmitter_control_select(bool reset_controller) { // Activate the flight mode corresponding to the switch position set_flight_mode(); ManualControlCommandGet(&cmd); FlightStatusGet(&flightStatus); // Depending on the mode update the Stabilization or Actuator objects static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; switch(flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_MANUAL: update_actuator_desired(&cmd); break; case FLIGHTSTATUS_FLIGHTMODE_ACRO: case FLIGHTSTATUS_FLIGHTMODE_LEVELING: case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR: case FLIGHTSTATUS_FLIGHTMODE_MWRATE: case FLIGHTSTATUS_FLIGHTMODE_HORIZON: case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: update_stabilization_desired(&cmd, &settings); break; case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: // Tuning takes settings directly from manualcontrolcommand. No need to // call anything else. This just avoids errors. break; case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: altitude_hold_desired(&cmd, lastFlightMode != flightStatus.FlightMode); break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: set_loiter_command(&cmd); case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: // The path planner module processes data here break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: break; default: set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_UNDEFINED); break; } lastFlightMode = flightStatus.FlightMode; return 0; }
/* library functions */ void FlightStatus_Get(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) { if (Param[0]->Val->Pointer == NULL) return; FlightStatusData data; FlightStatusGet(&data); // use the same struct like picoc. see below struct mystruct { unsigned char Armed; unsigned char FlightMode; unsigned char ControlSource; } *pdata; pdata = Param[0]->Val->Pointer; pdata->Armed = data.Armed; pdata->FlightMode = data.FlightMode; pdata->ControlSource = data.ControlSource; }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { /* create all modules thread */ MODULE_TASKCREATE_ALL; if (PIOS_heap_malloc_failed_p()) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); #if (defined(COPTERCONTROL) || defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX)) // Run this initially to make sure the configuration is checked configuration_check(); // Whenever the configuration changes, make sure it is safe to fly if (StabilizationSettingsHandle()) StabilizationSettingsConnectCallback(configurationUpdatedCb); if (SystemSettingsHandle()) SystemSettingsConnectCallback(configurationUpdatedCb); if (ManualControlSettingsHandle()) ManualControlSettingsConnectCallback(configurationUpdatedCb); if (FlightStatusHandle()) FlightStatusConnectCallback(configurationUpdatedCb); #endif #if (defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX)) if (StateEstimationHandle()) StateEstimationConnectCallback(configurationUpdatedCb); #endif // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(WDG_STATS_DIAGNOSTICS) updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF); #endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set if (indicateError()) { #if defined (PIOS_LED_ALARM) PIOS_LED_On(PIOS_LED_ALARM); #endif /* PIOS_LED_ALARM */ } else { #if defined (PIOS_LED_ALARM) PIOS_LED_Off(PIOS_LED_ALARM); #endif /* PIOS_LED_ALARM */ } FlightStatusData flightStatus; FlightStatusGet(&flightStatus); UAVObjEvent ev; int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? SYSTEM_UPDATE_PERIOD_MS / (LED_BLINK_RATE_HZ * 2) : SYSTEM_UPDATE_PERIOD_MS; if (PIOS_Queue_Receive(objectPersistenceQueue, &ev, delayTime) == true) { // If object persistence is updated call the callback objectUpdatedCb(&ev); } } }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { /* create all modules thread */ MODULE_TASKCREATE_ALL; if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(I2C_WDG_STATS_DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set #if defined (PIOS_LED_ALARM) if (AlarmsHasWarnings()) { PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); } #endif /* PIOS_LED_ALARM */ FlightStatusData flightStatus; FlightStatusGet(&flightStatus); UAVObjEvent ev; int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) : SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS; if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { // If object persistence is updated call the callback objectUpdatedCb(&ev); } } }
/** * 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); } }
/** * 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 thread, should not return. */ static void AutotuneTask(void *parameters) { //AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); enum AUTOTUNE_STATE state = AT_INIT; portTickType lastUpdateTime = xTaskGetTickCount(); while(1) { PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); // TODO: // 1. get from queue // 2. based on whether it is flightstatus or manualcontrol portTickType diffTime; const uint32_t PREPARE_TIME = 2000; const uint32_t MEAURE_TIME = 30000; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); // Only allow this module to run when autotuning if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { state = AT_INIT; vTaskDelay(50); continue; } StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); ManualControlSettingsData manualSettings; ManualControlSettingsGet(&manualSettings); ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); RelayTuningSettingsData relaySettings; RelayTuningSettingsGet(&relaySettings); bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE; if (rate) { // rate mode stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; } else { stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.Roll = manualControl.Roll * stabSettings.RollMax; stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax; } stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; stabDesired.Throttle = manualControl.Throttle; switch(state) { case AT_INIT: lastUpdateTime = xTaskGetTickCount(); // Only start when armed and flying if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) state = AT_START; break; case AT_START: diffTime = xTaskGetTickCount() - lastUpdateTime; // Spend the first block of time in normal rate mode to get airborne if (diffTime > PREPARE_TIME) { state = AT_ROLL; lastUpdateTime = xTaskGetTickCount(); } break; case AT_ROLL: diffTime = xTaskGetTickCount() - lastUpdateTime; // Run relay mode on the roll axis for the measurement time stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; if (diffTime > MEAURE_TIME) { // Move on to next state state = AT_PITCH; lastUpdateTime = xTaskGetTickCount(); } break; case AT_PITCH: diffTime = xTaskGetTickCount() - lastUpdateTime; // Run relay mode on the pitch axis for the measurement time stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE : STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE; if (diffTime > MEAURE_TIME) { // Move on to next state state = AT_FINISHED; lastUpdateTime = xTaskGetTickCount(); } break; case AT_FINISHED: // Wait until disarmed and landed before updating the settings if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) state = AT_SET; break; case AT_SET: update_stabilization_settings(); state = AT_INIT; break; default: // Set an alarm or some shit like that break; } StabilizationDesiredSet(&stabDesired); vTaskDelay(10); } }
int32_t LoggingStart(void) { DebugLogSettingsConnectCallback(SettingsUpdatedCb); DebugLogControlConnectCallback(ControlUpdatedCb); FlightStatusConnectCallback(FlightStatusUpdatedCb); SettingsUpdatedCb(DebugLogSettingsHandle()); UAVObjEvent ev = { .obj = DebugLogSettingsHandle(), .instId = 0, .event = EV_UPDATED_PERIODIC, .lowPriority = true, }; EventPeriodicCallbackCreate(&ev, StatusUpdatedCb, 1000); // invoke a periodic dispatcher callback - the event struct is a dummy, it could be filled with anything! StatusUpdatedCb(&ev); return 0; } MODULE_INITCALL(LoggingInitialize, LoggingStart); static void StatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { PIOS_DEBUGLOG_Info(&status.Flight, &status.Entry, &status.FreeSlots, &status.UsedSlots); DebugLogStatusSet(&status); } static void FlightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { FlightStatusGet(&flightstatus); if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ONLYWHENARMED) { if (flightstatus.Armed != FLIGHTSTATUS_ARMED_ARMED) { PIOS_DEBUGLOG_Printf("FlightStatus Disarmed: On board Logging disabled."); PIOS_DEBUGLOG_Enable(0); } else { PIOS_DEBUGLOG_Enable(1); PIOS_DEBUGLOG_Printf("FlightStatus Armed: On board logging enabled."); } } } static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { DebugLogSettingsGet(&settings); if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ALWAYS) { PIOS_DEBUGLOG_Enable(1); PIOS_DEBUGLOG_Printf("On board logging enabled."); } else if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_DISABLED) { PIOS_DEBUGLOG_Printf("On board logging disabled."); PIOS_DEBUGLOG_Enable(0); } else { FlightStatusUpdatedCb(NULL); } } static void ControlUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { DebugLogControlGet(&control); if (control.Operation == DEBUGLOGCONTROL_OPERATION_RETRIEVE) { memset(entry, 0, sizeof(DebugLogEntryData)); if (PIOS_DEBUGLOG_Read(entry, control.Flight, control.Entry) != 0) { // reading from log failed, mark as non existent in output entry->Flight = control.Flight; entry->Entry = control.Entry; entry->Type = DEBUGLOGENTRY_TYPE_EMPTY; } DebugLogEntrySet(entry); } else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) { uint8_t armed; FlightStatusArmedGet(&armed); if (armed == FLIGHTSTATUS_ARMED_DISARMED) { PIOS_DEBUGLOG_Format(); } } StatusUpdatedCb(ev); }
void plan_run_AutoTakeoff() { StatusVtolAutoTakeoffControlStateOptions priorState = autotakeoffState; switch (autotakeoffState) { case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST: { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (!flightStatus.Armed) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE; } } break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED: { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.Armed) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE; } } break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE: { ManualControlCommandData cmd; ManualControlCommandGet(&cmd); if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE; } } break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE: { ManualControlCommandData cmd; ManualControlCommandGet(&cmd); if (cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT; plan_setup_land(); } } break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT: { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (!flightStatus.Armed) { autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED; } } break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD: // nothing to do. land has been requested. stay here for forever until mode change. default: break; } if (autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT && autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD) { if (priorState != autotakeoffState) { PathDesiredData pathDesired; plan_setup_AutoTakeoff_helper(&pathDesired); PathDesiredSet(&pathDesired); } } }
/** * Module task */ static void manualControlTask(void *parameters) { /* Make sure disarmed on power up */ FlightStatusData flightStatus; FlightStatusGet(&flightStatus); flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; FlightStatusSet(&flightStatus); // Main task loop lastSysTime = PIOS_Thread_Systime(); // Select failsafe before run failsafe_control_select(true); while (1) { // Process periodic data for each of the controllers, including reading // all available inputs failsafe_control_update(); transmitter_control_update(); tablet_control_update(); geofence_control_update(); // Initialize to invalid value to ensure first update sets FlightStatus static FlightStatusControlSourceOptions last_control_selection = -1; enum control_events control_events = CONTROL_EVENTS_NONE; // Control logic to select the valid controller FlightStatusControlSourceOptions control_selection = control_source_select(); bool reset_controller = control_selection != last_control_selection; // This logic would be better collapsed into control_source_select but // in the case of the tablet we need to be able to abort and fall back // to failsafe for now switch(control_selection) { case FLIGHTSTATUS_CONTROLSOURCE_TRANSMITTER: transmitter_control_select(reset_controller); control_events = transmitter_control_get_events(); break; case FLIGHTSTATUS_CONTROLSOURCE_TABLET: { static bool tablet_previously_succeeded = false; if (tablet_control_select(reset_controller) == 0) { control_events = tablet_control_get_events(); tablet_previously_succeeded = true; } else { // Failure in tablet control. This would be better if done // at the selection stage before the tablet is even used. failsafe_control_select(reset_controller || tablet_previously_succeeded); control_events = failsafe_control_get_events(); tablet_previously_succeeded = false; } break; } case FLIGHTSTATUS_CONTROLSOURCE_GEOFENCE: geofence_control_select(reset_controller); control_events = geofence_control_get_events(); break; case FLIGHTSTATUS_CONTROLSOURCE_FAILSAFE: default: failsafe_control_select(reset_controller); control_events = failsafe_control_get_events(); break; } if (control_selection != last_control_selection) { FlightStatusControlSourceSet(&control_selection); last_control_selection = control_selection; } // TODO: This can evolve into a full FSM like I2C possibly switch(control_events) { case CONTROL_EVENTS_NONE: break; case CONTROL_EVENTS_ARM: control_event_arm(); break; case CONTROL_EVENTS_ARMING: control_event_arming(); break; case CONTROL_EVENTS_DISARM: control_event_disarm(); break; } // Wait until next update PIOS_Thread_Sleep_Until(&lastSysTime, UPDATE_PERIOD_MS); PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); } }
void FlightStatusControlSource(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) { FlightStatusData data; FlightStatusGet(&data); ReturnValue->Val->Integer = data.ControlSource; }
/* library functions */ void FlightStatusArmed(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) { FlightStatusData data; FlightStatusGet(&data); ReturnValue->Val->Integer = data.Armed; }
static void loggingTask(void *parameters) { UAVObjEvent ev; bool armed = false; uint32_t now = PIOS_Thread_Systime(); #if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) bool write_open = false; bool read_open = false; int32_t read_sector = 0; uint8_t read_data[LOGGINGSTATS_FILESECTOR_NUMELEM]; #endif // Get settings automatically for now on LoggingSettingsConnectCopy(&settings); LoggingStatsGet(&loggingData); loggingData.BytesLogged = 0; #if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) if (destination_spi_flash) { loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id); loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id); } #endif if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONSTART) { loggingData.Operation = LOGGINGSTATS_OPERATION_INITIALIZING; } else { loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE; } LoggingStatsSet(&loggingData); // Loop forever while (1) { LoggingStatsGet(&loggingData); // Check for change in armed state if logging on armed if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONARM) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !armed) { // Start logging because just armed loggingData.Operation = LOGGINGSTATS_OPERATION_INITIALIZING; armed = true; LoggingStatsSet(&loggingData); } else if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && armed) { loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE; armed = false; LoggingStatsSet(&loggingData); } } switch (loggingData.Operation) { case LOGGINGSTATS_OPERATION_FORMAT: // Format the file system #if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) if (destination_spi_flash){ if (read_open || write_open) { PIOS_STREAMFS_Close(streamfs_id); read_open = false; write_open = false; } PIOS_STREAMFS_Format(streamfs_id); loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id); loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id); } #endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */ loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE; LoggingStatsSet(&loggingData); break; case LOGGINGSTATS_OPERATION_INITIALIZING: // Unregister all objects UAVObjIterate(&unregister_object); // Register objects to be logged switch (settings.Profile) { case LOGGINGSETTINGS_PROFILE_DEFAULT: register_default_profile(); break; case LOGGINGSETTINGS_PROFILE_CUSTOM: UAVObjIterate(®ister_object); break; } #if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) if (destination_spi_flash){ // Close the file if it is open for reading if (read_open) { PIOS_STREAMFS_Close(streamfs_id); read_open = false; } // Open the file if it is not open for writing if (!write_open) { if (PIOS_STREAMFS_OpenWrite(streamfs_id) != 0) { loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR; continue; } else { write_open = true; } loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id); loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id); LoggingStatsSet(&loggingData); } } else { read_open = false; write_open = true; } #endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */ // Write information at start of the log file writeHeader(); // Log settings if (settings.LogSettingsOnStart == LOGGINGSETTINGS_LOGSETTINGSONSTART_TRUE){ UAVObjIterate(&logSettings); } // Empty the queue while(PIOS_Queue_Receive(logging_queue, &ev, 0)) LoggingStatsBytesLoggedSet(&written_bytes); loggingData.Operation = LOGGINGSTATS_OPERATION_LOGGING; LoggingStatsSet(&loggingData); break; case LOGGINGSTATS_OPERATION_LOGGING: { // Sleep between writing PIOS_Thread_Sleep_Until(&now, LOGGING_PERIOD_MS); // Log the objects registred to the shared queue for (int i=0; i<LOGGING_QUEUE_SIZE; i++) { if (PIOS_Queue_Receive(logging_queue, &ev, 0) == true) { UAVTalkSendObjectTimestamped(uavTalkCon, ev.obj, ev.instId, false, 0); } else { break; } } LoggingStatsBytesLoggedSet(&written_bytes); now = PIOS_Thread_Systime(); } break; case LOGGINGSTATS_OPERATION_DOWNLOAD: #if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) if (destination_spi_flash) { if (!read_open) { // Start reading if (PIOS_STREAMFS_OpenRead(streamfs_id, loggingData.FileRequest) != 0) { loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR; } else { read_open = true; read_sector = -1; } } if (read_open && read_sector == loggingData.FileSectorNum) { // Request received for same sector. Reupdate. memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM); loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE; } else if (read_open && (read_sector + 1) == loggingData.FileSectorNum) { int32_t bytes_read = PIOS_COM_ReceiveBuffer(logging_com_id, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM, 1); if (bytes_read < 0 || bytes_read > LOGGINGSTATS_FILESECTOR_NUMELEM) { // close on error loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR; PIOS_STREAMFS_Close(streamfs_id); read_open = false; } else if (bytes_read < LOGGINGSTATS_FILESECTOR_NUMELEM) { // Check it has really run out of bytes by reading again int32_t bytes_read2 = PIOS_COM_ReceiveBuffer(logging_com_id, &read_data[bytes_read], LOGGINGSTATS_FILESECTOR_NUMELEM - bytes_read, 1); memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM); if ((bytes_read + bytes_read2) < LOGGINGSTATS_FILESECTOR_NUMELEM) { // indicate end of file loggingData.Operation = LOGGINGSTATS_OPERATION_COMPLETE; PIOS_STREAMFS_Close(streamfs_id); read_open = false; } else { // Indicate sent loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE; } } else { // Indicate sent loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE; memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM); } read_sector = loggingData.FileSectorNum; } LoggingStatsSet(&loggingData); } #endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */ // fall-through to default case default: // Makes sure that we are not hogging the processor PIOS_Thread_Sleep(10); #if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) if (destination_spi_flash) { // Close the file if necessary if (write_open) { PIOS_STREAMFS_Close(streamfs_id); loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id); loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id); LoggingStatsSet(&loggingData); write_open = false; } } #endif /* defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) */ } } }
/** * 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); } }
static void loggingTask(void *parameters) { bool armed = false; bool write_open = false; bool read_open = false; int32_t read_sector = 0; uint8_t read_data[LOGGINGSTATS_FILESECTOR_NUMELEM]; //PIOS_STREAMFS_Format(streamfs_id); LoggingStatsData loggingData; LoggingStatsGet(&loggingData); loggingData.BytesLogged = 0; loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id); loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id); LoggingSettingsData settings; LoggingSettingsGet(&settings); if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONSTART) { if (PIOS_STREAMFS_OpenWrite(streamfs_id) != 0) { loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR; write_open = false; } else { loggingData.Operation = LOGGINGSTATS_OPERATION_LOGGING; write_open = true; } } else { loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE; } LoggingStatsSet(&loggingData); int i = 0; // Loop forever while (1) { // Do not update anything at more than 40 Hz PIOS_Thread_Sleep(20); LoggingStatsGet(&loggingData); // Check for change in armed state if logging on armed LoggingSettingsGet(&settings); if (settings.LogBehavior == LOGGINGSETTINGS_LOGBEHAVIOR_LOGONARM) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !armed) { // Start logging because just armed loggingData.Operation = LOGGINGSTATS_OPERATION_LOGGING; armed = true; LoggingStatsSet(&loggingData); } else if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && armed) { loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE; armed = false; LoggingStatsSet(&loggingData); } } // If currently downloading a log, close the file if (loggingData.Operation == LOGGINGSTATS_OPERATION_LOGGING && read_open) { PIOS_STREAMFS_Close(streamfs_id); read_open = false; } if (loggingData.Operation == LOGGINGSTATS_OPERATION_LOGGING && !write_open) { if (PIOS_STREAMFS_OpenWrite(streamfs_id) != 0) { loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR; } else { write_open = true; } loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id); loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id); LoggingStatsSet(&loggingData); } else if (loggingData.Operation != LOGGINGSTATS_OPERATION_LOGGING && write_open) { PIOS_STREAMFS_Close(streamfs_id); loggingData.MinFileId = PIOS_STREAMFS_MinFileId(streamfs_id); loggingData.MaxFileId = PIOS_STREAMFS_MaxFileId(streamfs_id); LoggingStatsSet(&loggingData); write_open = false; } switch (loggingData.Operation) { case LOGGINGSTATS_OPERATION_LOGGING: if (!write_open) continue; UAVTalkSendObjectTimestamped(uavTalkCon, AttitudeActualHandle(), 0, false, 0); UAVTalkSendObjectTimestamped(uavTalkCon, AccelsHandle(), 0, false, 0); UAVTalkSendObjectTimestamped(uavTalkCon, GyrosHandle(), 0, false, 0); UAVTalkSendObjectTimestamped(uavTalkCon, MagnetometerHandle(), 0, false, 0); if ((i % 10) == 0) { UAVTalkSendObjectTimestamped(uavTalkCon, BaroAltitudeHandle(), 0, false, 0); UAVTalkSendObjectTimestamped(uavTalkCon, GPSPositionHandle(), 0, false, 0); } if ((i % 50) == 1) { UAVTalkSendObjectTimestamped(uavTalkCon, GPSTimeHandle(), 0, false, 0); } LoggingStatsBytesLoggedSet(&written_bytes); break; case LOGGINGSTATS_OPERATION_DOWNLOAD: if (!read_open) { // Start reading if (PIOS_STREAMFS_OpenRead(streamfs_id, loggingData.FileRequest) != 0) { loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR; } else { read_open = true; read_sector = -1; } } if (read_open && read_sector == loggingData.FileSectorNum) { // Request received for same sector. Reupdate. memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM); loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE; } else if (read_open && (read_sector + 1) == loggingData.FileSectorNum) { int32_t bytes_read = PIOS_COM_ReceiveBuffer(logging_com_id, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM, 1); if (bytes_read < 0 || bytes_read > LOGGINGSTATS_FILESECTOR_NUMELEM) { // close on error loggingData.Operation = LOGGINGSTATS_OPERATION_ERROR; PIOS_STREAMFS_Close(streamfs_id); read_open = false; } else if (bytes_read < LOGGINGSTATS_FILESECTOR_NUMELEM) { // Check it has really run out of bytes by reading again int32_t bytes_read2 = PIOS_COM_ReceiveBuffer(logging_com_id, &read_data[bytes_read], LOGGINGSTATS_FILESECTOR_NUMELEM - bytes_read, 1); memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM); if ((bytes_read + bytes_read2) < LOGGINGSTATS_FILESECTOR_NUMELEM) { // indicate end of file loggingData.Operation = LOGGINGSTATS_OPERATION_COMPLETE; PIOS_STREAMFS_Close(streamfs_id); read_open = false; } else { // Indicate sent loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE; } } else { // Indicate sent loggingData.Operation = LOGGINGSTATS_OPERATION_IDLE; memcpy(loggingData.FileSector, read_data, LOGGINGSTATS_FILESECTOR_NUMELEM); } read_sector = loggingData.FileSectorNum; } LoggingStatsSet(&loggingData); } i++; } }
static void uavoMavlinkBridgeTask(void *parameters) { uint32_t lastSysTime; // Main task loop lastSysTime = PIOS_Thread_Systime(); FlightBatterySettingsData batSettings = {}; if (FlightBatterySettingsHandle() != NULL ) FlightBatterySettingsGet(&batSettings); SystemStatsData systemStats; while (1) { PIOS_Thread_Sleep_Until(&lastSysTime, 1000 / TASK_RATE_HZ); if (stream_trigger(MAV_DATA_STREAM_EXTENDED_STATUS)) { FlightBatteryStateData batState = {}; if (FlightBatteryStateHandle() != NULL ) FlightBatteryStateGet(&batState); SystemStatsGet(&systemStats); int8_t battery_remaining = 0; if (batSettings.Capacity != 0) { if (batState.ConsumedEnergy < batSettings.Capacity) { battery_remaining = 100 - lroundf(batState.ConsumedEnergy / batSettings.Capacity * 100); } } uint16_t voltage = 0; if (batSettings.VoltagePin != FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE) voltage = lroundf(batState.Voltage * 1000); uint16_t current = 0; if (batSettings.CurrentPin != FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE) current = lroundf(batState.Current * 100); mavlink_msg_sys_status_pack(0, 200, mav_msg, // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control 0, // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control 0, // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control 0, // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t)systemStats.CPULoad * 10, // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) voltage, // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current current, // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery battery_remaining, // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) 0, // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) 0, // errors_count1 Autopilot-specific errors 0, // errors_count2 Autopilot-specific errors 0, // errors_count3 Autopilot-specific errors 0, // errors_count4 Autopilot-specific errors 0); send_message(); } if (stream_trigger(MAV_DATA_STREAM_RC_CHANNELS)) { ManualControlCommandData manualState; FlightStatusData flightStatus; ManualControlCommandGet(&manualState); FlightStatusGet(&flightStatus); SystemStatsGet(&systemStats); //TODO connect with RSSI object and pass in last argument mavlink_msg_rc_channels_raw_pack(0, 200, mav_msg, // time_boot_ms Timestamp (milliseconds since system boot) systemStats.FlightTime, // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. 0, // chan1_raw RC channel 1 value, in microseconds manualState.Channel[0], // chan2_raw RC channel 2 value, in microseconds manualState.Channel[1], // chan3_raw RC channel 3 value, in microseconds manualState.Channel[2], // chan4_raw RC channel 4 value, in microseconds manualState.Channel[3], // chan5_raw RC channel 5 value, in microseconds manualState.Channel[4], // chan6_raw RC channel 6 value, in microseconds manualState.Channel[5], // chan7_raw RC channel 7 value, in microseconds manualState.Channel[6], // chan8_raw RC channel 8 value, in microseconds manualState.Channel[7], // rssi Receive signal strength indicator, 0: 0%, 255: 100% manualState.Rssi); send_message(); } if (stream_trigger(MAV_DATA_STREAM_POSITION)) { GPSPositionData gpsPosData = {}; HomeLocationData homeLocation = {}; SystemStatsGet(&systemStats); if (GPSPositionHandle() != NULL ) GPSPositionGet(&gpsPosData); if (HomeLocationHandle() != NULL ) HomeLocationGet(&homeLocation); SystemStatsGet(&systemStats); uint8_t gps_fix_type; switch (gpsPosData.Status) { case GPSPOSITION_STATUS_NOGPS: gps_fix_type = 0; break; case GPSPOSITION_STATUS_NOFIX: gps_fix_type = 1; break; case GPSPOSITION_STATUS_FIX2D: gps_fix_type = 2; break; case GPSPOSITION_STATUS_FIX3D: case GPSPOSITION_STATUS_DIFF3D: gps_fix_type = 3; break; default: gps_fix_type = 0; break; } mavlink_msg_gps_raw_int_pack(0, 200, mav_msg, // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)systemStats.FlightTime * 1000, // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. gps_fix_type, // lat Latitude in 1E7 degrees gpsPosData.Latitude, // lon Longitude in 1E7 degrees gpsPosData.Longitude, // alt Altitude in 1E3 meters (millimeters) above MSL gpsPosData.Altitude * 1000, // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 gpsPosData.HDOP * 100, // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 gpsPosData.VDOP * 100, // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 gpsPosData.Groundspeed * 100, // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 gpsPosData.Heading * 100, // satellites_visible Number of satellites visible. If unknown, set to 255 gpsPosData.Satellites); send_message(); mavlink_msg_gps_global_origin_pack(0, 200, mav_msg, // latitude Latitude (WGS84), expressed as * 1E7 homeLocation.Latitude, // longitude Longitude (WGS84), expressed as * 1E7 homeLocation.Longitude, // altitude Altitude(WGS84), expressed as * 1000 homeLocation.Altitude * 1000); send_message(); //TODO add waypoint nav stuff //wp_target_bearing //wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg); //alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg); //aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg); //xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg); //mavlink_msg_nav_controller_output_pack //wp_number //mavlink_msg_mission_current_pack } if (stream_trigger(MAV_DATA_STREAM_EXTRA1)) { AttitudeActualData attActual; SystemStatsData systemStats; AttitudeActualGet(&attActual); SystemStatsGet(&systemStats); mavlink_msg_attitude_pack(0, 200, mav_msg, // time_boot_ms Timestamp (milliseconds since system boot) systemStats.FlightTime, // roll Roll angle (rad) attActual.Roll * DEG2RAD, // pitch Pitch angle (rad) attActual.Pitch * DEG2RAD, // yaw Yaw angle (rad) attActual.Yaw * DEG2RAD, // rollspeed Roll angular speed (rad/s) 0, // pitchspeed Pitch angular speed (rad/s) 0, // yawspeed Yaw angular speed (rad/s) 0); send_message(); } if (stream_trigger(MAV_DATA_STREAM_EXTRA2)) { ActuatorDesiredData actDesired; AttitudeActualData attActual; AirspeedActualData airspeedActual = {}; GPSPositionData gpsPosData = {}; BaroAltitudeData baroAltitude = {}; FlightStatusData flightStatus; if (AirspeedActualHandle() != NULL ) AirspeedActualGet(&airspeedActual); if (GPSPositionHandle() != NULL ) GPSPositionGet(&gpsPosData); if (BaroAltitudeHandle() != NULL ) BaroAltitudeGet(&baroAltitude); ActuatorDesiredGet(&actDesired); AttitudeActualGet(&attActual); FlightStatusGet(&flightStatus); float altitude = 0; if (BaroAltitudeHandle() != NULL) altitude = baroAltitude.Altitude; else if (GPSPositionHandle() != NULL) altitude = gpsPosData.Altitude; // round attActual.Yaw to nearest int and transfer from (-180 ... 180) to (0 ... 360) int16_t heading = lroundf(attActual.Yaw); if (heading < 0) heading += 360; mavlink_msg_vfr_hud_pack(0, 200, mav_msg, // airspeed Current airspeed in m/s airspeedActual.TrueAirspeed, // groundspeed Current ground speed in m/s gpsPosData.Groundspeed, // heading Current heading in degrees, in compass units (0..360, 0=north) heading, // throttle Current throttle setting in integer percent, 0 to 100 actDesired.Throttle * 100, // alt Current altitude (MSL), in meters altitude, // climb Current climb rate in meters/second 0); send_message(); uint8_t armed_mode = 0; if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) armed_mode |= MAV_MODE_FLAG_SAFETY_ARMED; uint8_t custom_mode = CUSTOM_MODE_STAB; switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_MANUAL: case FLIGHTSTATUS_FLIGHTMODE_MWRATE: case FLIGHTSTATUS_FLIGHTMODE_VIRTUALBAR: case FLIGHTSTATUS_FLIGHTMODE_HORIZON: /* Kinda a catch all */ custom_mode = CUSTOM_MODE_SPORT; break; case FLIGHTSTATUS_FLIGHTMODE_ACRO: case FLIGHTSTATUS_FLIGHTMODE_AXISLOCK: custom_mode = CUSTOM_MODE_ACRO; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: /* May want these three to try and * infer based on roll axis */ case FLIGHTSTATUS_FLIGHTMODE_LEVELING: custom_mode = CUSTOM_MODE_STAB; break; case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: custom_mode = CUSTOM_MODE_DRIFT; break; case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: custom_mode = CUSTOM_MODE_ALTH; break; case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: custom_mode = CUSTOM_MODE_RTL; break; case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL: case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: custom_mode = CUSTOM_MODE_POSH; break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: custom_mode = CUSTOM_MODE_AUTO; break; } mavlink_msg_heartbeat_pack(0, 200, mav_msg, // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) MAV_TYPE_GENERIC, // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM MAV_AUTOPILOT_GENERIC, // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h armed_mode, // custom_mode A bitfield for use for autopilot-specific flags. custom_mode, // system_status System status flag, see MAV_STATE ENUM 0); send_message(); } } }
/** * @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization" * @input: ManualControlCommand * @output: StabilizationDesired */ void stabilizedHandler(bool newinit) { if (newinit) { StabilizationDesiredInitialize(); StabilizationBankInitialize(); } ManualControlCommandData cmd; ManualControlCommandGet(&cmd); FlightModeSettingsData settings; FlightModeSettingsGet(&settings); StabilizationDesiredData stabilization; StabilizationDesiredGet(&stabilization); StabilizationBankData stabSettings; StabilizationBankGet(&stabSettings); cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll); cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch); cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw); uint8_t *stab_settings; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: stab_settings = FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: stab_settings = FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: stab_settings = FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: stab_settings = FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: stab_settings = FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings); 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); stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings); return; } stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax : 0; // this is an invalid mode stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : 0; // this is an invalid mode // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order stabilization.StabilizationMode.Roll = stab_settings[0]; stabilization.StabilizationMode.Pitch = stab_settings[1]; // Other axes (yaw) cannot be Rattitude, so use Rate // Should really do this for Attitude mode as well? if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw; } else { stabilization.StabilizationMode.Yaw = stab_settings[2]; stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax : 0; // this is an invalid mode } stabilization.StabilizationMode.Thrust = stab_settings[3]; stabilization.Thrust = cmd.Thrust; StabilizationDesiredSet(&stabilization); }
/** * 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; } } }
/** * Module thread, should not return. */ static void AutotuneTask(void *parameters) { enum AUTOTUNE_STATE state = AT_INIT; uint32_t last_update_time = PIOS_Thread_Systime(); float X[AF_NUMX] = {0}; float P[AF_NUMP] = {0}; float noise[3] = {0}; af_init(X,P); uint32_t last_time = 0.0f; const uint32_t YIELD_MS = 2; GyrosConnectCallback(at_new_gyro_data); while(1) { PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE); uint32_t diff_time; const uint32_t PREPARE_TIME = 2000; const uint32_t MEASURE_TIME = 60000; static uint32_t update_counter = 0; bool doing_ident = false; bool can_sleep = true; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); // Only allow this module to run when autotuning if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { state = AT_INIT; PIOS_Thread_Sleep(50); continue; } switch(state) { case AT_INIT: last_update_time = PIOS_Thread_Systime(); // Only start when armed and flying if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { af_init(X,P); UpdateSystemIdent(X, NULL, 0.0f, 0, 0); state = AT_START; } break; case AT_START: diff_time = PIOS_Thread_Systime() - last_update_time; // Spend the first block of time in normal rate mode to get airborne if (diff_time > PREPARE_TIME) { last_time = PIOS_DELAY_GetRaw(); /* Drain the queue of all current data */ while (circ_queue_read_pos(at_queue)) { circ_queue_read_completed(at_queue); } /* And reset the point spill counter */ update_counter = 0; at_points_spilled = 0; state = AT_RUN; last_update_time = PIOS_Thread_Systime(); } break; case AT_RUN: diff_time = PIOS_Thread_Systime() - last_update_time; doing_ident = true; can_sleep = false; for (int i=0; i<MAX_PTS_PER_CYCLE; i++) { struct at_queued_data *pt; /* Grab an autotune point */ pt = circ_queue_read_pos(at_queue); if (!pt) { /* We've drained the buffer * fully. Yay! */ can_sleep = true; break; } /* calculate time between successive * points */ float dT_s = PIOS_DELAY_DiffuS2(last_time, pt->raw_time) * 1.0e-6f; /* This is for the first point, but * also if we have extended drops */ if (dT_s > 0.010f) { dT_s = 0.010f; } last_time = pt->raw_time; af_predict(X, P, pt->u, pt->y, dT_s); // Update uavo every 256 cycles to avoid // telemetry spam if (!((update_counter++) & 0xff)) { UpdateSystemIdent(X, noise, dT_s, update_counter, at_points_spilled); } for (uint32_t i = 0; i < 3; i++) { const float NOISE_ALPHA = 0.9997f; // 10 second time constant at 300 Hz noise[i] = NOISE_ALPHA * noise[i] + (1-NOISE_ALPHA) * (pt->y[i] - X[i]) * (pt->y[i] - X[i]); } /* Free the buffer containing an AT point */ circ_queue_read_completed(at_queue); } if (diff_time > MEASURE_TIME) { // Move on to next state state = AT_FINISHED; last_update_time = PIOS_Thread_Systime(); } break; case AT_FINISHED: // Wait until disarmed and landed before saving the settings UpdateSystemIdent(X, noise, 0, update_counter, at_points_spilled); state = AT_WAITING; // Fall through case AT_WAITING: // TODO do this unconditionally on disarm, // no matter what mode we're in. if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { // Save the settings locally. UAVObjSave(SystemIdentHandle(), 0); state = AT_INIT; } break; default: // Set an alarm or some shit like that break; } // Update based on manual controls UpdateStabilizationDesired(doing_ident); if (can_sleep) { PIOS_Thread_Sleep(YIELD_MS); } } }
static void FirmwareIAPCallback(UAVObjEvent* ev) { const struct pios_board_info * bdinfo = &pios_board_info_blob; static uint32_t last_time = 0; uint32_t this_time; uint32_t delta; if(iap_state == IAP_STATE_RESETTING) return; if ( ev->obj == FirmwareIAPObjHandle() ) { // Get the input object data FirmwareIAPObjGet(&data); this_time = get_time(); delta = this_time - last_time; last_time = this_time; if((data.BoardType==bdinfo->board_type)&&(data.crc != PIOS_BL_HELPER_CRC_Memory_Calc())) { PIOS_BL_HELPER_FLASH_Read_Description(data.Description,FIRMWAREIAPOBJ_DESCRIPTION_NUMELEM); PIOS_SYS_SerialNumberGetBinary(data.CPUSerial); data.BoardRevision=bdinfo->board_rev; data.crc = PIOS_BL_HELPER_CRC_Memory_Calc(); FirmwareIAPObjSet( &data ); } if((data.ArmReset==1)&&(iap_state!=IAP_STATE_RESETTING)) { data.ArmReset=0; FirmwareIAPObjSet( &data ); } switch(iap_state) { case IAP_STATE_READY: if( data.Command == IAP_CMD_STEP_1 ) { iap_state = IAP_STATE_STEP_1; } break; case IAP_STATE_STEP_1: if( data.Command == IAP_CMD_STEP_2 ) { if( delta > iap_time_2_low_end && delta < iap_time_2_high_end ) { iap_state = IAP_STATE_STEP_2; } else { iap_state = IAP_STATE_READY; } } else { iap_state = IAP_STATE_READY; } break; case IAP_STATE_STEP_2: if( data.Command == IAP_CMD_STEP_3 ) { if( delta > iap_time_3_low_end && delta < iap_time_3_high_end ) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if(flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) { // Abort any attempts if not disarmed iap_state = IAP_STATE_READY; break; } // we've met the three sequence of command numbers // we've met the time requirements. PIOS_IAP_SetRequest1(); PIOS_IAP_SetRequest2(); /* Note: Cant just wait timeout value, because first time is randomized */ reset_count = 0; lastResetSysTime = xTaskGetTickCount(); UAVObjEvent * ev = pvPortMalloc(sizeof(UAVObjEvent)); memset(ev,0,sizeof(UAVObjEvent)); EventPeriodicCallbackCreate(ev, resetTask, 100); iap_state = IAP_STATE_RESETTING; } else { iap_state = IAP_STATE_READY; } } else { iap_state = IAP_STATE_READY; } break; case IAP_STATE_RESETTING: // stay here permanentally, should reboot break; default: iap_state = IAP_STATE_READY; last_time = 0; // Reset the time counter, as we are not doing a IAP reset break; } } }
/** * Module thread, should not return. */ static void guidanceTask(void *parameters) { SystemSettingsData systemSettings; GuidanceSettingsData guidanceSettings; FlightStatusData flightStatus; 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]; accelData.East = accel_ned[1]; accelData.Down = accel_ned[2]; NedAccelSet(&accelData); FlightStatusGet(&flightStatus); SystemSettingsGet(&systemSettings); GuidanceSettingsGet(&guidanceSettings); if ((PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_GUIDANCE) && ((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( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ) updateVtolDesiredVelocity(); else manualSetDesiredVelocity(); updateVtolDesiredAttitude(); } else { // Be cleaner and get rid of global variables northVelIntegral = 0; eastVelIntegral = 0; downVelIntegral = 0; northPosIntegral = 0; eastPosIntegral = 0; downPosIntegral = 0; positionHoldLast = 0; } accel[0] = accel[1] = accel[2] = 0; accel_accum = 0; } }