void Client::updateState(xcb_ewmh_connection_t* ewmhConn)
{
xcb_connection_t* conn = ewmhConn->connection;
xcb_get_geometry_cookie_t geomCookie;
if (!mOwned)
geomCookie = xcb_get_geometry_unchecked(conn, mWindow);
const xcb_get_property_cookie_t normalHintsCookie = xcb_icccm_get_wm_normal_hints(conn, mWindow);
const xcb_get_property_cookie_t leaderCookie = xcb_get_property(conn, 0, mWindow, Atoms::WM_CLIENT_LEADER, XCB_ATOM_WINDOW, 0, 1);
const xcb_get_property_cookie_t transientCookie = xcb_icccm_get_wm_transient_for(conn, mWindow);
const xcb_get_property_cookie_t hintsCookie = xcb_icccm_get_wm_hints(conn, mWindow);
const xcb_get_property_cookie_t classCookie = xcb_icccm_get_wm_class(conn, mWindow);
const xcb_get_property_cookie_t nameCookie = xcb_icccm_get_wm_name(conn, mWindow);
const xcb_get_property_cookie_t protocolsCookie = xcb_icccm_get_wm_protocols(conn, mWindow, Atoms::WM_PROTOCOLS);
const xcb_get_property_cookie_t strutCookie = xcb_ewmh_get_wm_strut(ewmhConn, mWindow);
const xcb_get_property_cookie_t partialStrutCookie = xcb_ewmh_get_wm_strut_partial(ewmhConn, mWindow);
const xcb_get_property_cookie_t stateCookie = xcb_ewmh_get_wm_state(ewmhConn, mWindow);
const xcb_get_property_cookie_t typeCookie = xcb_ewmh_get_wm_window_type(ewmhConn, mWindow);
const xcb_get_property_cookie_t pidCookie = xcb_ewmh_get_wm_pid(ewmhConn, mWindow);
if (!mOwned)
updateSize(conn, geomCookie);
updateNormalHints(conn, normalHintsCookie);
updateLeader(conn, leaderCookie);
updateTransient(conn, transientCookie);
updateHints(conn, hintsCookie);
updateClass(conn, classCookie);
updateName(conn, nameCookie);
updateProtocols(conn, protocolsCookie);
updateStrut(ewmhConn, strutCookie);
updatePartialStrut(ewmhConn, partialStrutCookie);
updateEwmhState(ewmhConn, stateCookie);
updateWindowTypes(ewmhConn, typeCookie);
updatePid(ewmhConn, pidCookie);
}
task ctls() {
float dx,
dy;
int time,
timeLast,
dt;
while(1) {
timeLast = time;
time = nSysTime;
dt = time - timeLast;
if(time - timeLast > 0) {
updatePid(&drivePid, driveVelAvg.mean, dt);
updatePid(&gyroPid, theta, dt);
}
wait1Msec(20);
}
}
void
CpuElapsedTime::add(pid_t pid)
{
sensor_t cval, pval;
_cvPIDMap.insert(std::pair<pid_t, sensor_t>(pid, cval));
_pvPIDMap.insert(std::pair<pid_t, sensor_t>(pid, pval));
updatePid(pid);
_pvPIDMap[pid].U64 = _cvPIDMap[pid].U64;
}
void vTaskUpdatePid(void *pvParameters)
{
// Initialize delay wake time
portTickType wakeTime;
portTickType frequency = 1000;
wakeTime = xTaskGetTickCount();
// Execute & delay
while(1){
updatePid(p_motorSpeedsPtr);
vTaskDelayUntil(&wakeTime, frequency);
}
}
/*
* TIM2_IRQHandler - The interrupt handler for timer 2. There are 4 possible
* interrupt channels.
*
* Inputs: None
* Outputs: None
* Modifies: Possibly extern global var pid_set_flag
*/
void TIM2_IRQHandler(void) {
// Channel 1 is the interrupt for the detectEmegency() function
if (TIM_GetITStatus(TIM2, TIM_IT_CC1) != RESET) {
// Clear the IRQ
TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
detectEmergency();
// Interrupt needs to trigger again in 10ms
capture = TIM_GetCapture1(TIM2);
TIM_SetCompare1(TIM2, capture + DELAY_10MS);
// Channel 2 is the interrupt for refreshSensorData()
} else if (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET) {
// Clear the IRQ
TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
refreshSensorData();
// Interrupt needs to trigger again in 100ms
capture = TIM_GetCapture2(TIM2);
TIM_SetCompare2(TIM2, capture + DELAY_100MS);
// Channel 3 is the interupt for calculateOrientation()
} else if (TIM_GetITStatus(TIM2, TIM_IT_CC3) != RESET) {
// Clear the IRQ
TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
calculateOrientation();
// This function refreshes once a second, so no need to reset the
// compare register
// Channel 4 is the interrupt for updatePid()
} else {
// Clear the IRQ
TIM_ClearITPendingBit(TIM2, TIM_IT_CC4);
// This function returns new motor PIDs, which means the main
// program needs to do additional processing after the interrupt
updatePid(&motor_speeds);
pid_set_flag = True;
// This function refreshes once a second, so no need to reset the
// compare register
}
}
void Client::propertyNotify(xcb_atom_t atom)
{
#warning Need to notify js that properties have changed
warning() << "Got propertyNotify" << Atoms::name(atom) << mWindow;
auto ewmhConnection = WindowManager::instance()->ewmhConnection();
auto conn = ewmhConnection->connection;
if (atom == XCB_ATOM_WM_NORMAL_HINTS) {
const xcb_get_property_cookie_t normalHintsCookie = xcb_icccm_get_wm_normal_hints(conn, mWindow);
updateNormalHints(conn, normalHintsCookie);
} else if (atom == XCB_ATOM_WM_TRANSIENT_FOR) {
const xcb_get_property_cookie_t transientCookie = xcb_icccm_get_wm_transient_for(conn, mWindow);
updateTransient(conn, transientCookie);
} else if (atom == Atoms::WM_CLIENT_LEADER) {
const xcb_get_property_cookie_t leaderCookie = xcb_get_property(conn, 0, mWindow, Atoms::WM_CLIENT_LEADER, XCB_ATOM_WINDOW, 0, 1);
updateLeader(conn, leaderCookie);
} else if (atom == XCB_ATOM_WM_HINTS) {
const xcb_get_property_cookie_t hintsCookie = xcb_icccm_get_wm_hints(conn, mWindow);
updateHints(conn, hintsCookie);
} else if (atom == XCB_ATOM_WM_CLASS) {
const xcb_get_property_cookie_t classCookie = xcb_icccm_get_wm_class(conn, mWindow);
updateClass(conn, classCookie);
} else if (atom == XCB_ATOM_WM_NAME) {
const xcb_get_property_cookie_t nameCookie = xcb_icccm_get_wm_name(conn, mWindow);
updateName(conn, nameCookie);
} else if (atom == Atoms::WM_PROTOCOLS) {
const xcb_get_property_cookie_t protocolsCookie = xcb_icccm_get_wm_protocols(conn, mWindow, Atoms::WM_PROTOCOLS);
updateProtocols(conn, protocolsCookie);
} else if (atom == ewmhConnection->_NET_WM_STRUT) {
const xcb_get_property_cookie_t strutCookie = xcb_ewmh_get_wm_strut(ewmhConnection, mWindow);
updateStrut(ewmhConnection, strutCookie);
} else if (atom == ewmhConnection->_NET_WM_STRUT_PARTIAL) {
const xcb_get_property_cookie_t partialStrutCookie = xcb_ewmh_get_wm_strut_partial(ewmhConnection, mWindow);
updatePartialStrut(ewmhConnection, partialStrutCookie);
} else if (atom == ewmhConnection->_NET_WM_STATE) {
const xcb_get_property_cookie_t stateCookie = xcb_ewmh_get_wm_state(ewmhConnection, mWindow);
updateEwmhState(ewmhConnection, stateCookie);
} else if (atom == ewmhConnection->_NET_WM_WINDOW_TYPE) {
const xcb_get_property_cookie_t typeCookie = xcb_ewmh_get_wm_window_type(ewmhConnection, mWindow);
updateWindowTypes(ewmhConnection, typeCookie);
} else if (atom == ewmhConnection->_NET_WM_PID) {
const xcb_get_property_cookie_t pidCookie = xcb_ewmh_get_wm_pid(ewmhConnection, mWindow);
updatePid(ewmhConnection, pidCookie);
} else {
warning() << "Unhandled propertyNotify atom" << Atoms::name(atom);
}
}
void UpdatePid(void * Parameters)
{
static float M1=0,M2=0,M3=0,M4=0;
static MotorSpeeds p_motorSpeeds;
while(1){
vTaskDelay(1000);
updatePid(&p_motorSpeeds);
if(p_motorSpeeds.m1) M1=1;
else M1=0;
if(p_motorSpeeds.m2) M2=1;
else M2=0;
if(p_motorSpeeds.m3) M3=1;
else M3=0;
if(p_motorSpeeds.m4) M4=1;
else M4=0;
SwitchMotor(M1,M2,M3,M4);
}
}
void Com::loop(int time) {
while(Serial.available()>0) {
char cmd = Serial.read();
switch(cmd) {
case 't':
temp = readLong();
sendOk();
Serial.print("temp = ");
Serial.println(temp);
break;
case 's': // read settings
sendSettings();
break;
case 'P': // write PID
updatePid();
break;
case 'G': // write Gain
updateFloat(&g_userControlGain);
break;
case 'W': // write Windup Guard
updateFloat(&g_pidWindupGuard);
break;
default:
sendError();
break;
}
}
{
Telem telem;
sendPacketStart(PACKET_TELEMETRY);
telem.Yaw = g_telemetryCom.Yaw;
telem.Pitch = g_telemetryCom.Pitch;
telem.Roll = g_telemetryCom.Roll;
telem.MotorL = g_motorLeft.get();
telem.MotorR = g_motorRight.get();
telem.MotorF = g_motorFront.get();
telem.MotorB = g_motorBack.get();
telem.BatteryLevel = g_batteryManager.getLastReading();
telem.UserElev = g_receiverCom.pos[RECEIVER_Throttle];
telem.UserPitch = g_receiverCom.pos[RECEIVER_Elevator];
telem.UserRoll = g_receiverCom.pos[RECEIVER_Aileron];
telem.UserYaw = g_receiverCom.pos[RECEIVER_Rudder];
telem.loopCount = g_loopCount;
Serial.write((byte*)&telem, sizeof(Telem));
}
if(time - _lastSlowTime > 1000) {
_lastSlowTime = time;
Serial.print("RECV: ");
Serial.print(g_receiverCom.hasSignal ? "HAS SIGNAL " : "NO SIGNAL ");
for(int i=0; i<SERVO_INPUT_CHANNELS; i++) {
if(i != 0) {
Serial.print(", ");
}
Serial.print(g_receiverCom.pos[i]);
}
Serial.print(" TELEM: ");
Serial.print(g_telemetryCom.Yaw);
Serial.print(", ");
Serial.print(g_telemetryCom.Pitch);
Serial.print(", ");
Serial.print(g_telemetryCom.Roll);
Serial.print(" MOTOR: ");
Serial.print(g_motorFront.get());
Serial.print(", ");
Serial.print(g_motorBack.get());
Serial.print(", ");
Serial.print(g_motorLeft.get());
Serial.print(", ");
Serial.print(g_motorRight.get());
Serial.print(" PID: ");
Serial.print(g_rollPid.getLastPosition());
Serial.print(", ");
Serial.print(g_pitchPid.getLastPosition());
Serial.print(", ");
Serial.print(g_yawPid.getLastPosition());
Serial.print(" BAT: ");
Serial.print(g_batteryManager.getLastReading());
Serial.println(g_batteryManager.getIsWarning() ? " warning" : " ");
}
}
void TIM3_IRQHandler(void) {
if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)
{
if(s.inputMethod==inputMethod_stepDir)
{
updatePid();
TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
}
else
{
updateCtr++;
if(updateCtr>50)
{
//pwm_motorStop();
//ERROR_LED_ON;
}
}
}
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
uint16_t tim3_dc = TIM3->CCR2;
uint16_t tim3_per = TIM3->CCR1;
static uint8_t prevdir;
uint16_t DC;
#if DIR_POLARITY == 1
dir = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7);
#else
dir = (~(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7)))&1;
#endif
if(dir!=prevdir)
{
pwm_InitialBLDCCommutation();
}
prevdir=dir;
if(tim3_per>0 && tim3_per>tim3_dc)
{
DC = ((uint32_t)(BLDC_CHOPPER_PERIOD*tim3_dc)/(uint32_t)tim3_per);
}
else
DC=0;
if(DC<BLDC_NOL*3) DC=BLDC_NOL*3;
pwm_setDutyCycle(DC);
updateCtr=0;
/* if(motor_running==0)
{
motor_running=1;
//BLDCMotorPrepareCommutation();
}*/
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);
}
else if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)
{
//if PWM signal is lost for some reason, stop motor
TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
}
}
