/**
* Initialize the Ultrasonic Sensor.
* This is the common code that initializes the ultrasonic sensor given that there
* are two digital I/O channels allocated. If the system was running in automatic mode (round robin)
* when the new sensor is added, it is stopped, the sensor is added, then automatic mode is
* restored.
*/
void Ultrasonic::Initialize()
{
m_table = NULL;
bool originalMode = m_automaticEnabled;
if (m_semaphore == 0) m_semaphore = initializeSemaphore(SEMAPHORE_FULL);
SetAutomaticMode(false); // kill task when adding a new sensor
takeSemaphore(m_semaphore); // link this instance on the list
{
m_nextSensor = m_firstSensor;
m_firstSensor = this;
}
giveSemaphore(m_semaphore);
m_counter = new Counter(m_echoChannel); // set up counter for this sensor
m_counter->SetMaxPeriod(1.0);
m_counter->SetSemiPeriodMode(true);
m_counter->Reset();
m_enabled = true; // make it available for round robin scheduling
SetAutomaticMode(originalMode);
static int instances = 0;
instances++;
HALReport(HALUsageReporting::kResourceType_Ultrasonic, instances);
LiveWindow::GetInstance()->AddSensor("Ultrasonic", m_echoChannel->GetChannel(), this);
}
/**
* DriverStation constructor.
*
* This is only called once the first time GetInstance() is called
*/
DriverStation::DriverStation()
: m_task ("DriverStation", (FUNCPTR)DriverStation::InitTask)
, m_newControlData(0)
, m_packetDataAvailableMultiWait(0)
, m_waitForDataSem(0)
, m_userInDisabled(false)
, m_userInAutonomous(false)
, m_userInTeleop(false)
, m_userInTest(false)
, m_nextMessageTime(0)
{
// All joysticks should default to having zero axes, povs and buttons, so
// uninitialized memory doesn't get sent to speed controllers.
for(unsigned int i = 0; i < kJoystickPorts; i++) {
m_joystickAxes[i].count = 0;
m_joystickPOVs[i].count = 0;
m_joystickButtons[i].count = 0;
m_joystickDescriptor[i].isXbox = 0;
m_joystickDescriptor[i].type = -1;
m_joystickDescriptor[i].name[0] = '\0';
}
// Create a new semaphore
m_packetDataAvailableMultiWait = initializeMultiWait();
m_newControlData = initializeSemaphore(SEMAPHORE_EMPTY);
m_waitForDataSem = initializeMultiWait();
m_waitForDataMutex = initializeMutexNormal();
m_packetDataAvailableMultiWait = initializeMultiWait();
m_packetDataAvailableMutex = initializeMutexNormal();
// Register that semaphore with the network communications task.
// It will signal when new packet data is available.
HALSetNewDataSem(m_packetDataAvailableMultiWait);
AddToSingletonList();
// They need to be identical or it could lead to runtime stack corruption if
// the caller and callee push and pop different amounts of data on the stack.
static_assert(sizeof(this) == sizeof(uint32_t),
"We are passing a pointer through a uint32_t");
if (!m_task.Start((uint32_t)this))
{
wpi_setWPIError(DriverStationTaskError);
}
}
/**
* DriverStation constructor.
*
* This is only called once the first time GetInstance() is called
*/
DriverStation::DriverStation()
: m_task ("DriverStation", (FUNCPTR)DriverStation::InitTask)
, m_newControlData(0)
, m_packetDataAvailableMultiWait(0)
, m_waitForDataSem(0)
, m_userInDisabled(false)
, m_userInAutonomous(false)
, m_userInTeleop(false)
, m_userInTest(false)
, m_nextMessageTime(0)
{
// All joysticks should default to having zero axes, povs and buttons, so
// uninitialized memory doesn't get sent to speed controllers.
for(unsigned int i = 0; i < kJoystickPorts; i++) {
m_joystickAxes[i].count = 0;
m_joystickPOVs[i].count = 0;
m_joystickButtons[i].count = 0;
}
// Create a new semaphore
m_packetDataAvailableMultiWait = initializeMultiWait();
m_newControlData = initializeSemaphore(SEMAPHORE_EMPTY);
m_waitForDataSem = initializeMultiWait();
m_waitForDataMutex = initializeMutexNormal();
m_packetDataAvailableMultiWait = initializeMultiWait();
m_packetDataAvailableMutex = initializeMutexNormal();
// Register that semaphore with the network communications task.
// It will signal when new packet data is available.
HALSetNewDataSem(m_packetDataAvailableMultiWait);
AddToSingletonList();
if (!m_task.Start((int32_t)this))
{
wpi_setWPIError(DriverStationTaskError);
}
}
/**
* Create a Notifier for timer event notification.
* @param handler The handler is called at the notification time which is set
* using StartSingle or StartPeriodic.
*/
Notifier::Notifier(TimerEventHandler handler, void *param)
{
if (handler == NULL)
wpi_setWPIErrorWithContext(NullParameter, "handler must not be NULL");
m_handler = handler;
m_param = param;
m_periodic = false;
m_expirationTime = 0;
m_period = 0;
m_nextEvent = NULL;
m_queued = false;
m_handlerSemaphore = initializeSemaphore(SEMAPHORE_FULL);
{
Synchronized sync(queueSemaphore);
// do the first time intialization of static variables
if (refcount == 0)
{
task = new Task("NotifierTask", (FUNCPTR)Notifier::Run, Task::kDefaultPriority, 64000);
task->Start();
}
refcount++;
}
}
/**
* Create a Notifier for timer event notification.
* @param handler The handler is called at the notification time which is set
* using StartSingle or StartPeriodic.
*/
Notifier::Notifier(TimerEventHandler handler, void *param)
{
if (handler == NULL)
wpi_setWPIErrorWithContext(NullParameter, "handler must not be NULL");
m_handler = handler;
m_param = param;
m_periodic = false;
m_expirationTime = 0;
m_period = 0;
m_nextEvent = NULL;
m_queued = false;
m_handlerSemaphore = initializeSemaphore(SEMAPHORE_FULL);
{
::std::unique_lock<ReentrantMutex> sync(queueSemaphore);
// do the first time intialization of static variables
if (refcount == 0)
{
int32_t status = 0;
m_notifier = initializeNotifier(ProcessQueue, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
refcount++;
}
}
