static void uavoRelayTask(void *parameters) { UAVObjEvent ev; // Loop forever while (1) { PIOS_Thread_Sleep(50); // Wait for queue message if (PIOS_Queue_Receive(queue, &ev, 2) == true) { // Process event. This calls transmitData UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0); } // Process incoming data in sufficient chunks that we keep up uint8_t serial_data[8]; uint16_t bytes_to_process; bytes_to_process = PIOS_COM_ReceiveBuffer(uavorelay_com_id, serial_data, sizeof(serial_data), 0); do { bytes_to_process = PIOS_COM_ReceiveBuffer(uavorelay_com_id, serial_data, sizeof(serial_data), 0); for (uint8_t i = 0; i < bytes_to_process; i++) UAVTalkProcessInputStream(uavTalkCon,serial_data[i]); } while (bytes_to_process > 0); } }
/** * @brief Gimbal output control task */ static void brushlessGimbalTask(void* parameters) { UAVObjEvent ev; TIM2->CNT = 0; TIM3->CNT = 0; TIM15->CNT = 0; TIM17->CNT = 0; bool armed = false; bool previous_armed = false; while (1) { PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR); // Wait until the ActuatorDesired object is updated PIOS_Queue_Receive(queue, &ev, 1); previous_armed = armed; armed |= PIOS_Thread_Systime() > 10000; if (armed && !previous_armed) { PIOS_Brushless_SetUpdateRate(60000); } if (!armed) continue; ActuatorDesiredData actuatorDesired; ActuatorDesiredGet(&actuatorDesired); // Set the rotation in electrical degrees per second. Note these // will be divided by the number of physical poles to get real // mechanical degrees per second BrushlessGimbalSettingsData settings; BrushlessGimbalSettingsGet(&settings); PIOS_Brushless_SetScale(settings.PowerScale[0], settings.PowerScale[1], settings.PowerScale[2]); PIOS_Brushless_SetMaxAcceleration(settings.SlewLimit[0], settings.SlewLimit[1], settings.SlewLimit[2]); PIOS_Brushless_SetSpeed(0, actuatorDesired.Roll * settings.MaxDPS[BRUSHLESSGIMBALSETTINGS_MAXDPS_ROLL], 0.001f); PIOS_Brushless_SetSpeed(1, actuatorDesired.Pitch * settings.MaxDPS[BRUSHLESSGIMBALSETTINGS_MAXDPS_PITCH], 0.001f); // Use the gyros to set a damping term. This creates a phase offset of the integrated // driving position to make the control pull against any momentum. Essentially the main // output to the driver (above) is a velocity signal which the driver takes care of // integrating to create a position. The current rate of roll creates a shift in that // position (without changing the integrated position). // This idea was taken from https://code.google.com/p/brushless-gimbal/ GyrosData gyros; GyrosGet(&gyros); PIOS_Brushless_SetPhaseLag(0, -gyros.x * settings.Damping[BRUSHLESSGIMBALSETTINGS_DAMPING_ROLL]); PIOS_Brushless_SetPhaseLag(1, -gyros.y * settings.Damping[BRUSHLESSGIMBALSETTINGS_DAMPING_PITCH]); } }
/** * Telemetry transmit task, regular priority * * Logic: We need to double buffer the DMA transfers. Pack the buffer until either * 1) it is full (and then we should record the number of missed events then) * 2) the current transaction is done (we should immediately schedule since we are slave) * when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer * and then take the semaphrore */ static void overoSyncTask(void *parameters) { UAVObjEvent ev; // Kick off SPI transfers (once one is completed another will automatically transmit) overosync->transaction_done = true; overosync->sent_objects = 0; overosync->failed_objects = 0; overosync->received_objects = 0; uint32_t lastUpdateTime = PIOS_Thread_Systime(); uint32_t updateTime; fid = fopen("sim_log.opl", "w"); // Loop forever while (1) { // Wait for queue message if (PIOS_Queue_Receive(queue, &ev, PIOS_QUEUE_TIMEOUT_MAX) == true) { // Check it will fit before packetizing if ((overosync->write_pointer + UAVObjGetNumBytes(ev.obj) + 12) >= sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) { overosync->failed_objects ++; } else { // Process event. This calls transmitData UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0); } updateTime = PIOS_Thread_Systime(); if(((uint32_t) (updateTime - lastUpdateTime)) > 1000) { // Update stats. This will trigger a local send event too OveroSyncStatsData syncStats; syncStats.Send = overosync->sent_bytes; syncStats.Received = 0; syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE; syncStats.DroppedUpdates = overosync->failed_objects; OveroSyncStatsSet(&syncStats); overosync->failed_objects = 0; overosync->sent_bytes = 0; lastUpdateTime = updateTime; } } } }
static void gimbalControlTask(void *parameters) { // Loop forever while (1) { struct pios_can_gimbal_message bgc_message; // Wait for queue message if (PIOS_Queue_Receive(queue, &bgc_message, 5) == true) { CameraDesiredData cameraDesired; CameraDesiredGet(&cameraDesired); cameraDesired.Declination = bgc_message.setpoint_pitch; cameraDesired.Bearing = bgc_message.setpoint_yaw; cameraDesired.Roll = bgc_message.fc_roll; cameraDesired.Pitch = bgc_message.fc_pitch; cameraDesired.Yaw = bgc_message.fc_yaw; CameraDesiredSet(&cameraDesired); } } }
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) */ } } }
/** * 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); } } }
/** * 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 stabilizationTask(void* parameters) { UAVObjEvent ev; uint32_t timeval = PIOS_DELAY_GetRaw(); ActuatorDesiredData actuatorDesired; StabilizationDesiredData stabDesired; RateDesiredData rateDesired; AttitudeActualData attitudeActual; GyrosData gyrosData; FlightStatusData flightStatus; float *stabDesiredAxis = &stabDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; float horizonRateFraction = 0.0f; // Force refresh of all settings immediately before entering main task loop SettingsUpdatedCb((UAVObjEvent *) NULL); // Settings for system identification uint32_t iteration = 0; const uint32_t SYSTEM_IDENT_PERIOD = 75; uint32_t system_ident_timeval = PIOS_DELAY_GetRaw(); float dT_filtered = 0; // Main task loop zero_pids(); while(1) { iteration++; PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if (PIOS_Queue_Receive(queue, &ev, FAILSAFE_TIMEOUT_MS) != true) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); continue; } calculate_pids(); float dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; timeval = PIOS_DELAY_GetRaw(); // exponential moving averaging (EMA) of dT to reduce jitter; ~200points // to have more or less equivalent noise reduction to a normal N point moving averaging: alpha = 2 / (N + 1) // run it only at the beginning for the first samples, to reduce CPU load, and the value should converge to a constant value if (iteration < 100) { dT_filtered = dT; } else if (iteration < 2000) { dT_filtered = 0.01f * dT + (1.0f - 0.01f) * dT_filtered; } else if (iteration == 2000) { gyro_filter_updated = true; } if (gyro_filter_updated) { if (settings.GyroCutoff < 1.0f) { gyro_alpha = 0; } else { gyro_alpha = expf(-2.0f * (float)(M_PI) * settings.GyroCutoff * dT_filtered); } // Compute time constant for vbar decay term if (settings.VbarTau < 0.001f) { vbar_decay = 0; } else { vbar_decay = expf(-dT_filtered / settings.VbarTau); } gyro_filter_updated = false; } FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); GyrosGet(&gyrosData); ActuatorDesiredGet(&actuatorDesired); #if defined(RATEDESIRED_DIAGNOSTICS) RateDesiredGet(&rateDesired); #endif struct TrimmedAttitudeSetpoint { float Roll; float Pitch; float Yaw; } trimmedAttitudeSetpoint; // Mux in level trim values, and saturate the trimmed attitude setpoint. trimmedAttitudeSetpoint.Roll = bound_min_max( stabDesired.Roll + trimAngles.Roll, -settings.RollMax + trimAngles.Roll, settings.RollMax + trimAngles.Roll); trimmedAttitudeSetpoint.Pitch = bound_min_max( stabDesired.Pitch + trimAngles.Pitch, -settings.PitchMax + trimAngles.Pitch, settings.PitchMax + trimAngles.Pitch); trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw; // For horizon mode we need to compute the desire attitude from an unscaled value and apply the // trim offset. Also track the stick with the most deflection to choose rate blending. horizonRateFraction = 0.0f; if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) { trimmedAttitudeSetpoint.Roll = bound_min_max( stabDesired.Roll * settings.RollMax + trimAngles.Roll, -settings.RollMax + trimAngles.Roll, settings.RollMax + trimAngles.Roll); horizonRateFraction = fabsf(stabDesired.Roll); } if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) { trimmedAttitudeSetpoint.Pitch = bound_min_max( stabDesired.Pitch * settings.PitchMax + trimAngles.Pitch, -settings.PitchMax + trimAngles.Pitch, settings.PitchMax + trimAngles.Pitch); horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Pitch)); } if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) { trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw * settings.YawMax; horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Yaw)); } // For weak leveling mode the attitude setpoint is the trim value (drifts back towards "0") if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) { trimmedAttitudeSetpoint.Roll = trimAngles.Roll; } if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) { trimmedAttitudeSetpoint.Pitch = trimAngles.Pitch; } if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) { trimmedAttitudeSetpoint.Yaw = 0; } // Note we divide by the maximum limit here so the fraction ranges from 0 to 1 depending on // how much is requested. horizonRateFraction = bound_sym(horizonRateFraction, HORIZON_MODE_MAX_BLEND) / HORIZON_MODE_MAX_BLEND; // Calculate the errors in each axis. The local error is used in the following modes: // ATTITUDE, HORIZON, WEAKLEVELING float local_attitude_error[3]; local_attitude_error[0] = trimmedAttitudeSetpoint.Roll - attitudeActual.Roll; local_attitude_error[1] = trimmedAttitudeSetpoint.Pitch - attitudeActual.Pitch; local_attitude_error[2] = trimmedAttitudeSetpoint.Yaw - attitudeActual.Yaw; // Wrap yaw error to [-180,180] local_attitude_error[2] = circular_modulus_deg(local_attitude_error[2]); static float gyro_filtered[3]; gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha); gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha); gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha); // A flag to track which stabilization mode each axis is in static uint8_t previous_mode[MAX_AXES] = {255,255,255}; bool error = false; //Run the selected stabilization algorithm on each axis: for(uint8_t i=0; i< MAX_AXES; i++) { // Check whether this axis mode needs to be reinitialized bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]); // The unscaled input (-1,1) float *raw_input = &stabDesired.Roll; previous_mode[i] = stabDesired.StabilizationMode[i]; // Apply the selected control law switch(stabDesired.StabilizationMode[i]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: if(reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; // Store to rate desired variable for storing to UAVO rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_ACROPLUS: // this implementation is based on the Openpilot/Librepilot Acro+ flightmode // and our existing rate & MWRate flightmodes if(reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; // The factor for gyro suppression / mixing raw stick input into the output; scaled by raw stick input float factor = fabsf(raw_input[i]) * settings.AcroInsanityFactor / 100; // Store to rate desired variable for storing to UAVO rateDesiredAxis[i] = bound_sym(raw_input[i] * settings.ManualRate[i], settings.ManualRate[i]); // Zero integral for aggressive maneuvers, like it is done for MWRate if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) || (i == 0 && fabsf(raw_input[i]) > 0.2f)) { pids[PID_GROUP_RATE + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].i = 0; } // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = factor * raw_input[i] + (1.0f - factor) * actuatorDesiredAxis[i]; actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: if(reinit) { pids[PID_GROUP_ATT + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].iAccumulator = 0; } // Compute the outer loop rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: // Store for debugging output rateDesiredAxis[i] = stabDesiredAxis[i]; // Run a virtual flybar stabilization algorithm on this axis stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &pids[PID_GROUP_VBAR + i], &settings); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { if (reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; float weak_leveling = local_attitude_error[i] * weak_leveling_kp; weak_leveling = bound_sym(weak_leveling, weak_leveling_max); // Compute desired rate as input biased towards leveling rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling; actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; } case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: if (reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) { // While getting strong commands act like rate mode rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]); // Reset accumulator axis_lock_accum[i] = 0; } else { // For weaker commands or no command simply lock (almost) on no gyro change axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT; axis_lock_accum[i] = bound_sym(axis_lock_accum[i], max_axis_lock); // Compute the inner loop float tmpRateDesired = pid_apply(&pids[PID_GROUP_ATT + i], axis_lock_accum[i], dT); rateDesiredAxis[i] = bound_sym(tmpRateDesired, settings.MaximumRate[i]); } actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON: if(reinit) { pids[PID_GROUP_RATE + i].iAccumulator = 0; } // Do not allow outer loop integral to wind up in this mode since the controller // is often disengaged. pids[PID_GROUP_ATT + i].iAccumulator = 0; // Compute the outer loop for the attitude control float rateDesiredAttitude = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT); // Compute the desire rate for a rate control float rateDesiredRate = raw_input[i] * settings.ManualRate[i]; // Blend from one rate to another. The maximum of all stick positions is used for the // amount so that when one axis goes completely to rate the other one does too. This // prevents doing flips while one axis tries to stay in attitude mode. rateDesiredAxis[i] = rateDesiredAttitude * (1.0f-horizonRateFraction) + rateDesiredRate * horizonRateFraction; rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_MWRATE: { if(reinit) { pids[PID_GROUP_MWR + i].iAccumulator = 0; } /* Conversion from MultiWii PID settings to our units. Kp = Kp_mw * 4 / 80 / 500 Kd = Kd_mw * looptime * 1e-6 * 4 * 3 / 32 / 500 Ki = Ki_mw * 4 / 125 / 64 / (looptime * 1e-6) / 500 These values will just be approximate and should help you get started. */ // The unscaled input (-1,1) - note in MW this is from (-500,500) float *raw_input = &stabDesired.Roll; // dynamic PIDs are scaled both by throttle and stick position float scale = (i == 0 || i == 1) ? mwrate_settings.RollPitchRate : mwrate_settings.YawRate; float pid_scale = (100.0f - scale * fabsf(raw_input[i])) / 100.0f; float dynP8 = pids[PID_GROUP_MWR + i].p * pid_scale; float dynD8 = pids[PID_GROUP_MWR + i].d * pid_scale; // these terms are used by the integral loop this proportional term is scaled by throttle (this is different than MW // that does not apply scale float cfgP8 = pids[PID_GROUP_MWR + i].p; float cfgI8 = pids[PID_GROUP_MWR + i].i; // Dynamically adjust PID settings struct pid mw_pid; mw_pid.p = 0; // use zero Kp here because of strange setpoint. applied later. mw_pid.d = dynD8; mw_pid.i = cfgI8; mw_pid.iLim = pids[PID_GROUP_MWR + i].iLim; mw_pid.iAccumulator = pids[PID_GROUP_MWR + i].iAccumulator; mw_pid.lastErr = pids[PID_GROUP_MWR + i].lastErr; mw_pid.lastDer = pids[PID_GROUP_MWR + i].lastDer; // Zero integral for aggressive maneuvers if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) || (i == 0 && fabsf(raw_input[i]) > 0.2f)) { mw_pid.iAccumulator = 0; mw_pid.i = 0; } // Apply controller as if we want zero change, then add stick input afterwards actuatorDesiredAxis[i] = pid_apply_setpoint(&mw_pid, raw_input[i] / cfgP8, gyro_filtered[i], dT); actuatorDesiredAxis[i] += raw_input[i]; // apply input actuatorDesiredAxis[i] -= dynP8 * gyro_filtered[i]; // apply Kp term actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); // Store PID accumulators for next cycle pids[PID_GROUP_MWR + i].iAccumulator = mw_pid.iAccumulator; pids[PID_GROUP_MWR + i].lastErr = mw_pid.lastErr; pids[PID_GROUP_MWR + i].lastDer = mw_pid.lastDer; } break; case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT: if(reinit) { pids[PID_GROUP_ATT + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].iAccumulator = 0; } static uint32_t ident_iteration = 0; static float ident_offsets[3] = {0}; if (PIOS_DELAY_DiffuS(system_ident_timeval) / 1000.0f > SYSTEM_IDENT_PERIOD && SystemIdentHandle()) { ident_iteration++; system_ident_timeval = PIOS_DELAY_GetRaw(); SystemIdentData systemIdent; SystemIdentGet(&systemIdent); const float SCALE_BIAS = 7.1f; float roll_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_ROLL]); float pitch_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_PITCH]); float yaw_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_YAW]); if (roll_scale > 0.25f) roll_scale = 0.25f; if (pitch_scale > 0.25f) pitch_scale = 0.25f; if (yaw_scale > 0.25f) yaw_scale = 0.2f; switch(ident_iteration & 0x07) { case 0: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = yaw_scale; break; case 1: ident_offsets[0] = roll_scale; ident_offsets[1] = 0; ident_offsets[2] = 0; break; case 2: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = -yaw_scale; break; case 3: ident_offsets[0] = -roll_scale; ident_offsets[1] = 0; ident_offsets[2] = 0; break; case 4: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = yaw_scale; break; case 5: ident_offsets[0] = 0; ident_offsets[1] = pitch_scale; ident_offsets[2] = 0; break; case 6: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = -yaw_scale; break; case 7: ident_offsets[0] = 0; ident_offsets[1] = -pitch_scale; ident_offsets[2] = 0; break; } } if (i == ROLL || i == PITCH) { // Compute the outer loop rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] += ident_offsets[i]; actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); } else { // Get the desired rate. yaw is always in rate mode in system ident. rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop only for yaw actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] += ident_offsets[i]; actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); } break; case STABILIZATIONDESIRED_STABILIZATIONMODE_COORDINATEDFLIGHT: switch (i) { case YAW: if (reinit) { pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; pids[PID_RATE_YAW].iAccumulator = 0; axis_lock_accum[YAW] = 0; } //If we are not in roll attitude mode, trigger an error if (stabDesired.StabilizationMode[ROLL] != STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { error = true; break ; } if (fabsf(stabDesired.Yaw) < COORDINATED_FLIGHT_MAX_YAW_THRESHOLD) { //If yaw is within the deadband... if (fabsf(stabDesired.Roll) > COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting more roll than the threshold float accelsDataY; AccelsyGet(&accelsDataY); //Reset integral if we have changed roll to opposite direction from rudder. This implies that we have changed desired turning direction. if ((stabDesired.Roll > 0 && actuatorDesiredAxis[YAW] < 0) || (stabDesired.Roll < 0 && actuatorDesiredAxis[YAW] > 0)){ pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; } // Coordinate flight can simply be seen as ensuring that there is no lateral acceleration in the // body frame. As such, we use the (noisy) accelerometer data as our measurement. Ideally, at // some point in the future we will estimate acceleration and then we can use the estimated value // instead of the measured value. float errorSlip = -accelsDataY; float command = pid_apply(&pids[PID_COORDINATED_FLIGHT_YAW], errorSlip, dT); actuatorDesiredAxis[YAW] = bound_sym(command ,1.0); // Reset axis-lock integrals pids[PID_RATE_YAW].iAccumulator = 0; axis_lock_accum[YAW] = 0; } else if (fabsf(stabDesired.Roll) <= COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting less roll than the threshold // Axis lock on no gyro change axis_lock_accum[YAW] += (0 - gyro_filtered[YAW]) * dT; rateDesiredAxis[YAW] = pid_apply(&pids[PID_ATT_YAW], axis_lock_accum[YAW], dT); rateDesiredAxis[YAW] = bound_sym(rateDesiredAxis[YAW], settings.MaximumRate[YAW]); actuatorDesiredAxis[YAW] = pid_apply_setpoint(&pids[PID_RATE_YAW], rateDesiredAxis[YAW], gyro_filtered[YAW], dT); actuatorDesiredAxis[YAW] = bound_sym(actuatorDesiredAxis[YAW],1.0f); // Reset coordinated-flight integral pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; } } else { //... yaw is outside the deadband. Pass the manual input directly to the actuator. actuatorDesiredAxis[YAW] = bound_sym(stabDesiredAxis[YAW], 1.0); // Reset all integrals pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; pids[PID_RATE_YAW].iAccumulator = 0; axis_lock_accum[YAW] = 0; } break; case ROLL: case PITCH: default: //Coordinated Flight has no effect in these modes. Trigger a configuration error. error = true; break; } break; case STABILIZATIONDESIRED_STABILIZATIONMODE_POI: // The sanity check enforces this is only selectable for Yaw // for a gimbal you can select pitch too. if(reinit) { pids[PID_GROUP_ATT + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].iAccumulator = 0; } float error; float angle; if (CameraDesiredHandle()) { switch(i) { case PITCH: CameraDesiredDeclinationGet(&angle); error = circular_modulus_deg(angle - attitudeActual.Pitch); break; case ROLL: { uint8_t roll_fraction = 0; #ifdef GIMBAL if (BrushlessGimbalSettingsHandle()) { BrushlessGimbalSettingsRollFractionGet(&roll_fraction); } #endif /* GIMBAL */ // For ROLL POI mode we track the FC roll angle (scaled) to // allow keeping some motion CameraDesiredRollGet(&angle); angle *= roll_fraction / 100.0f; error = circular_modulus_deg(angle - attitudeActual.Roll); } break; case YAW: CameraDesiredBearingGet(&angle); error = circular_modulus_deg(angle - attitudeActual.Yaw); break; default: error = true; } } else error = true; // Compute the outer loop rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], error, dT); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.PoiMaximumRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: actuatorDesiredAxis[i] = bound_sym(stabDesiredAxis[i],1.0f); break; default: error = true; break; } } if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT); #if defined(RATEDESIRED_DIAGNOSTICS) RateDesiredSet(&rateDesired); #endif // Save dT actuatorDesired.UpdateTime = dT * 1000; actuatorDesired.Throttle = stabDesired.Throttle; if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_MANUAL) { ActuatorDesiredSet(&actuatorDesired); } else { // Force all axes to reinitialize when engaged for(uint8_t i=0; i< MAX_AXES; i++) previous_mode[i] = 255; } if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || (lowThrottleZeroIntegral && stabDesired.Throttle < 0)) { // Force all axes to reinitialize when engaged for(uint8_t i=0; i< MAX_AXES; i++) previous_mode[i] = 255; } // Clear or set alarms. Done like this to prevent toggling each cycle // and hammering system alarms if (error) AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR); else AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } }
/** * Telemetry transmit task, regular priority * * Logic: We need to double buffer the DMA transfers. Pack the buffer until either * 1) it is full (and then we should record the number of missed events then) * 2) the current transaction is done (we should immediately schedule since we are slave) * when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer * and then take the semaphrore */ static void overoSyncTask(void *parameters) { UAVObjEvent ev; // Kick off SPI transfers (once one is completed another will automatically transmit) overosync->sent_objects = 0; overosync->failed_objects = 0; overosync->received_objects = 0; uint32_t lastUpdateTime = PIOS_Thread_Systime(); uint32_t updateTime; bool initialized = false; uint8_t last_connected = OVEROSYNCSTATS_CONNECTED_FALSE; // Loop forever while (1) { // Wait for queue message if (PIOS_Queue_Receive(queue, &ev, PIOS_QUEUE_TIMEOUT_MAX) == true) { // For the first seconds do not send updates to allow the // overo to boot. Then enable it and act normally. if (!initialized && PIOS_Thread_Systime() < 5000) { continue; } else if (!initialized) { initialized = true; PIOS_OVERO_Enable(pios_overo_id); } // Process event. This calls transmitData UAVTalkSendObjectTimestamped(uavTalkCon, ev.obj, ev.instId, false, 0); updateTime = PIOS_Thread_Systime(); if(((uint32_t) (updateTime - lastUpdateTime)) > 1000) { // Update stats. This will trigger a local send event too OveroSyncStatsData syncStats; syncStats.Send = overosync->sent_bytes; syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE; syncStats.DroppedUpdates = overosync->failed_objects; syncStats.Packets = PIOS_OVERO_GetPacketCount(pios_overo_id); OveroSyncStatsSet(&syncStats); overosync->failed_objects = 0; overosync->sent_bytes = 0; lastUpdateTime = updateTime; // When first connected, send all the settings. Right now this // will fail since all the settings will overfill the buffer and if (last_connected == OVEROSYNCSTATS_CONNECTED_FALSE && syncStats.Connected == OVEROSYNCSTATS_CONNECTED_TRUE) { UAVObjIterate(&send_settings); } // Because the previous code only happens on connection and the // remote logging program doesn't send the settings to the log // when arming starts we send all settings every thirty seconds static uint32_t second_count = 0; if (second_count ++ > 30) { UAVObjIterate(&send_settings); second_count = 0; } last_connected = syncStats.Connected; } // TODO: Check the receive buffer } } }