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++;
}
}
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();
}
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;
}
}
}
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
}
}
uint32_t Task::getSysticks()
{
return xTaskGetTickCount();
}
/**
* 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;
}
}
}
}
UINT32 StatsTimerGet()
{
return xTaskGetTickCount();
}
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();
}
}
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 );
}
}
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();
}
}
/**
* 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);
}
}
}
//
//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;
}
/* Return current time */
uint32_t time_now(void) {
return (uint32_t)(xTaskGetTickCount() * (1000/configTICK_RATE_HZ));
}
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;
}
}
}
static umtick umac_impl_now_tick(void) {
return xTaskGetTickCount();
}
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 );
}
}
}
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 );
}
}
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();
}
}
}
}
uint32_t Task::getTime()
{
return xTaskGetTickCount()/4;
}
/*-----------------------------------------------------------------------------------*/
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 (;;)
}
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);
}
}
/* 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;
}
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 );
}
}
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++;
}
}