static void vSecondaryBlockTimeTestTask( void *pvParameters )
{
TickType_t xTimeWhenBlocking, xBlockedTime;
BaseType_t xData;

	#ifdef USE_STDIO
	void vPrintDisplayMessage( const char * const * ppcMessageToSend );
	
		const char * const pcTaskStartMsg = "Alt secondary block time test started.\r\n";

		/* Queue a message for printing to say the task has started. */
		vPrintDisplayMessage( &pcTaskStartMsg );
	#endif

	( void ) pvParameters;

	for( ;; )
	{
		/*********************************************************************
        Test 1 and 2

		This task does does not participate in these tests. */
		vTaskSuspend( NULL );

		/*********************************************************************
        Test 3

		The first thing we do is attempt to read from the queue.  It should be
		full so we block.  Note the time before we block so we can check the
		wake time is as per that expected. */
		portENTER_CRITICAL();
		{
			xTimeWhenBlocking = xTaskGetTickCount();
			
			/* We should unblock after bktTIME_TO_BLOCK having not received
			anything on the queue. */
			xData = 0;
			xRunIndicator = bktRUN_INDICATOR;
			if( xQueueAltSendToBack( xTestQueue, &xData, bktTIME_TO_BLOCK ) != errQUEUE_FULL )
			{
				xErrorOccurred = pdTRUE;
			}

			/* How long were we inside the send function? */
			xBlockedTime = xTaskGetTickCount() - xTimeWhenBlocking;
		}
		portEXIT_CRITICAL();

		/* We should not have blocked for less time than bktTIME_TO_BLOCK. */
		if( xBlockedTime < bktTIME_TO_BLOCK )
		{
			xErrorOccurred = pdTRUE;
		}

		/* We should of not blocked for much longer than bktALLOWABLE_MARGIN
		either.  A margin is permitted as we would not necessarily run as
		soon as we unblocked. */
		if( xBlockedTime > ( bktTIME_TO_BLOCK + bktALLOWABLE_MARGIN ) )
		{
			xErrorOccurred = pdTRUE;
		}

		/* Suspend ready for test 3. */
		xRunIndicator = bktRUN_INDICATOR;
		vTaskSuspend( NULL );

		/*********************************************************************
        Test 4

		As per test three, but with the send and receive reversed. */
		portENTER_CRITICAL();
		{
			xTimeWhenBlocking = xTaskGetTickCount();
			
			/* We should unblock after bktTIME_TO_BLOCK having not received
			anything on the queue. */
			xRunIndicator = bktRUN_INDICATOR;
			if( xQueueAltReceive( xTestQueue, &xData, bktTIME_TO_BLOCK ) != errQUEUE_EMPTY )
			{
				xErrorOccurred = pdTRUE;
			}

			xBlockedTime = xTaskGetTickCount() - xTimeWhenBlocking;
		}
		portEXIT_CRITICAL();

		/* We should not have blocked for less time than bktTIME_TO_BLOCK. */
		if( xBlockedTime < bktTIME_TO_BLOCK )
		{
			xErrorOccurred = pdTRUE;
		}

		/* We should of not blocked for much longer than bktALLOWABLE_MARGIN
		either.  A margin is permitted as we would not necessarily run as soon
		as we unblocked. */
		if( xBlockedTime > ( bktTIME_TO_BLOCK + bktALLOWABLE_MARGIN ) )
		{
			xErrorOccurred = pdTRUE;
		}

		xRunIndicator = bktRUN_INDICATOR;

		xSecondaryCycles++;
	}
}
Exemple #2
0
static void vSerialTxCoRoutine( xCoRoutineHandle xHandle, unsigned portBASE_TYPE uxIndex )
{
portTickType xDelayPeriod;
static unsigned long *pulRandomBytes = mainFIRST_PROGRAM_BYTES;

	/* Co-routine MUST start with a call to crSTART. */
	crSTART( xHandle );

	for(;;)
    {	
		/* Was the previously transmitted string received correctly? */
		if( uxErrorStatus != pdPASS )
		{
			/* An error was encountered so set the error LED. */
			vSetErrorLED();
		}

		/* The next character to Tx is the first in the string. */
		cNextChar = mainFIRST_TX_CHAR;

		UARTIntDisable( UART0_BASE, UART_INT_TX );
		{
			/* Send the first character. */
			if( !( HWREG( UART0_BASE + UART_O_FR ) & UART_FR_TXFF ) )
			{
				HWREG( UART0_BASE + UART_O_DR ) = cNextChar;
			}

			/* Move the variable to the char to Tx on so the ISR transmits
			the next character in the string once this one has completed. */
			cNextChar++;
		}
		UARTIntEnable(UART0_BASE, UART_INT_TX);

		/* Toggle the LED to show a new string is being transmitted. */
        vParTestToggleLED( mainCOMMS_TX_LED );

		/* Delay before we start the string off again.  A pseudo-random delay
		is used as this will provide a better test. */
		xDelayPeriod = xTaskGetTickCount() + ( *pulRandomBytes );

		pulRandomBytes++;
		if( pulRandomBytes > mainTOTAL_PROGRAM_MEMORY )
		{
			pulRandomBytes = mainFIRST_PROGRAM_BYTES;
		}

		/* Make sure we don't wait too long... */
		xDelayPeriod &= mainMAX_TX_DELAY;

		/* ...but we do want to wait. */
		if( xDelayPeriod < mainMIN_TX_DELAY )
		{
			xDelayPeriod = mainMIN_TX_DELAY;
		}

		/* Block for the random(ish) time. */
		crDELAY( xHandle, xDelayPeriod );
    }

	/* Co-routine MUST end with a call to crEND. */
	crEND();
}
Exemple #3
0
static void vCheckTask( void *pvParameters )
{
static unsigned long ulErrorDetected = pdFALSE;	
portTickType xLastExecutionTime;
unsigned char *ucErrorMessage = ( unsigned char * )"              FAIL";
unsigned char *ucSuccessMessage = ( unsigned char * )"              PASS";
unsigned portBASE_TYPE uxColumn = mainMAX_WRITE_COLUMN;
LCDMessage xMessage;

	/* Initialise xLastExecutionTime so the first call to vTaskDelayUntil()
	works correctly. */
	xLastExecutionTime = xTaskGetTickCount();

	for( ;; )
	{
		/* Wait until it is time for the next cycle. */
		vTaskDelayUntil( &xLastExecutionTime, mainCHECK_TASK_CYCLE_TIME );

		/* Has an error been found in any of the standard demo tasks? */
		
		if( xAreIntegerMathsTaskStillRunning() != pdTRUE )
		{
			ulErrorDetected = pdTRUE;
		}

		if( xAreBlockTimeTestTasksStillRunning() != pdTRUE )
		{
			ulErrorDetected = pdTRUE;
		}

		if( xAreBlockingQueuesStillRunning() != pdTRUE )
		{
			ulErrorDetected = pdTRUE;
		}
		
		if( xAreComTestTasksStillRunning() != pdTRUE )
		{
			ulErrorDetected = pdTRUE;
		}		
		
		if( xAreDynamicPriorityTasksStillRunning() != pdTRUE )
		{
			ulErrorDetected = pdTRUE;
		}		
	
		/* Calculate the LCD line on which we would like the message to
		be displayed.  The column variable is used for convenience as
		it is incremented each cycle anyway. */
		xMessage.ucLine = ( unsigned char ) ( uxColumn & 0x01 );

		/* The message displayed depends on whether an error was found or
		not.  Any discovered error is latched.  Here the column variable
		is used as an index into the text string as a simple way of moving
		the text from column to column. */		
		if( ulErrorDetected == pdFALSE )
		{
			xMessage.pucString = ucSuccessMessage + uxColumn;
		}
		else
		{
			xMessage.pucString = ucErrorMessage + uxColumn;			
		}		

		/* Send the message to the print task for display. */
		xQueueSend( xLCDQueue, ( void * ) &xMessage, mainNO_DELAY );
		
		/* Make sure the message is printed in a different column the next
		time around. */
		uxColumn--;
		if( uxColumn == 0 )
		{
			uxColumn = mainMAX_WRITE_COLUMN;
		}
	}
}
Exemple #4
0
void vIMU_tasks()
{
	uint8_t time_count;
	portTickType ticks_now,ticks_old = 0;
	uint8_t IMU_update_count = 0;

   	IMU_setup();
   	Baro_init();

    if(RCC_GetFlagStatus(RCC_FLAG_SFTRST) == RESET)		//if it was a hardware reset
	{
		NVIC_SystemReset();								//force software reset so that MPU sensitivity returns to normal range
	}
    NVIC_Configuration();
	SD_Init();

//////////////////////////****************************//////////////////////////////
//	create_log_file();		//Uncomment to enable log on start, else log will start only when the button on GCS is pressed
//////////////////////////****************************//////////////////////////////

	for(int y=0; y<=5; y++)   // Read first initial ADC values for offset.
		AN_OFFSET[y] = 0;

    for(;;)
	{
    	WDT_status |= 1;		//update status for IMU tasks
    	if(log_init_flag)
		{
			log_data();			//append new data to buffer
		}
    	ticks_now = xTaskGetTickCount()*2;

    	if((ticks_now - ticks_old) >= 17)		//do the following every 17ms
    	{
    		G_Dt = (ticks_now - ticks_old)/1000.0;
    		ticks_old = ticks_now;

    		if((!calibrating_IMU) && (!calibrating_mag))	//update IMU only if not calibrating IMU or MAG
    		{
    			IMU_Update_ARDU();
    			HMC5883_calculate(roll,pitch);
    			Matrix_update();
    		    Normalize();
    		    Drift_correction();
    		    Euler_angles();
    		    update_variables();
    		}
    		else if(calibrating_mag)						//if calibrating MAG then do not process IMU, only get raw values and obtain MAG offsets
    		{
    		    if(IMU_update_count >= 5)
    			{
    		    	IMU_Update_ARDU();
    				IMU_update_count = 0;
    			}
    			else
    				IMU_update_count++;
    		}
			altitude = ((Baro_update(G_Dt*1000)) - ground_alt_offset);		//time in ms

			innerloop();									//set servo output
		    if(time_count == 5)								//do this every 5*17ms ~= 100ms
		    {
		    	send_attitude();
		    	send_attitude_RAW();
		    	time_count = 0;
		    }
		    else
		    	time_count++;
    	}
		vTaskDelay( 17/ portTICK_RATE_MS );					//3ms less to compensate for IMU update time
	}
}
Exemple #5
0
uint32_t Task::getSysticks()
{
	return xTaskGetTickCount();
}
Exemple #6
0
/**
 * 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;
	
	portTickType lastUpdateTime = xTaskGetTickCount();
	portTickType updateTime;

	bool initialized = false;
	uint8_t last_connected = OVEROSYNCSTATS_CONNECTED_FALSE;

	// Loop forever
	while (1) {
		// Wait for queue message
		if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) {

			// For the first seconds do not send updates to allow the
			// overo to boot.  Then enable it and act normally.
			if (!initialized && xTaskGetTickCount() < 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 = xTaskGetTickCount();
			if(((portTickType) (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
		}
	}
}
/**
 * Module task
 */
static void stabilizationTask(void* parameters)
{
	UAVObjEvent ev;

	StabilizationSettingsData stabSettings;
	ActuatorDesiredData actuatorDesired;
	AttitudeDesiredData attitudeDesired;
	AttitudeActualData attitudeActual;
	ManualControlCommandData manualControl;
	SystemSettingsData systemSettings;
	portTickType lastSysTime;
	portTickType thisSysTime;
	float pitchErrorGlobal, pitchErrorLastGlobal;
	float yawErrorGlobal, yawErrorLastGlobal;
	float pitchError, pitchErrorLast;
	float yawError, yawErrorLast;
	float rollError, rollErrorLast;
	float pitchDerivative;
	float yawDerivative;
	float rollDerivative;
	float pitchIntegral, pitchIntegralLimit;
	float yawIntegral, yawIntegralLimit;
	float rollIntegral, rollIntegralLimit;
	float yawPrevious;
	float yawChange;

	// Initialize
	pitchIntegral = 0.0;
	yawIntegral = 0.0;
	rollIntegral = 0.0;
	pitchErrorLastGlobal = 0.0;
	yawErrorLastGlobal = 0.0;
	rollErrorLast = 0.0;
	yawPrevious = 0.0;

	// Main task loop
	lastSysTime = xTaskGetTickCount();
	while (1)
	{
		// Wait until the ActuatorDesired 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);
                }

		// 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;

		// Read settings and other objects
		StabilizationSettingsGet(&stabSettings);
		SystemSettingsGet(&systemSettings);
		ManualControlCommandGet(&manualControl);
		AttitudeDesiredGet(&attitudeDesired);
		AttitudeActualGet(&attitudeActual);

		// For all three axis, calculate Error and ErrorLast - translating from global to local reference frame.

		// global pitch error
		if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
		{
			pitchErrorGlobal = angleDifference(
					bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch
				);
		} 
		else
		{
			// in AUTO mode, Stabilization is used to steer the plane freely,
			// while Navigation dictates the flight path, including maneuvers outside stable limits.
			pitchErrorGlobal = angleDifference(attitudeDesired.Pitch - attitudeActual.Pitch);
		}

		// global yaw error
		if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
		{
			// VTOLS consider yaw. AUTO mode considers YAW, too.
			if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) {  // rate stabilization on yaw
					yawChange = (attitudeActual.Yaw - yawPrevious) / dT;
					yawPrevious = attitudeActual.Yaw;
					yawErrorGlobal = angleDifference(bound(attitudeDesired.Yaw, -stabSettings.YawMax, stabSettings.YawMax) - yawChange);
				} else { // heading stabilization
					yawError = angleDifference(attitudeDesired.Yaw - attitudeActual.Yaw);
				}
		} else {
			// FIXED WING STABILIZATION however does not.
			yawErrorGlobal = 0;
		}
	
		// local pitch error
		pitchError = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal;
		// local roll error (no translation needed - always local)
		if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
		{
			rollError = angleDifference(
					bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll
				);
		}
		else
		{
			// in AUTO mode, Stabilization is used to steer the plane freely,
			// while Navigation dictates the flight path, including maneuvers outside stable limits.
			rollError = angleDifference(attitudeDesired.Roll - attitudeActual.Roll);
		}
		// local yaw error
		yawError = cos(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal;
		
		// for the derivative, the local last errors are needed. Therefore global lasts are translated into local lasts

		// pitch last
		pitchErrorLast = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal;

		// yaw last
		yawErrorLast = cos(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal;
		
		// global last variables are no longer needed 
		pitchErrorLastGlobal = pitchErrorGlobal;
		yawErrorLastGlobal = yawErrorGlobal;
		
		// local Pitch stabilization control loop
		pitchDerivative = pitchError - pitchErrorLast;
		pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
		pitchIntegral = bound(pitchIntegral+pitchError*dT, -pitchIntegralLimit, pitchIntegralLimit);
		actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
		actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);

		// local Roll stabilization control loop
		rollDerivative = rollError - rollErrorLast;
		rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
		rollIntegral = bound(rollIntegral+rollError*dT, -rollIntegralLimit, rollIntegralLimit);
		actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
		actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
		rollErrorLast = rollError;


		// local Yaw stabilization control loop (only enabled on VTOL airframes)
		if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
		{
			yawDerivative = yawError - yawErrorLast;
			yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
			yawIntegral = bound(yawIntegral+yawError*dT, -yawIntegralLimit, yawIntegralLimit);
			actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
			actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
		}
		else
		{
			actuatorDesired.Yaw = 0.0;
		}


		// Setup throttle
		actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);

		// Save dT
		actuatorDesired.UpdateTime = dT * 1000;

		// Write actuator desired (if not in manual mode)
		if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
		{
			ActuatorDesiredSet(&actuatorDesired);
		}
		else
		{
			pitchIntegral = 0.0;
			yawIntegral = 0.0;
			rollIntegral = 0.0;
			pitchErrorLastGlobal = 0.0;
			yawErrorLastGlobal = 0.0;
			rollErrorLast = 0.0;
		}

		// Clear alarms
		AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
	}
}
/**
 * Telemetry transmit task. Processes queue events and periodic updates.
 */
static void telemetryRxTask(void *parameters)
{
	uint32_t inputPort;
	uint8_t	c;

	// Task loop
	while (1) {
#if defined(PIOS_INCLUDE_USB_HID)
		// Determine input port (USB takes priority over telemetry port)
		if (PIOS_USB_HID_CheckAvailable(0)) {
			inputPort = PIOS_COM_TELEM_USB;
		} else
#endif /* PIOS_INCLUDE_USB_HID */
		{
			inputPort = telemetryPort;
		}

		mavlink_channel_t mavlink_chan = MAVLINK_COMM_0;

		// Block until a byte is available
		PIOS_COM_ReceiveBuffer(inputPort, &c, 1, portMAX_DELAY);

		// And process it

		if (mavlink_parse_char(mavlink_chan, c, &rx_msg, &rx_status))
		{

			// Handle packet with waypoint component
			mavlink_wpm_message_handler(&rx_msg);

			// Handle packet with parameter component
			mavlink_pm_message_handler(mavlink_chan, &rx_msg);

			switch (rx_msg.msgid)
			{
			case MAVLINK_MSG_ID_HEARTBEAT:
			{
				// Check if this is the gcs
				mavlink_heartbeat_t beat;
				mavlink_msg_heartbeat_decode(&rx_msg, &beat);
				if (beat.type == MAV_TYPE_GCS)
				{
					// Got heartbeat from the GCS, we're good!
					lastOperatorHeartbeat = xTaskGetTickCount() * portTICK_RATE_MS;
				}
			}
			break;
			case MAVLINK_MSG_ID_SET_MODE:
			{
				mavlink_set_mode_t mode;
				mavlink_msg_set_mode_decode(&rx_msg, &mode);
				// Check if this system should change the mode
				if (mode.target_system == mavlink_system.sysid)
				{
					FlightStatusData flightStatus;
					FlightStatusGet(&flightStatus);

					switch (mode.base_mode)
					{
					case MAV_MODE_MANUAL_ARMED:
					{
						flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
						flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
					}
					break;
					case MAV_MODE_MANUAL_DISARMED:
					{
						flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
						flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
					}
					break;
					case MAV_MODE_PREFLIGHT:
					{
						flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
					}
					break;
					case MAV_MODE_STABILIZE_ARMED:
					{
						flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
						flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
					}
					break;
					case MAV_MODE_GUIDED_ARMED:
					{
						flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED2;
						flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
					}
					break;
					case MAV_MODE_AUTO_ARMED:
					{
						flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED3;
						flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
					}
					break;
					}

					bool newHilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
					if (newHilEnabled != hilEnabled)
					{
						if (newHilEnabled)
						{
							// READ-ONLY flag write to ActuatorCommand
							UAVObjMetadata meta;
							UAVObjHandle handle = ActuatorCommandHandle();
							UAVObjGetMetadata(handle, &meta);
							meta.access = ACCESS_READONLY;
							UAVObjSetMetadata(handle, &meta);

							mavlink_missionlib_send_gcs_string("ENABLING HIL SIMULATION");
							mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
							mavlink_missionlib_send_gcs_string("BLOCKING ALL ACTUATORS");
						}
						else
						{
							// READ-ONLY flag write to ActuatorCommand
							UAVObjMetadata meta;
							UAVObjHandle handle = ActuatorCommandHandle();
							UAVObjGetMetadata(handle, &meta);
							meta.access = ACCESS_READWRITE;
							UAVObjSetMetadata(handle, &meta);

							mavlink_missionlib_send_gcs_string("DISABLING HIL SIMULATION");
							mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
							mavlink_missionlib_send_gcs_string("ACTIVATING ALL ACTUATORS");
						}
					}
					hilEnabled = newHilEnabled;

					FlightStatusSet(&flightStatus);

					// Check HIL
					bool hilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
					enableHil(hilEnabled);
				}
			}
			break;
			case MAVLINK_MSG_ID_HIL_STATE:
			{
				if (hilEnabled)
				{
					mavlink_hil_state_t hil;
					mavlink_msg_hil_state_decode(&rx_msg, &hil);

					// Write GPSPosition
					GPSPositionData gps;
					GPSPositionGet(&gps);
					gps.Altitude = hil.alt/10;
					gps.Latitude = hil.lat/10;
					gps.Longitude = hil.lon/10;
					GPSPositionSet(&gps);

					// Write PositionActual
					PositionActualData pos;
					PositionActualGet(&pos);
					// FIXME WRITE POSITION HERE
					PositionActualSet(&pos);

					// Write AttitudeActual
					AttitudeActualData att;
					AttitudeActualGet(&att);
					att.Roll = hil.roll;
					att.Pitch = hil.pitch;
					att.Yaw = hil.yaw;
					// FIXME
					//att.RollSpeed = hil.rollspeed;
					//att.PitchSpeed = hil.pitchspeed;
					//att.YawSpeed = hil.yawspeed;

					// Convert to quaternion formulation
					RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1);
					// Write AttitudeActual
					AttitudeActualSet(&att);

					// Write AttitudeRaw
					AttitudeRawData raw;
					AttitudeRawGet(&raw);
					raw.gyros[0] = hil.rollspeed;
					raw.gyros[1] = hil.pitchspeed;
					raw.gyros[2] = hil.yawspeed;
					raw.accels[0] = hil.xacc;
					raw.accels[1] = hil.yacc;
					raw.accels[2] = hil.zacc;
					//				raw.magnetometers[0] = hil.xmag;
					//				raw.magnetometers[0] = hil.ymag;
					//				raw.magnetometers[0] = hil.zmag;
					AttitudeRawSet(&raw);
				}
			}
			break;
			case MAVLINK_MSG_ID_COMMAND_LONG:
			{
				// FIXME Implement
			}
			break;
			}
		}
	}
}
Exemple #9
0
UINT32 StatsTimerGet()
{
	return xTaskGetTickCount();
}
Exemple #10
0
static void prvCheckTask( void *pvParameters )
{
unsigned ulLastRegTest1Count = 0, ulLastRegTest2Count = 0;
portTickType xLastExecutionTime;

	/* To prevent compiler warnings. */
	( void ) pvParameters;

	/* Initialise the variable used to control our iteration rate prior to
	its first use. */
	xLastExecutionTime = xTaskGetTickCount();

	for( ;; )
	{
		/* Wait until it is time to run the tests again. */
		vTaskDelayUntil( &xLastExecutionTime, mainCHECK_TASK_PERIOD );

		/* Has an error been found in any task? */
		if( xAreGenericQueueTasksStillRunning() != pdTRUE )
		{
			ulErrorCode |= 0x01UL;
		}

		if( xAreQueuePeekTasksStillRunning() != pdTRUE )
		{
			ulErrorCode |= 0x02UL;
		}

		if( xAreBlockingQueuesStillRunning() != pdTRUE )
		{
			ulErrorCode |= 0x04UL;
		}

		if( xAreSemaphoreTasksStillRunning() != pdTRUE )
	    {
	    	ulErrorCode |= 0x20UL;
	    }

		if( xArePollingQueuesStillRunning() != pdTRUE )
	    {
	    	ulErrorCode |= 0x40UL;
	    }

		if( xAreBlockTimeTestTasksStillRunning() != pdTRUE )
		{
			ulErrorCode |= 0x80UL;
		}

	    if( xAreRecursiveMutexTasksStillRunning() != pdTRUE )
	    {
	    	ulErrorCode |= 0x100UL;
	    }

		if( ulLastRegTest1Count == ulRegTest1Counter )
		{
			ulErrorCode |= 0x200UL;
		}

		if( ulLastRegTest2Count == ulRegTest2Counter )
		{
			ulErrorCode |= 0x200UL;
		}

		/* Remember the reg test counts so a stall in their values can be
		detected next time around. */
		ulLastRegTest1Count = ulRegTest1Counter;
		ulLastRegTest2Count = ulRegTest2Counter;
	}
}
/**
 * Update telemetry statistics and handle connection handshake
 */
static void updateTelemetryStats()
{
	//	flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
	//	gcsStats.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;

	FlightTelemetryStatsData flightStats;
	GCSTelemetryStatsData gcsStats;
	uint8_t forceUpdate;
	uint8_t connectionTimeout;
	uint32_t timeNow;

	// Get object data
	FlightTelemetryStatsGet(&flightStats);
	GCSTelemetryStatsGet(&gcsStats);

	// Update stats object
	if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
		//flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0);
		//flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0);
		//flightStats.RxFailures += utalkStats.rxErrors;
		flightStats.TxFailures += txErrors;
		flightStats.TxRetries += txRetries;
		txErrors = 0;
		txRetries = 0;
	} else {
		flightStats.RxDataRate = 0;
		flightStats.TxDataRate = 0;
		flightStats.RxFailures = 0;
		flightStats.TxFailures = 0;
		flightStats.TxRetries = 0;
		txErrors = 0;
		txRetries = 0;
	}

	// Check for connection timeout
	timeNow = xTaskGetTickCount() * portTICK_RATE_MS;
	if ((timeNow - lastOperatorHeartbeat) > CONNECTION_TIMEOUT_MS) {
		connectionTimeout = 1;
	} else {
		connectionTimeout = 0;
	}

	// Update connection state
	forceUpdate = 1;

	// FIXME Hardcode value for now
	//	connectionTimeout = 0;

	// If the GCS heartbeat has been received in the last five seconds
	// set as connected
	if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) {
		if (connectionTimeout == 0) {
			// Switching from disconnected to connected
			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
			gcsStats.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
		} else {
			// Not switching, no update needed
			forceUpdate = 0;
		}
	} else {
		// We were connected, checking if we're still connected
		if (connectionTimeout == 1) {
			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
			gcsStats.Status = GCSTELEMETRYSTATS_STATUS_DISCONNECTED;
		} else {
			// We were connected and still are, no update needed
			forceUpdate = 0;
		}
	}

	// FIXME HARDCODED VALUES
	flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
	gcsStats.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
	forceUpdate = 1;

	//	if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) {
	//		// Wait for connection request
	//		if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ) {
	//			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK;
	//		}
	//	} else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK) {
	//		// Wait for connection
	//		if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_CONNECTED) {
	//			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
	//		} else if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_DISCONNECTED) {
	//			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
	//		}
	//	} else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
	//		if (gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || connectionTimeout) {
	//			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
	//		} else {
	//			forceUpdate = 0;
	//		}
	//	} else {
	//		flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
	//	}

	// Update the telemetry alarm
	if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
		AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
	} else {
		AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR);
	}

	// Update object
	FlightTelemetryStatsSet(&flightStats);

	// Force telemetry update if not connected
	if (forceUpdate) {
		FlightTelemetryStatsUpdated();
	}
}
Exemple #12
0
static void prvCheckTask( void *pvParameters )
{
unsigned long ulLastRegTest1Value = 0, ulLastRegTest2Value = 0, ulTicksToWait = mainNO_ERROR_PERIOD;
portTickType xLastExecutionTime;

/* Buffer into which the high frequency timer count is written as a string. */
static char cStringBuffer[ mainMAX_STRING_LENGTH ];

/* The count of the high frequency timer interrupts. */
extern unsigned long ulHighFrequencyTimerInterrupts;
xLCDMessage xMessage = { ( 200 / portTICK_RATE_MS ), cStringBuffer };

	/* Setup the high frequency, high priority, timer test.  It is setup here
	to ensure it does not fire before the scheduler is started. */
	vSetupTimerTest( mainTEST_INTERRUPT_FREQUENCY );

	/* Initialise the variable used to control our iteration rate prior to
	its first use. */
	xLastExecutionTime = xTaskGetTickCount();

	for( ;; )
	{
		/* Wait until it is time to run the tests again. */
		vTaskDelayUntil( &xLastExecutionTime, ulTicksToWait );

		/* Has either register check 1 or 2 task discovered an error? */
		if( ulStatus1 != pdPASS )
		{
			ulTicksToWait = mainERROR_PERIOD;
			xMessage.pcMessage = "Error: Reg test1";
		}

		/* Check that the register test 1 task is still running. */
		if( ulLastRegTest1Value == ulRegTest1Cycles )
		{
			ulTicksToWait = mainERROR_PERIOD;
			xMessage.pcMessage = "Error: Reg test2";
		}
		ulLastRegTest1Value = ulRegTest1Cycles;

		
		/* Check that the register test 2 task is still running. */
		if( ulLastRegTest2Value == ulRegTest2Cycles )
		{
			ulTicksToWait = mainERROR_PERIOD;
			xMessage.pcMessage = "Error: Reg test3";
		}
		ulLastRegTest2Value = ulRegTest2Cycles;
		

		/* Have any of the standard demo tasks detected an error in their 
		operation? */
		if( xAreGenericQueueTasksStillRunning() != pdTRUE )
		{
			ulTicksToWait = mainERROR_PERIOD;
			xMessage.pcMessage = "Error: Gen Q";
		}
		else if( xAreQueuePeekTasksStillRunning() != pdTRUE )
		{
			ulTicksToWait = mainERROR_PERIOD;
			xMessage.pcMessage = "Error: Q Peek";
		}
		else if( xAreComTestTasksStillRunning() != pdTRUE )
		{
			ulTicksToWait = mainERROR_PERIOD;
			xMessage.pcMessage = "Error: COM test";
		}
		else if( xAreBlockTimeTestTasksStillRunning() != pdTRUE )
		{
			ulTicksToWait = mainERROR_PERIOD;
			xMessage.pcMessage = "Error: Blck time";
		}
	    else if( xAreSemaphoreTasksStillRunning() != pdTRUE )
	    {
	        ulTicksToWait = mainERROR_PERIOD;
			xMessage.pcMessage = "Error: Sem test";
	    }
		else if( xAreIntQueueTasksStillRunning() != pdTRUE )
		{
			ulTicksToWait = mainERROR_PERIOD;
			xMessage.pcMessage = "Error: Int queue";
		}

		/* Write the ulHighFrequencyTimerInterrupts value to the string 
		buffer.  It will only be displayed if no errors have been detected. */
		sprintf( cStringBuffer, "Pass %u", ( unsigned int ) ulHighFrequencyTimerInterrupts );

		xQueueSend( xLCDQueue, &xMessage, mainDONT_WAIT );
		vParTestToggleLED( mainCHECK_LED );
	}
}
Exemple #13
0
static void vErrorChecks( void *pvParameters )
{
portTickType xExpectedWakeTime;
const portTickType xPrintRate = ( portTickType ) 5000 / portTICK_RATE_MS;
const long lMaxAllowableTimeDifference = ( long ) 0;
portTickType xWakeTime;
long lTimeDifference;
const char *pcReceivedMessage;
const char * const pcTaskBlockedTooLongMsg = "Print task blocked too long!\r\n";
const char * const pcUnexpectedHookValueMsg = "Task hook has unexpected value!\r\n";

	( void ) pvParameters;

	/* Register our callback function. */
	vTaskSetApplicationTaskTag( NULL, prvExampleTaskHook );
	
	/* Just for test purposes. */
	if( xTaskGetApplicationTaskTag( NULL ) != prvExampleTaskHook )
	{
		vPrintDisplayMessage( &pcUnexpectedHookValueMsg );
	}

	/* Loop continuously, blocking, then checking all the other tasks are still
	running, before blocking once again.  This task blocks on the queue of
	messages that require displaying so will wake either by its time out expiring,
	or a message becoming available. */
	for( ;; )
	{
		/* Calculate the time we will unblock if no messages are received
		on the queue.  This is used to check that we have not blocked for too long. */
		xExpectedWakeTime = xTaskGetTickCount();
		xExpectedWakeTime += xPrintRate;

		/* Block waiting for either a time out or a message to be posted that
		required displaying. */
		pcReceivedMessage = pcPrintGetNextMessage( xPrintRate );

		/* Was a message received? */
		if( pcReceivedMessage == NULL )
		{
			/* A message was not received so we timed out, did we unblock at the
			expected time? */
			xWakeTime = xTaskGetTickCount();

			/* Calculate the difference between the time we unblocked and the
			time we should have unblocked. */
			if( xWakeTime > xExpectedWakeTime )
			{
				lTimeDifference = ( long ) ( xWakeTime - xExpectedWakeTime );
			}
			else
			{
				lTimeDifference = ( long ) ( xExpectedWakeTime - xWakeTime );
			}

			if( lTimeDifference > lMaxAllowableTimeDifference )
			{
				/* We blocked too long - create a message that will get
				printed out the next time around.  If we are not using
				preemption then we won't expect the timing to be so
				accurate. */
				if( sUsingPreemption == pdTRUE )
				{
					vPrintDisplayMessage( &pcTaskBlockedTooLongMsg );
				}
			}

			/* Check the other tasks are still running, just in case. */
			prvCheckOtherTasksAreStillRunning();
		}
		else
		{
			/* We unblocked due to a message becoming available.  Send the message
			for printing. */
			vDisplayMessage( pcReceivedMessage );
		}

		/* Key presses are used to invoke the trace visualisation utility, or end
		the program. */
		prvCheckForKeyPresses();
	}
}
Exemple #14
0
/**
 * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
 */
static void systemTask(void *parameters)
{
    portTickType lastSysTime;

    /* 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;
    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

#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);

        // 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);
        }
    }
}
Exemple #15
0
//
//int thk_set_dep_ant_deployed(void* param){
//    MemEEPROM_Vars mem_eeprom_var = mem_dep_ant_deployed;
//    int value = *((int*)param);
//    writeIntEEPROM1(mem_eeprom_var, value);
//    return 1;   //se asume operacion exitosa
//}
//int thk_set_dep_ant_tries(void* param){
//    MemEEPROM_Vars mem_eeprom_var = mem_dep_ant_tries;
//    int value = *((int*)param);
//    writeIntEEPROM1(mem_eeprom_var, value);
//    return 1;   //se asume operacion exitosa
//}
//int thk_set_dep_year(void* param){
//    MemEEPROM_Vars mem_eeprom_var = mem_dep_year;
//    int value = *((int*)param);
//    writeIntEEPROM1(mem_eeprom_var, value);
//    return 1;   //se asume operacion exitosa
//}
//int thk_set_dep_month(void* param){
//    MemEEPROM_Vars mem_eeprom_var = mem_dep_month;
//    int value = *((int*)param);
//    writeIntEEPROM1(mem_eeprom_var, value);
//    return 1;   //se asume operacion exitosa
//}
//int thk_set_dep_week_day(void* param){
//    MemEEPROM_Vars mem_eeprom_var = mem_dep_week_day;
//    int value = *((int*)param);
//    writeIntEEPROM1(mem_eeprom_var, value);
//    return 1;   //se asume operacion exitosa
//}
//int thk_set_dep_day_number(void* param){
//    MemEEPROM_Vars mem_eeprom_var = mem_dep_day_number;
//    int value = *((int*)param);
//    writeIntEEPROM1(mem_eeprom_var, value);
//    return 1;   //se asume operacion exitosa
//}
//int thk_set_dep_hours(void* param){
//    MemEEPROM_Vars mem_eeprom_var = mem_dep_hours;
//    int value = *((int*)param);
//    writeIntEEPROM1(mem_eeprom_var, value);
//    return 1;   //se asume operacion exitosa
//}
//int thk_set_dep_minutes(void* param){
//    MemEEPROM_Vars mem_eeprom_var = mem_dep_minutes;
//    int value = *((int*)param);
//    writeIntEEPROM1(mem_eeprom_var, value);
//    return 1;   //se asume operacion exitosa
//}
//int thk_set_dep_seconds(void* param){
//    MemEEPROM_Vars mem_eeprom_var = mem_dep_seconds;
//    int value = *((int*)param);
//    writeIntEEPROM1(mem_eeprom_var, value);
//    return 1;   //se asume operacion exitosa
//}
//------------------------------------------------------------------------------
int thk_suchai_deployment(void *param)
{
    printf("[thk_suchai_deployment] Suchai deployment routine..\r\n");
    
    int delay_min = *( (int*)param );
//    portTickType xLastWakeTime = xTaskGetTickCount();
//    portTickType delay_60s    = 60000;    //Task period in [ms]
//    portTickType delay_tick_60s = delay_60s / portTICK_RATE_MS; //Task period in ticks

    unsigned long initial_tick_10ms = xTaskGetTickCount(); //get initial tick-time
    //unsigned long silent_time_10ms = (180000);     // 30 minutes = 1800 sec = 180000 [10ms]
    unsigned long silent_time_10ms = delay_min*60*100; // time_s*100 [ms] = time_ms
    unsigned long final_tick_10ms = initial_tick_10ms + silent_time_10ms;

    printf("[thk_suchai_deployment] initial_tick_10ms = %lu | final_tick_10ms = %lu \r\n", initial_tick_10ms, final_tick_10ms);

    // print rtc time
    rtc_print(NULL);

    //take picture
    #if(SCH_PAY_CAM_nMEMFLASH_ONBOARD==1 )
        #if(SCH_THK_SILENT_REALTIME==1)
            pay_takePhoto_camera(NULL); //takes 10min to complete
        #else
            printf("  Jumping pay_takePhoto_camera(NULL) call, it takes 10min to complete ..\r\n");
        #endif
    #endif

    // print rtc time
    rtc_print(NULL);

    unsigned int elapsed_mins = 0;
    while(TRUE){
        unsigned long int cu_tick_10ms = xTaskGetTickCount();
        if( cu_tick_10ms >= final_tick_10ms || elapsed_mins>35 ){
            printf("[thk_suchai_deployment] Waiting timeout, cu_tick_10ms = %lu, elapsed_mins = %d\r\n", cu_tick_10ms, elapsed_mins);
            break;
        }
        printf("[thk_suchai_deployment] Waiting for timeout, cu_tick_10ms = %lu, elapsed_mins = %d\r\n", cu_tick_10ms, elapsed_mins);
        //vTaskDelayUntil(&xLastWakeTime, delay_tick_60s); //Suspend task 60 sec
        ClrWdt();
        __delay_ms(60000);  //delay 60sec
        elapsed_mins++;

    }

    // print rtc time
    rtc_print(NULL);

    /* Deploy Antena */
    #if (SCH_ANTENNA_ONBOARD==1)
        int rt_mode = SCH_THK_ANT_DEP_REALTIME; /* 1=Real Time, 0=Debug Time */
        thk_deploy_antenna(&rt_mode);
    #endif

    // print rtc time
    rtc_print(NULL);

    ClrWdt();
    __delay_ms(60000);  //delay 60sec to avoid drain-out the EPS

    /* Ceploy langmuir should NOT be here, but there is no way
     * to check deployment, so its included here */
    #if (SCH_ANTENNA_ONBOARD==1 && SCH_PAY_LANGMUIR_ONBOARD==1)
        if( sta_get_stateVar(sta_pay_lagmuirProbe_isDeployed)==0 ){
            int rt_mode = SCH_THK_ANT_DEP_REALTIME; /* 1=Real Time, 0=Debug Time */
            pay_deploy_langmuirProbe(rt_mode);    //realtime mode
            //set var lang dep = 1
        }
    #endif

    //other "only once"-tasks
    //..

    return 1;
}
Exemple #16
0
/* Return current time */
uint32_t time_now(void) {
	return (uint32_t)(xTaskGetTickCount() * (1000/configTICK_RATE_HZ));
}
Exemple #17
0
static void prvCheckTask( void *pvParameters )
{
TickType_t xDelayPeriod = mainNO_ERROR_CHECK_TASK_PERIOD;
TickType_t xLastExecutionTime;
static unsigned long ulLastRegTest1Value = 0, ulLastRegTest2Value = 0;
unsigned long ulErrorFound = pdFALSE;

	/* Just to stop compiler warnings. */
	( void ) pvParameters;

	/* Initialise xLastExecutionTime so the first call to vTaskDelayUntil()
	works correctly. */
	xLastExecutionTime = xTaskGetTickCount();

	/* Cycle for ever, delaying then checking all the other tasks are still
	operating without error.  The onboard LED is toggled on each iteration.
	If an error is detected then the delay period is decreased from
	mainNO_ERROR_CHECK_TASK_PERIOD to mainERROR_CHECK_TASK_PERIOD.  This has the
	effect of increasing the rate at which the onboard LED toggles, and in so
	doing gives visual feedback of the system status. */
	for( ;; )
	{
		/* Delay until it is time to execute again. */
		vTaskDelayUntil( &xLastExecutionTime, xDelayPeriod );

		/* Check all the demo tasks (other than the flash tasks) to ensure
		that they are all still running, and that none have detected an error. */
		if( xAreIntQueueTasksStillRunning() != pdTRUE )
		{
			ulErrorFound = 1UL << 0UL;
		}

		if( xAreMathsTaskStillRunning() != pdTRUE )
		{
			ulErrorFound = 1UL << 1UL;
		}

		if( xAreDynamicPriorityTasksStillRunning() != pdTRUE )
		{
			ulErrorFound = 1UL << 2UL;
		}

		if( xAreBlockingQueuesStillRunning() != pdTRUE )
		{
			ulErrorFound = 1UL << 3UL;
		}

		if ( xAreBlockTimeTestTasksStillRunning() != pdTRUE )
		{
			ulErrorFound = 1UL << 4UL;
		}

		if ( xAreGenericQueueTasksStillRunning() != pdTRUE )
		{
			ulErrorFound = 1UL << 5UL;
		}

		if ( xAreRecursiveMutexTasksStillRunning() != pdTRUE )
		{
			ulErrorFound = 1UL << 6UL;
		}

		if( xIsCreateTaskStillRunning() != pdTRUE )
		{
			ulErrorFound = 1UL << 7UL;
		}

		if( xAreSemaphoreTasksStillRunning() != pdTRUE )
		{
			ulErrorFound = 1UL << 8UL;
		}

		if( xAreTimerDemoTasksStillRunning( ( TickType_t ) xDelayPeriod ) != pdPASS )
		{
			ulErrorFound = 1UL << 9UL;
		}

		if( xAreCountingSemaphoreTasksStillRunning() != pdTRUE )
		{
			ulErrorFound = 1UL << 10UL;
		}

		if( xIsQueueOverwriteTaskStillRunning() != pdPASS )
		{
			ulErrorFound = 1UL << 11UL;
		}

		if( xAreEventGroupTasksStillRunning() != pdPASS )
		{
			ulErrorFound = 1UL << 12UL;
		}

		if( xAreInterruptSemaphoreTasksStillRunning() != pdPASS )
		{
			ulErrorFound = 1UL << 13UL;
		}

		if( xAreTaskNotificationTasksStillRunning() != pdPASS )
		{
			ulErrorFound = 1UL << 14UL;
		}

		/* Check that the register test 1 task is still running. */
		if( ulLastRegTest1Value == ulRegTest1LoopCounter )
		{
			ulErrorFound = 1UL << 15UL;
		}
		ulLastRegTest1Value = ulRegTest1LoopCounter;

		/* Check that the register test 2 task is still running. */
		if( ulLastRegTest2Value == ulRegTest2LoopCounter )
		{
			ulErrorFound = 1UL << 16UL;
		}
		ulLastRegTest2Value = ulRegTest2LoopCounter;

		/* Toggle the check LED to give an indication of the system status.  If
		the LED toggles every mainNO_ERROR_CHECK_TASK_PERIOD milliseconds then
		everything is ok.  A faster toggle indicates an error. */
		ioport_toggle_pin_level( mainTASK_LED );

		if( ulErrorFound != pdFALSE )
		{
			/* An error has been detected in one of the tasks - flash the LED
			at a higher frequency to give visible feedback that something has
			gone wrong (it might just be that the loop back connector required
			by the comtest tasks has not been fitted). */
			xDelayPeriod = mainERROR_CHECK_TASK_PERIOD;
		}
	}
}
Exemple #18
0
static umtick umac_impl_now_tick(void) {
    return xTaskGetTickCount();
}
Exemple #19
0
void vCheckTask( void *pvParameters )
{
    unsigned long ulRow = 0;
    TickType_t xDelay = 0;
    unsigned short usErrorCode = 0;
    unsigned long ulIteration = 0;
    extern unsigned short usMaxJitter;

    /* Intialise the sleeper. */
    xDelay = xTaskGetTickCount();

    for( ;; )
    {
        /* Perform this check every mainCHECK_DELAY milliseconds. */
        vTaskDelayUntil( &xDelay, mainCHECK_DELAY );

        /* Check that all of the Demo tasks are still running. */
        if( pdTRUE != xAreBlockingQueuesStillRunning() )
        {
            usErrorCode |= 0x1;
        }

        if( pdTRUE != xAreBlockTimeTestTasksStillRunning() )
        {
            usErrorCode |= 0x2;
        }

        if( pdTRUE != xAreCountingSemaphoreTasksStillRunning() )
        {
            usErrorCode |= 0x4;
        }

        if( pdTRUE != xIsCreateTaskStillRunning() )
        {
            usErrorCode |= 0x8;
        }

        if( pdTRUE != xAreDynamicPriorityTasksStillRunning() )
        {
            usErrorCode |= 0x10;
        }

        if( pdTRUE != xAreMathsTaskStillRunning() )
        {
            usErrorCode |= 0x20;
        }

        if( pdTRUE != xAreGenericQueueTasksStillRunning() )
        {
            usErrorCode |= 0x40;
        }

        if( pdTRUE != xAreIntegerMathsTaskStillRunning() )
        {
            usErrorCode |= 0x80;
        }

        if( pdTRUE != xArePollingQueuesStillRunning() )
        {
            usErrorCode |= 0x100;
        }

        if( pdTRUE != xAreQueuePeekTasksStillRunning() )
        {
            usErrorCode |= 0x200;
        }

        if( pdTRUE != xAreSemaphoreTasksStillRunning() )
        {
            usErrorCode |= 0x400;
        }

        if( pdTRUE != xAreComTestTasksStillRunning() )
        {
            usErrorCode |= 0x800;
        }

        if( pdTRUE != xAreIntQueueTasksStillRunning() )
        {
            usErrorCode |= 0x1000;
        }

        /* Clear the display. */
        LCD_Character_Display_ClearDisplay();
        if( 0 == usErrorCode )
        {
            LCD_Character_Display_Position( ( ulRow ) & 0x1, 0);
            LCD_Character_Display_PrintString( "Pass: "******"Jitter(ns):" );
            LCD_Character_Display_PrintNumber( ( usMaxJitter * mainNS_PER_CLOCK ) );
        }
        else
        {
            /* Do something to indicate the failure. */
            LCD_Character_Display_Position( ( ulRow ) & 0x1, 0 );
            LCD_Character_Display_PrintString( "Fail at: " );
            LCD_Character_Display_PrintNumber( ulIteration );
            LCD_Character_Display_Position( ( ++ulRow ) & 0x1, 0 );
            LCD_Character_Display_PrintString( "Error: 0x" );
            LCD_Character_Display_PrintHexUint16( usErrorCode );
        }
    }
}
Exemple #20
0
static void vCheckTask( void *pvParameters )
{
/* Used to wake the task at the correct frequency. */
TickType_t xLastExecutionTime; 

/* The maximum jitter time measured by the fast interrupt test. */
extern unsigned short usMaxJitter ;

/* Buffer into which the maximum jitter time is written as a string. */
static char cStringBuffer[ mainMAX_STRING_LENGTH ];

/* The message that is sent on the queue to the LCD task.  The first
parameter is the minimum time (in ticks) that the message should be
left on the LCD without being overwritten.  The second parameter is a pointer
to the message to display itself. */
xLCDMessage xMessage = { 0, cStringBuffer };

/* Set to pdTRUE should an error be detected in any of the standard demo tasks. */
unsigned short usErrorDetected = pdFALSE;

	/* Remove compiler warnings. */
	( void ) pvParameters;

	/* Initialise xLastExecutionTime so the first call to vTaskDelayUntil()
	works correctly. */
	xLastExecutionTime = xTaskGetTickCount();

	for( ;; )
	{
		/* Wait until it is time for the next cycle. */
		vTaskDelayUntil( &xLastExecutionTime, mainCHECK_TASK_PERIOD );

		/* Has an error been found in any of the standard demo tasks? */

		if( xAreIntegerMathsTaskStillRunning() != pdTRUE )
		{
			usErrorDetected = pdTRUE;
			sprintf( cStringBuffer, "FAIL #1" );
		}
	
		if( xAreComTestTasksStillRunning() != pdTRUE )
		{
			usErrorDetected = pdTRUE;
			sprintf( cStringBuffer, "FAIL #2" );
		}

		if( xAreBlockTimeTestTasksStillRunning() != pdTRUE )
		{
			usErrorDetected = pdTRUE;
			sprintf( cStringBuffer, "FAIL #3" );
		}

		if( xAreBlockingQueuesStillRunning() != pdTRUE )
		{
			usErrorDetected = pdTRUE;
			sprintf( cStringBuffer, "FAIL #4" );
		}

		if( usErrorDetected == pdFALSE )
		{
			/* No errors have been discovered, so display the maximum jitter
			timer discovered by the "fast interrupt test". */
			sprintf( cStringBuffer, "%dns max jitter", ( short ) ( usMaxJitter - mainEXPECTED_CLOCKS_BETWEEN_INTERRUPTS ) * mainNS_PER_CLOCK );
		}

		/* Send the message to the LCD gatekeeper for display. */
		xQueueSend( xLCDQueue, &xMessage, portMAX_DELAY );
	}
}
Exemple #21
0
void vmisc_tasks()
{
	uint8_t time_count,time_200ms;
	portTickType ticks_now,ticks_old = 0;
	uint16_t time_elapsed = 0;

	modem_config();
	ublox_init();
//	debug_config();			//configure debug serial port
	/*
	 * data from external sensors initialized, version M21
	 * can disable it by commenting here and changing dataFromInternalSensors from 0 to 1
	 * check externalPackets.h for more information
	 */
		if (!(dataFromInternalSensors))
	/*	initialize external sensors data UART3, debug port and this can't be enabled together		*/
	externalDataPortInit();
	RPM_config();

	ADC_config();
	ADC_SoftwareStartConv(ADC1);
	ADC_3_config();
	ADC_SoftwareStartConv(ADC3);
	PWM_out_config();

	for(;;)
	{
		if(WDT_status == 1)
		{
			IWDG_ReloadCounter();
			WDT_status = 0;		//clear it so that it can be set again if all is OK
		}
		if(xbee_tx_count>0)								//enable TX interrupts if transmit data is present
			USART_ITConfig(USART1, USART_IT_TXE, ENABLE);
		if(gps_tx.count >0)								//enable TX interrupts if transmit data is present
			USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
		if(debug_tx_count >0)								//enable TX interrupts if transmit data is present
			USART_ITConfig(USART3, USART_IT_TXE, ENABLE);

		parse_gps_data();
		if(xbee_rx_count)
			parse_modem_data();

		if(xbee_fresh_data)	//if received fresh data
		{
			calculate_xbee();		//call appropriate function based on received data
			xbee_fresh_data = 0;
		}

		if(log_init_flag)
		{
			if(write_buff_to_card == 1)		//if one buffer is full
			{
				res = f_open(&fil, file_name, FA_OPEN_ALWAYS | FA_WRITE |FA_READ);
				f_lseek(&fil, cursor_pos);
				if(log_buff_status == 1)
				{
					f_write(&fil, SD_buffer2.BUFFER, SD_buffer2.write_count, &bw);
					cursor_pos += SD_buffer2.write_count;
				}
				else if(log_buff_status == 2)
				{
					f_write(&fil, SD_buffer1.BUFFER, SD_buffer1.write_count, &bw);
					cursor_pos += SD_buffer1.write_count;
				}
				write_buff_to_card = 0;
				f_close(&fil);
				GPIOF->ODR ^= GPIO_Pin_12;
			}
		}

    	ticks_now = xTaskGetTickCount()*2;
    	if((ticks_now - ticks_old) >=100)		//do the following every 100ms
    	{
    		time_elapsed = ticks_now - ticks_old;
    	   	rpm = pulse_cnt*2*60/MOTOR_POLES*(1000/time_elapsed);

    	   	pulse_cnt = 0;

    		ticks_old = ticks_now;
    		air_speed_indicator();
    		send_status();
    		if(link_count >= 5 )		//no data from more than 500ms
    		{
    			modem_link_active = 0;	//indicate absence of GIB
    			link_count = 0;
    		}
    		else
    			link_count++;

    		if(time_200ms == 2)			//do the following every 200ms
    		{
    			send_gps_data();
    			time_200ms = 0;
    		}
    		else
    			time_200ms++;

		    if(time_count == 5)			//do the following every 500ms
		    {
		    	queue_data();			//append debug data for transmission
		    	update_battery();
		    	update_RPM();
		    	send_press_temp();
		    	send_defections();
		    	send_power_propulsion();
		    	time_count = 0;
		    }
		    else
		    	time_count++;
    	}
	}
}
static void stabilizerTask(void* param)
{
  uint32_t attitudeCounter = 0;
  uint32_t altHoldCounter = 0;
  uint32_t lastWakeTime;

  vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR);

  //Wait for the system to be fully started to start stabilization loop
  systemWaitStart();

  lastWakeTime = xTaskGetTickCount ();

  while(1)
  {
    vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz

    // Magnetometer not yet used more then for logging.
    imu9Read(&gyro, &acc, &mag);

    if (imu6IsCalibrated())
    {
      commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired);
      commanderGetRPYType(&rollType, &pitchType, &yawType);

      // 250HZ
      if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER)
      {
        sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT);
        sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);

        accWZ = sensfusion6GetAccZWithoutGravity(acc.x, acc.y, acc.z);
        accMAG = (acc.x*acc.x) + (acc.y*acc.y) + (acc.z*acc.z);
        // Estimate speed from acc (drifts)
        vSpeed += deadband(accWZ, vAccDeadband) * FUSION_UPDATE_DT;

        controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual,
                                     eulerRollDesired, eulerPitchDesired, -eulerYawDesired,
                                     &rollRateDesired, &pitchRateDesired, &yawRateDesired);
        attitudeCounter = 0;
      }

      // 100HZ
      if (imuHasBarometer() && (++altHoldCounter >= ALTHOLD_UPDATE_RATE_DIVIDER))
      {
        stabilizerAltHoldUpdate();
        altHoldCounter = 0;
      }

      if (rollType == RATE)
      {
        rollRateDesired = eulerRollDesired;
      }
      if (pitchType == RATE)
      {
        pitchRateDesired = eulerPitchDesired;
      }
      if (yawType == RATE)
      {
        yawRateDesired = -eulerYawDesired;
      }

      // TODO: Investigate possibility to subtract gyro drift.
      controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z,
                               rollRateDesired, pitchRateDesired, yawRateDesired);

      controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw);

      if (!altHold || !imuHasBarometer())
      {
        // Use thrust from controller if not in altitude hold mode
        commanderGetThrust(&actuatorThrust);
      }
      else
      {
        // Added so thrust can be set to 0 while in altitude hold mode after disconnect
        commanderWatchdog();
      }

      if (actuatorThrust > 0)
      {
#if defined(TUNE_ROLL)
        distributePower(actuatorThrust, actuatorRoll, 0, 0);
#elif defined(TUNE_PITCH)
        distributePower(actuatorThrust, 0, actuatorPitch, 0);
#elif defined(TUNE_YAW)
        distributePower(actuatorThrust, 0, 0, -actuatorYaw);
#else
        distributePower(actuatorThrust, actuatorRoll, actuatorPitch, -actuatorYaw);
#endif
      }
      else
      {
        distributePower(0, 0, 0, 0);
        controllerResetAllPID();
      }
    }
  }
}
Exemple #23
0
uint32_t Task::getTime()
{
	return xTaskGetTickCount()/4;
}
Exemple #24
0
/*-----------------------------------------------------------------------------------*/
void vTaskGPIOswitch(void *pvParameters)
{
    portBASE_TYPE xStatus_TaskGPIOsw;
    portTickType xLastWakeTime;
    xLastWakeTime = xTaskGetTickCount();
    xTCPTypeDef      uTCPsw;
    xTCPTypeDef uTCPt;

    //int tau_step_loc = 10 / portTICK_RATE_MS ;


    static portBASE_TYPE xHigherPriorityTaskWoken;
    xHigherPriorityTaskWoken = pdFALSE;


    // начальна¤ инициализаци¤
    T_step = 2*tau_step;
    calc = 1;
    BSRRL_val = GPIO_Pin_0;   // включаем драйвер, запитываютс¤ обмотки
    BSRRH_val = 0;

    for (;;)  {

        // контроль выполенни¤ расчета. –аботала ли задача TaskGPIOcalc?
        if (calc == 1)      {

            if (T_step> tau_step)      {     //скважность больше одного

                GPIOE->BSRRL = BSRRL_val;     // фронт STEP

                // повыаем приоритет задачи вывода
                vTaskPrioritySet( xHandleTaskTCPt, ( configMAX_PRIORITIES - 1 ) );
                vTaskDelay( tau_step);



                GPIOE->BSRRH |= 0x0E;     // спад STEP

                //vTaskDelay( T_step / 2 );
                // if (chg_dir_x)   тогда перекинуть пин
                // if (chg_dir_y)   тогда перекинуть пин
                // if (chg_dir_z)   тогда перекинуть пин


                // ѕ≈–≈ƒј„ј Ќ≈ –јЅќ“ј≈“. ѕќ„≈ћ”??????????
                // ѕередача управлени¤ задаче расчета нового положени¤ с помощью бинарного семафора
                //xSemaphoreGive(xBinarySemaphore);
                /*  xSemaphoreGiveFromISR( xBinarySemaphore,&xHigherPriorityTaskWoken );
                      if( xHigherPriorityTaskWoken == pdTRUE )
                      {

                       taskYIELD();
                      }
                */


                // ”величиваем приоритет TaskGPIOcalc
                vTaskPrioritySet( xHandleTaskGPIOcalc, ( configMAX_PRIORITIES - 2 ) );
                calc = 0;

                // задержка до DIR
                vTaskDelay( (T_step - tau_step) / 2);

                // проверка Change DIR
                if (CDX) {
                    BSRRL_val ^=  1 << 5;         // ѕ–ќ¬≈–»“№  ј  –јЅќ“ј≈“
                    CDX = 0;
                }
                if (CDY) {
                    BSRRL_val ^=  1 << 6;         // ѕ–ќ¬≈–»“№  ј  –јЅќ“ј≈“
                    CDX = 0;
                }
                if (CDZ) {
                    BSRRL_val ^=  1 << 7;         // ѕ–ќ¬≈–»“№  ј  –јЅќ“ј≈“
                    CDX = 0;
                }
                // задержка после DIR


                // vTaskDelayUntil( &xLastWakeTime, (T_step - tau_step_loc) / 2 );
                vTaskDelayUntil( &xLastWakeTime, T_step);

            }
            else  {
                uTCPsw = MSG_Q_BELLOW;
                xStatus_TaskGPIOsw = xQueueSend(qTCPt, &uTCPsw, portMAX_DELAY);
                // исключение данной команды

                // ƒќЋ∆Ќј Ѕџ“№ ќЅ–јЅќ“ ј

            }     // if (T_speed > tau_speed)
        }   // if calc
        else    {
            // —нимаетс¤ enable - аварийный останов
            GPIOE->BSRRH = GPIO_Pin_0;     // En
            uTCPsw = MSG_ERR_CALC;
            xStatus_TaskGPIOsw = xQueueSend(qTCPt, &uTCPsw, portMAX_DELAY);
        }


    } // for (;;)

}
Exemple #25
0
void taskFlightPlan3(void *param)
{
#if (SCH_FLIGHTPLAN3_VERBOSE)
        printf(">>[FlightPlan3] Started\r\n");
#endif

#if SCH_FLIGHTPLAN3_REALTIME
    unsigned int min_check_period_ms = 10000;      /* check every x ms  */
    portTickType xDelay_ticks = (min_check_period_ms) / portTICK_RATE_MS;
    portTickType check_deployment_time = (10000) / portTICK_RATE_MS;      /* check every 10sec  */
#else
    unsigned int min_check_period_ms = 1000;      /* check every 2sec  */
    portTickType xDelay_ticks = (min_check_period_ms) / portTICK_RATE_MS;
    portTickType check_deployment_time = (10000) / portTICK_RATE_MS;      /* check every 10sec  */
#endif

    DispCmd NewCmd;
    NewCmd.idOrig = CMD_IDORIG_TFLIGHTPLAN3;
    NewCmd.cmdId = CMD_CMDNULL;
    NewCmd.param = 0;

    /*Avoid the acummulation of commands while the SUCHAI is still deploying.. */
    portTickType xLastWakeTime = xTaskGetTickCount();
    #if (SCH_USE_HOUSEKEEPING == 1)
        while( TRUE ){
            if( sta_get_stateVar(sta_dep_ant_deployed)==1 ){
                break;
            }
            vTaskDelayUntil(&xLastWakeTime, check_deployment_time);
        }
    #endif

    while(1)
    {
        /* min_check_period_ms actions */
        vTaskDelayUntil(&xLastWakeTime, xDelay_ticks);
        
        /* Check if the next tick to wake has already
         * expired (*pxPreviousWakeTime = xTimeToWake;)
         * This avoids multiple reentries on vTaskDelayUntil */
        BOOL xShouldDelay = shouldDelayTask(&xLastWakeTime, xDelay_ticks);
        if( xShouldDelay == FALSE )
        {
             xLastWakeTime = xTaskGetTickCount();
            #if (SCH_FLIGHTPLAN3_VERBOSE>=1)
                printf("[FlightPlan3] xLastWakeTime + xDelay_ticks < xTickCount, "
                        "update xLastWakeTime to xTickCount ..\r\n");
            #endif
        }

        //Add commands below ..

        #if (SCH_FLIGHTPLAN3_VERBOSE>=1)
            printf("[FlightPlan3] min_check_period_ms (%d) actions ..\r\n", min_check_period_ms);
        #endif

        //execute regular/cyclic payloads ..
        if(sta_get_stateVar(sta_ppc_opMode)==STA_PPC_OPMODE_NORMAL){
            NewCmd.cmdId = pay_id_fp2_default_fsm;
            NewCmd.param = 0;
            xQueueSend(dispatcherQueue, (const void *) &NewCmd, portMAX_DELAY);
        }

        //execute programmed/itinerary actions ..
        NewCmd.cmdId = drp_id_fpl_check_and_exec;
        NewCmd.param = 0;
        xQueueSend(dispatcherQueue, (const void *) &NewCmd, portMAX_DELAY);

    }
}
Exemple #26
0
/* This function is usually called by the worker subsystem */
void logRunBlock(void * arg)
{
	struct log_block *blk = arg;
	struct log_ops *ops = blk->ops;
	static CRTPPacket pk;
	unsigned int timestamp;

	xSemaphoreTake(logLock, portMAX_DELAY);

	timestamp = ((long long) xTaskGetTickCount()) / portTICK_RATE_MS;

	pk.header = CRTP_HEADER(CRTP_PORT_LOG, LOG_CH);
	pk.size = 4;
	pk.data[0] = blk->id;
	pk.data[1] = timestamp & 0x0ff;
	pk.data[2] = (timestamp >> 8) & 0x0ff;
	pk.data[3] = (timestamp >> 16) & 0x0ff;

	while (ops)
	{
		int valuei = 0;
		float valuef = 0;

		switch (ops->storageType)
		{
			case LOG_UINT8:
				valuei = *(uint8_t *) ops->variable;
				break;
			case LOG_INT8:
				valuei = *(int8_t *) ops->variable;
				break;
			case LOG_UINT16:
				valuei = *(uint16_t *) ops->variable;
				break;
			case LOG_INT16:
				valuei = *(int16_t *) ops->variable;
				break;
			case LOG_UINT32:
				valuei = *(uint32_t *) ops->variable;
				break;
			case LOG_INT32:
				valuei = *(int32_t *) ops->variable;
				break;
			case LOG_FLOAT:
				valuei = *(float *) ops->variable;
				break;
		}

		if (ops->logType == LOG_FLOAT || ops->logType == LOG_FP16)
		{
			if (ops->storageType == LOG_FLOAT)
				valuef = *(float *) ops->variable;
			else
				valuef = valuei;

			if (ops->logType == LOG_FLOAT)
			{
				memcpy(&pk.data[pk.size], &valuef, 4);
				pk.size += 4;
			}
			else
			{
				valuei = single2half(valuef);
				memcpy(&pk.data[pk.size], &valuei, 2);
				pk.size += 2;
			}
		}
		else  //logType is an integer
		{
			memcpy(&pk.data[pk.size], &valuei, typeLength[ops->logType]);
			pk.size += typeLength[ops->logType];
		}

		ops = ops->next;
	}

	xSemaphoreGive(logLock);

	// Check if the connection is still up, oherwise disable
	// all the logging and flush all the CRTP queues.
	if (!crtpIsConnected())
	{
		logReset();
		crtpReset();
	}
	else
	{
		crtpSendPacket(&pk);
	}
}
time_t lwm2m_gettime(void)
{
    return xTaskGetTickCount()/configTICK_RATE_HZ;
}
Exemple #28
0
static void prvCheckTask( void *pvParameters )
{
TickType_t xNextWakeTime;
const TickType_t xCycleFrequency = 2500 / portTICK_PERIOD_MS;

	/* Just to remove compiler warning. */
	( void ) pvParameters;

	/* Initialise xNextWakeTime - this only needs to be done once. */
	xNextWakeTime = xTaskGetTickCount();

	for( ;; )
	{
		/* Place this task in the blocked state until it is time to run again. */
		vTaskDelayUntil( &xNextWakeTime, xCycleFrequency );

		/* Check the standard demo tasks are running without error. */
		#if( configUSE_PREEMPTION != 0 )
		{
			/* These tasks are only created when preemption is used. */
			if( xAreTimerDemoTasksStillRunning( xCycleFrequency ) != pdTRUE )
			{
				pcStatusMessage = "Error: TimerDemo";
			}
		}
		#endif

		if( xAreEventGroupTasksStillRunning() != pdTRUE )
		{
			pcStatusMessage = "Error: EventGroup";
		}
	    else if( xAreIntegerMathsTaskStillRunning() != pdTRUE )
	    {
			pcStatusMessage = "Error: IntMath";
	    }
		else if( xAreGenericQueueTasksStillRunning() != pdTRUE )
		{
			pcStatusMessage = "Error: GenQueue";
		}
		else if( xAreQueuePeekTasksStillRunning() != pdTRUE )
		{
			pcStatusMessage = "Error: QueuePeek";
		}
		else if( xAreBlockingQueuesStillRunning() != pdTRUE )
		{
			pcStatusMessage = "Error: BlockQueue";
		}
	    else if( xAreSemaphoreTasksStillRunning() != pdTRUE )
	    {
			pcStatusMessage = "Error: SemTest";
	    }
	    else if( xArePollingQueuesStillRunning() != pdTRUE )
	    {
			pcStatusMessage = "Error: PollQueue";
	    }
		else if( xAreMathsTaskStillRunning() != pdPASS )
		{
			pcStatusMessage = "Error: Flop";
		}
	    else if( xAreRecursiveMutexTasksStillRunning() != pdTRUE )
	    {
			pcStatusMessage = "Error: RecMutex";
		}
		else if( xAreCountingSemaphoreTasksStillRunning() != pdTRUE )
		{
			pcStatusMessage = "Error: CountSem";
		}
		else if( xIsCreateTaskStillRunning() != pdTRUE )
		{
			pcStatusMessage = "Error: Death";
		}
		else if( xAreDynamicPriorityTasksStillRunning() != pdPASS )
		{
			pcStatusMessage = "Error: Dynamic";
		}
		else if( xAreQueueSetTasksStillRunning() != pdPASS )
		{
			pcStatusMessage = "Error: Queue set";
		}
		else if( xIsQueueOverwriteTaskStillRunning() != pdPASS )
		{
			pcStatusMessage = "Error: Queue overwrite";
		}

		/* This is the only task that uses stdout so its ok to call printf()
		directly. */
		printf( ( char * ) "%s - %u\r\n", pcStatusMessage, ( unsigned int ) xTaskGetTickCount() );
		fflush( stdout );
	}
}
Exemple #29
0
clock_time_t clock_time( void )
{
	return xTaskGetTickCount();
}
static void vPrimaryBlockTimeTestTask( void *pvParameters )
{
BaseType_t xItem, xData;
TickType_t xTimeWhenBlocking;
TickType_t xTimeToBlock, xBlockedTime;

	#ifdef USE_STDIO
	void vPrintDisplayMessage( const char * const * ppcMessageToSend );
	
		const char * const pcTaskStartMsg = "Alt primary block time test started.\r\n";

		/* Queue a message for printing to say the task has started. */
		vPrintDisplayMessage( &pcTaskStartMsg );
	#endif

	( void ) pvParameters;

	for( ;; )
	{
		/*********************************************************************
        Test 1

        Simple block time wakeup test on queue receives. */
		for( xItem = 0; xItem < bktQUEUE_LENGTH; xItem++ )
		{
			/* The queue is empty. Attempt to read from the queue using a block
			time.  When we wake, ensure the delta in time is as expected. */
			xTimeToBlock = bktPRIMARY_BLOCK_TIME << xItem;

			/* A critical section is used to minimise the jitter in the time
			measurements. */
			portENTER_CRITICAL();
			{
				xTimeWhenBlocking = xTaskGetTickCount();
				
				/* We should unblock after xTimeToBlock having not received
				anything on the queue. */
				if( xQueueAltReceive( xTestQueue, &xData, xTimeToBlock ) != errQUEUE_EMPTY )
				{
					xErrorOccurred = pdTRUE;
				}

				/* How long were we blocked for? */
				xBlockedTime = xTaskGetTickCount() - xTimeWhenBlocking;
			}
			portEXIT_CRITICAL();

			if( xBlockedTime < xTimeToBlock )
			{
				/* Should not have blocked for less than we requested. */
				xErrorOccurred = pdTRUE;
			}

			if( xBlockedTime > ( xTimeToBlock + bktALLOWABLE_MARGIN ) )
			{
				/* Should not have blocked for longer than we requested,
				although we would not necessarily run as soon as we were
				unblocked so a margin is allowed. */
				xErrorOccurred = pdTRUE;
			}
		}


		#if configUSE_PREEMPTION == 0
			taskYIELD();
		#endif


		/*********************************************************************
        Test 2

        Simple block time wakeup test on queue sends.

		First fill the queue.  It should be empty so all sends should pass. */
		for( xItem = 0; xItem < bktQUEUE_LENGTH; xItem++ )
		{
			if( xQueueAltSendToBack( xTestQueue, &xItem, bktDONT_BLOCK ) != pdPASS )
			{
				xErrorOccurred = pdTRUE;
			}
		}

		for( xItem = 0; xItem < bktQUEUE_LENGTH; xItem++ )
		{
			/* The queue is full. Attempt to write to the queue using a block
			time.  When we wake, ensure the delta in time is as expected. */
			xTimeToBlock = bktPRIMARY_BLOCK_TIME << xItem;

			portENTER_CRITICAL();
			{
				xTimeWhenBlocking = xTaskGetTickCount();
				
				/* We should unblock after xTimeToBlock having not received
				anything on the queue. */
				if( xQueueAltSendToBack( xTestQueue, &xItem, xTimeToBlock ) != errQUEUE_FULL )
				{
					xErrorOccurred = pdTRUE;
				}

				/* How long were we blocked for? */
				xBlockedTime = xTaskGetTickCount() - xTimeWhenBlocking;
			}
			portEXIT_CRITICAL();

			if( xBlockedTime < xTimeToBlock )
			{
				/* Should not have blocked for less than we requested. */
				xErrorOccurred = pdTRUE;
			}

			if( xBlockedTime > ( xTimeToBlock + bktALLOWABLE_MARGIN ) )
			{
				/* Should not have blocked for longer than we requested,
				although we would not necessarily run as soon as we were
				unblocked so a margin is allowed. */
				xErrorOccurred = pdTRUE;
			}
		}

		#if configUSE_PREEMPTION == 0
			taskYIELD();
		#endif

		
		/*********************************************************************
        Test 3

		Wake the other task, it will block attempting to post to the queue.
		When we read from the queue the other task will wake, but before it
		can run we will post to the queue again.  When the other task runs it
		will find the queue still full, even though it was woken.  It should
		recognise that its block time has not expired and return to block for
		the remains of its block time.

		Wake the other task so it blocks attempting to post to the already
		full queue. */
		xRunIndicator = 0;
		vTaskResume( xSecondary );

		/* We need to wait a little to ensure the other task executes. */
		while( xRunIndicator != bktRUN_INDICATOR )
		{
			/* The other task has not yet executed. */
			vTaskDelay( bktSHORT_WAIT );
		}
		/* Make sure the other task is blocked on the queue. */
		vTaskDelay( bktSHORT_WAIT );
		xRunIndicator = 0;

		for( xItem = 0; xItem < bktQUEUE_LENGTH; xItem++ )
		{
			/* Now when we make space on the queue the other task should wake
			but not execute as this task has higher priority. */				
			if( xQueueAltReceive( xTestQueue, &xData, bktDONT_BLOCK ) != pdPASS )
			{
				xErrorOccurred = pdTRUE;
			}

			/* Now fill the queue again before the other task gets a chance to
			execute.  If the other task had executed we would find the queue
			full ourselves, and the other task have set xRunIndicator. */
			if( xQueueAltSendToBack( xTestQueue, &xItem, bktDONT_BLOCK ) != pdPASS )
			{
				xErrorOccurred = pdTRUE;
			}

			if( xRunIndicator == bktRUN_INDICATOR )
			{
				/* The other task should not have executed. */
				xErrorOccurred = pdTRUE;
			}

			/* Raise the priority of the other task so it executes and blocks
			on the queue again. */
			vTaskPrioritySet( xSecondary, bktPRIMARY_PRIORITY + 2 );

			/* The other task should now have re-blocked without exiting the
			queue function. */
			if( xRunIndicator == bktRUN_INDICATOR )
			{
				/* The other task should not have executed outside of the
				queue function. */
				xErrorOccurred = pdTRUE;
			}

			/* Set the priority back down. */
			vTaskPrioritySet( xSecondary, bktSECONDARY_PRIORITY );			
		}

		/* Let the other task timeout.  When it unblockes it will check that it
		unblocked at the correct time, then suspend itself. */
		while( xRunIndicator != bktRUN_INDICATOR )
		{
			vTaskDelay( bktSHORT_WAIT );
		}
		vTaskDelay( bktSHORT_WAIT );
		xRunIndicator = 0;

		#if configUSE_PREEMPTION == 0
			taskYIELD();
		#endif

		/*********************************************************************
        Test 4

		As per test 3 - but with the send and receive the other way around.
		The other task blocks attempting to read from the queue.

		Empty the queue.  We should find that it is full. */
		for( xItem = 0; xItem < bktQUEUE_LENGTH; xItem++ )
		{
			if( xQueueAltReceive( xTestQueue, &xData, bktDONT_BLOCK ) != pdPASS )
			{
				xErrorOccurred = pdTRUE;
			}
		}
		
		/* Wake the other task so it blocks attempting to read from  the
		already	empty queue. */
		vTaskResume( xSecondary );

		/* We need to wait a little to ensure the other task executes. */
		while( xRunIndicator != bktRUN_INDICATOR )
		{
			vTaskDelay( bktSHORT_WAIT );
		}
		vTaskDelay( bktSHORT_WAIT );
		xRunIndicator = 0;

		for( xItem = 0; xItem < bktQUEUE_LENGTH; xItem++ )
		{
			/* Now when we place an item on the queue the other task should
			wake but not execute as this task has higher priority. */				
			if( xQueueAltSendToBack( xTestQueue, &xItem, bktDONT_BLOCK ) != pdPASS )
			{
				xErrorOccurred = pdTRUE;
			}

			/* Now empty the queue again before the other task gets a chance to
			execute.  If the other task had executed we would find the queue
			empty ourselves, and the other task would be suspended. */
			if( xQueueAltReceive( xTestQueue, &xData, bktDONT_BLOCK ) != pdPASS )
			{
				xErrorOccurred = pdTRUE;
			}

			if( xRunIndicator == bktRUN_INDICATOR )
			{
				/* The other task should not have executed. */
				xErrorOccurred = pdTRUE;
			}

			/* Raise the priority of the other task so it executes and blocks
			on the queue again. */
			vTaskPrioritySet( xSecondary, bktPRIMARY_PRIORITY + 2 );

			/* The other task should now have re-blocked without exiting the
			queue function. */
			if( xRunIndicator == bktRUN_INDICATOR )
			{
				/* The other task should not have executed outside of the
				queue function. */
				xErrorOccurred = pdTRUE;
			}
			vTaskPrioritySet( xSecondary, bktSECONDARY_PRIORITY );			
		}

		/* Let the other task timeout.  When it unblockes it will check that it
		unblocked at the correct time, then suspend itself. */
		while( xRunIndicator != bktRUN_INDICATOR )
		{
			vTaskDelay( bktSHORT_WAIT );
		}
		vTaskDelay( bktSHORT_WAIT );

		xPrimaryCycles++;
	}
}