uint8_t SIMCOM::getGeo(geoSmsData *retSms, char *pwd)
{
static uint8_t l = 1;
retSms->smsDataValid = false;
retSms->smsCmdNum = 0xFF;
retSms->smsPending = checkForMessages();
if(!retSms->smsPending)
{
l = 1;
return 0;//no messages
}
if(retSms->smsPending == 0xFF)
{
retSms->smsPending = 0x00;
return 1; //there was an error
}
while(retSms->smsPending && (l <= SIMSIZE))
{
if(readMessageBreakOut(l) == 0xFF)
{
l++;
continue;
}
confirmAtCommand("\"",100);
atRxBuffer[strlen(atRxBuffer) - 1] = '\0';
strcpy(retSms->smsNumber,atRxBuffer);
confirmAtCommand("\r\n",100);
/* if(!strstr(retSms->smsNumber,"+") != NULL)
{
// confirmAtCommand("/",500);
confirmAtCommand("/",500);
}*/
confirmAtCommand(DELIMITER,100);
atRxBuffer[strlen(atRxBuffer) - 1] = '\0';
if(strncmp((atRxBuffer + (strlen(atRxBuffer)-4)),pwd,4) != 0)
retSms->smsNumber[0] = '\0';
else
{
retSms->smsDataValid = true;
confirmAtCommand(DELIMITER,100);
atRxBuffer[strlen(atRxBuffer) - 1] = '\0';
retSms->smsCmdNum = atoi(atRxBuffer);
confirmAtCommand("\r\n",100);
strcpy(retSms->smsCmdString,atRxBuffer);
}
deleteMessage(l);
retSms->smsPending--;
l++;
if(!retSms->smsPending)
l = 1;
return 0; //message read
}
deleteAllMessages(); // too many messages on SIM card
return 2;
}
//--------------------------------------------------------------
void DonutCop::update(int size) {
if (currentSecond() != lastSecond) {
sendStatusMessage(size);
if (id == 0) {
sendControlMessage();
removeExpiredIds();
}
lastSecond = currentSecond();
createdSprinkles = 0;
}
checkForMessages();
}
void PollingBuffer::popMessages() {
while (checkForMessages()) {
uint32_t msgSize;
msgSize = *(reinterpret_cast<uint32_t *>(m_inputData.data()));
std::string msg(m_inputData.begin() + sizeof(uint32_t),
m_inputData.begin() + sizeof(uint32_t) + msgSize);
m_inputData.erase(m_inputData.begin(),
m_inputData.begin() + (msgSize + sizeof(uint32_t)));
m_inbox.push_back(msg);
}
}
void LJBloggingPlatform::handleAccountValidated (bool validated)
{
IAccount *acc = qobject_cast<IAccount*> (sender ());
if (!acc)
{
qWarning () << Q_FUNC_INFO
<< sender ()
<< "is not an IAccount";;
return;
}
emit accountValidated (acc->GetQObject (), validated);
if (validated &&
XmlSettingsManager::Instance ().Property ("CheckingInboxEnabled", true).toBool ())
checkForMessages ();;
}
/*!*****************************************************************************
*******************************************************************************
\note run_ros_servo
\date Feb 1999
\remarks
This program executes the sequence of ros servo routines
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
run_ros_servo(void)
{
int j,i;
double dt;
int dticks;
setOsc(d2a_cr,0.0);
/*********************************************************************
* adjust time
*/
++ros_servo_calls;
ros_servo_time += 1./(double)ros_servo_rate;
servo_time = ros_servo_time;
// check for unexpected time drift
dticks = round((ros_servo_time - last_ros_servo_time)*(double)ros_servo_rate);
if (dticks != 1 && ros_servo_calls > 2) // need transient ticks to sync servos
ros_servo_errors += abs(dticks-1);
/*********************************************************************
* check for messages
*/
checkForMessages();
/*********************************************************************
* receive sensory data
*/
if (!receive_ros_state()) {
printf("Problem when receiving ros state\n");
return FALSE;
}
setOsc(d2a_cr,10.0);
/*********************************************************************
* compute useful kinematic variables
*/
compute_kinematics();
setOsc(d2a_cr,20.0);
#ifdef __XENO__
// we want to be in secondary mode here
//rt_task_set_mode(T_PRIMARY,0,NULL);
#endif
/**********************************************************************
* do ROS communication
*/
if (default_publisher_enabled_)
ros_communicator_.publish();
/**********************************************************************
* do user specific ROS functions
*/
run_user_ros();
ros::spinOnce();
#ifdef __XENO__
// we want to be in real-time mode here
#if (CONFIG_XENO_VERSION_MAJOR < 2) || (CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR < 6)
// we are on xenomai version < 2.6
rt_task_set_mode(0,T_PRIMARY,NULL);
#else
// we are on xenomai version < 2.6
rt_task_set_mode(0,T_CONFORMING,NULL);
#endif
#endif
setOsc(d2a_cr,90.0);
/*************************************************************************
* collect data
*/
writeToBuffer();
sendOscilloscopeData();
setOsc(d2a_cr,100.0);
/*************************************************************************
* end of program sequence
*/
last_ros_servo_time = ros_servo_time;
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note run_simulation_servo
\date Nov 2007
\remarks
main functions executed during running the simulation servo
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
run_simulation_servo(void)
{
int i;
double k,kd;
double delta;
double dt;
int dtick;
double max_vel = 10.;
double aux;
static double last_time = 0;
static double current_time = 0;
// advance the simulation servo
++simulation_servo_calls;
simulation_servo_time += 1./(double)simulation_servo_rate;
servo_time = simulation_servo_time;
// check for missed calls to the servo
dtick = round((simulation_servo_time - last_simulation_servo_time)*(double)simulation_servo_rate);
if (dtick != 1 && simulation_servo_calls > 2) // need transient ticks to sync servos
simulation_servo_errors += abs(dtick-1);
// first, send out all the current state variables
// to shared memory
send_sim_state();
send_misc_sensors();
send_contacts();
// zero any external forces
bzero((void *)uext_sim,sizeof(SL_uext)*(n_dofs+1));
// read the commands
receive_des_commands();
// check for messages
checkForMessages();
// only after the write/read steps above, trigger the motor servo to avoid that
// the motor servo can read data that has the wrong time stamp
semGive(sm_motor_servo_sem);
// real-time processing if needed
#ifdef __XENO__
RTIME t = rt_timer_read();
current_time = (double)t/1.e9;
if (real_time) {
double delta_time;
delta_time = (1./(double)simulation_servo_rate-(current_time - last_time));
if (delta_time < 0)
delta_time = 0;
else
taskDelay(ns2ticks((long)(delta_time*1.e9)));
RTIME t = rt_timer_read();
current_time = (double)t/1.e9;
}
#else
struct timeval t;
gettimeofday(&t,NULL);
current_time = (double) t.tv_sec + ((double)t.tv_usec)/1.e6;
if (real_time) {
while (current_time - last_time < 1./(double)simulation_servo_rate) {
taskDelay(ns2ticks((long)(1./((double)simulation_servo_rate)*1000000000./5.)));
gettimeofday(&t,NULL);
current_time = (double) t.tv_sec + ((double)t.tv_usec)/1.e6;
}
}
#endif
last_time = current_time;
// check limits
for (i=1; i<=n_dofs; ++i) {
// use Geyer limited rebound model for (set aux=1 to avoid)
k = controller_gain_th[i]*10.0;
kd = controller_gain_thd[i]*sqrt(10.0);
delta = joint_sim_state[i].th - joint_range[i][MIN_THETA];
if ( delta < 0 ){ // only print at 10Hz
if ((int)(((double)simulation_servo_calls)/
(((double)simulation_servo_rate)/10.))%10 == 0) {
printf("-%s",joint_names[i]);
fflush(stdout);
}
if (joint_sim_state[i].thd > max_vel)
aux = 0.0;
else
aux = 1.-joint_sim_state[i].thd/max_vel;
joint_sim_state[i].u += - delta * k * aux - joint_sim_state[i].thd * kd * aux;
}
delta = joint_range[i][MAX_THETA] - joint_sim_state[i].th;
if (delta < 0){ // only print at 10Hz
if ((int)(((double)simulation_servo_calls)/
(((double)simulation_servo_rate)/10.))%10 == 0) {
printf("+%s",joint_names[i]);
fflush(stdout);
}
if (joint_sim_state[i].thd < -max_vel)
aux = 0.0;
else
aux = 1.-joint_sim_state[i].thd/(-max_vel);
joint_sim_state[i].u += delta * k * aux - joint_sim_state[i].thd * kd * aux;
}
}
// general numerical integration: integration runs at higher rate
dt = 1./(double)(simulation_servo_rate)/(double)n_integration;
for (i=1; i<=n_integration; ++i) {
// integrate the simulation
switch (integrate_method) {
case INTEGRATE_RK:
SL_IntegrateRK(joint_sim_state, &base_state,
&base_orient, ucontact, endeff,dt,n_dofs);
break;
case INTEGRATE_EULER:
SL_IntegrateEuler(joint_sim_state, &base_state,
&base_orient, ucontact, endeff,dt,n_dofs,TRUE);
break;
default:
printf("invalid integration method\n");
}
}
// compute miscellenous sensors
run_user_simulation();
// run user specific simulations
runUserSimulation();
// data collection
writeToBuffer();
last_simulation_servo_time = simulation_servo_time;
return TRUE;
}
