/*----------------------------------------------------------------------*
| SendInclude |
*----------------------------------------------------------------------*/
void
SendIncludeCommand(Window window, int commID, Boolean is_in_line)
{
Time time;
XtPointer msg, save;
CARD32 commandID = 0L;
CARD32 in_line = 0L;
CARD32 selection = 0L;
CARD32 count = 1L;
CARD32 windowBit = 1<<30;
/* CARD32 iconBit = 1<<31;*/
char str[128];
int size=0;
commandID += commID;
selection += atoms.own_selection;
if (is_in_line)
{
in_line = 1L;
}
time = GetTimestamp(dsp);
/*
* Fixup the window id receiving the client command.
* Top 2 bits:
* 10 - Icon only
* 01 - Window only
* 11 - Both Icon and Window
* 00 - Reserved (Don't Use!)
*
* Bottom 30 bits:
* 0x3fffffff - All windows and/or icons
* 0x00000000 - Root window menu
*/
size += sizeof(CARD32); /* in-line*/
size += sizeof(CARD32); /* command id */
size += sizeof(CARD32); /* selection */
size += sizeof(CARD32); /* count */
size += sizeof(CARD32); /* window */
save = msg = (XtPointer) XtMalloc(size);
window |= windowBit;
msg = PackCARD32(msg, (CARD32)in_line);
msg = PackCARD32(msg, (CARD32)commandID);
msg = PackCARD32(msg, (CARD32)selection);
msg = PackCARD32(msg, (CARD32)count);
msg = PackCARD32(msg, (CARD32)window);
PRINT("IncludeCommand #%d on window %x (%u).\n", commandID, window, window);
SendMessage(INCLUDE, save, size, time);
}
BOOL CMeterUploader::Initialize()
{
TIMESTAMP tmNow;
GetTimestamp(&tmNow, NULL);
memset(m_szBusyFile, 0, sizeof(m_szBusyFile));
if (!CTimeoutThread::StartupThread(9))
return FALSE;
return TRUE;
}
void
LogDispatch(uint64_t aTaskId, uint64_t aParentTaskId, uint64_t aSourceEventId,
SourceEventType aSourceEventType, int aDelayTimeMs)
{
TraceInfo* info = GetOrCreateTraceInfo();
ENSURE_TRUE_VOID(info);
// aDelayTimeMs is the expected delay time in milliseconds, thus the dispatch
// time calculated of it might be slightly off in the real world.
uint64_t time = (aDelayTimeMs <= 0) ? GetTimestamp() :
GetTimestamp() + aDelayTimeMs;
// Log format:
// [0 taskId dispatchTime sourceEventId sourceEventType parentTaskId]
nsCString* log = info->AppendLog();
if (log) {
log->AppendPrintf("%d %lld %lld %lld %d %lld",
ACTION_DISPATCH, aTaskId, time, aSourceEventId,
aSourceEventType, aParentTaskId);
}
}
// Heart Beat requeset
APP_Error HeartBeat_Request(TLP_Head_Struct *pHeadInfo)
{
GSM_Error res;
uint32_t timestamp;
Packet_Struct Packet;
uint16_t crc_resault;
TLP_Head_Struct *PacketHead;
uint8_t *pData;
PacketHead = (TLP_Head_Struct *)&(Packet.Data[0]);
pData = (uint8_t *)&(Packet.Data[0]) + sizeof(TLP_Head_Struct);
// no need to change
PacketHead->version = _VERSION;
PacketHead->length = sizeof(TLP_Head_Struct);
PacketHead->type = _TransmitCtrl; // device to server
PacketHead->medium = _TransmitType; // UDP
PacketHead->encrypt = _Encrypt; // encrypt none
PacketHead->reserve = _Reserve;
PacketHead->DEV_IDH = HTONL(_DEV_IDH);
PacketHead->DEV_IDL = HTONL(_DEV_IDL);
//need to change
GetTimestamp(×tamp);
PacketHead->timestamp = HTONL(timestamp); // need to fix
PacketHead->SEQ_num = HTONL(Get_SEQ());
// send the ack
*(pData++) = HEART_BEAT; // command type
*(pData++) = (uint8_t)(_HeartBeat>>8);
*(pData++) = (uint8_t)(_HeartBeat); // action
Packet.length = pData - ((uint8_t *)&(Packet.Data[0]));
// get the crc16
pData = (uint8_t *)&(Packet.Data[0]) + 2;
crc_resault = Caculate(pData, (Packet.length-2));
PacketHead->crc16 = HTONS(crc_resault);
// send packet
res = UDP_SendPacket((char *)&(Packet.Data[0]), Packet.length);
if(res!=ERR_NONE){
DEBUG("%s : ACK ...Error!\n",__func__);
return APP_ERR_SEND;
} else {
DEBUG("%s : ACK ...OK\n",__func__);
return APP_ERR_NONE;
}
}
void
LogEnd(uint64_t aTaskId, uint64_t aSourceEventId)
{
TraceInfo* info = GetOrCreateTraceInfo();
ENSURE_TRUE_VOID(info);
// Log format:
// [2 taskId endTime]
nsCString* log = info->AppendLog();
if (log) {
log->AppendPrintf("%d %lld %lld", ACTION_END, aTaskId, GetTimestamp());
}
}
std::string Logger::GetFormattedTimestamp()
{
time_t timestamp = GetTimestamp();
struct tm* timeinfo;
char buffer[80];
time( ×tamp );
timeinfo = localtime ( ×tamp );
// http://www.cplusplus.com/reference/ctime/strftime/
strftime( buffer, 80, "%H:%M:%S", timeinfo );
std::string str( buffer );
return str;
}
bool FlotillaDock::set_port(sp_port *new_port){
if (state != Disconnected) return false;
state = Connecting;
if (sp_copy_port(new_port, &port) == SP_OK){
const char* port_name = sp_get_port_name(port);
sp_set_baudrate(port, BAUD_RATE);
if (sp_open(port, SP_MODE_READ_WRITE) == SP_OK){
if (get_version_info()){
state = Connected;
return true;
}
else
{
std::ostringstream msg;
msg << GetTimestamp() << "Warning: Failed to get dock version information" << std::endl;
std::cout << msg.str();
}
}
else
{
std::ostringstream msg;
msg << GetTimestamp() << "Warning: Failed to open port " << port_name << std::endl;
std::cout << msg.str();
}
}
else{
std::ostringstream msg;
msg << GetTimestamp() << "Warning: Failed to copy port!?" << std::endl;
std::cout << msg.str();
}
state = Disconnected;
return false;
}
void
LogBegin(uint64_t aTaskId, uint64_t aSourceEventId)
{
TraceInfo* info = GetOrCreateTraceInfo();
ENSURE_TRUE_VOID(info);
// Log format:
// [1 taskId beginTime processId threadId]
nsCString* log = info->AppendLog();
if (log) {
log->AppendPrintf("%d %lld %lld %d %d",
ACTION_BEGIN, aTaskId, GetTimestamp(), getpid(), gettid());
}
}
BOOL CMeterFileWriter::AddData(ENDIENTRY *pEndDevice, EUI64 *id, BYTE *pszData, int nLength)
{
CMeterParser *parser;
METERWRITEITEM *pItem;
METERLPENTRY *pSave;
if (!pEndDevice || !pszData || (nLength <= 0))
return FALSE;
pItem = (METERWRITEITEM *)MALLOC(sizeof(METERWRITEITEM));
if (pItem == NULL)
return FALSE;
memset(pItem, 0, sizeof(METERWRITEITEM));
memcpy(&pItem->id, id, sizeof(EUI64));
pItem->nSize = sizeof(METERLPENTRY) + nLength;
pItem->pData = (METERLPENTRY *)MALLOC(pItem->nSize);
if (pItem->pData == NULL)
{
FREE(pItem);
return FALSE;
}
pSave = (METERLPENTRY *)pItem->pData;
memcpy(&pSave->mlpId, id, sizeof(EUI64));
strncpy(pSave->mlpMid, pEndDevice->szSerial, sizeof(pSave->mlpMid));
parser = m_pMeterParserFactory->SelectParser(pEndDevice->szParser);
pSave->mlpType = parser ? parser->GetType() : SENSOR_TYPE_UNKNOWN;
/** Pulse 식 Meter들은 Service Type이 검침데이터에 의해 결정된다.
* 따라서 기존에 parser의 기본 Service 정보를 쓰던 것을 EndDevice별 Service 정보를 쓰도록 수정한다.
*
* pSave->mlpServiceType = parser ? parser->GetService() : SERVICE_TYPE_UNKNOWN;
*/
pSave->mlpServiceType = pEndDevice->nServiceType;
pSave->mlpVendor = pEndDevice->nVendor;
pSave->mlpDataCount = 1;
pSave->mlpLength = HostToLittleShort(sizeof(TIMESTAMP) + nLength);
GetTimestamp(&pSave->mlpTime, NULL);
memcpy(pSave->mlpData, pszData, nLength);
m_Locker.Lock();
pItem->nPosition = (int)m_List.AddTail(pItem);
m_Locker.Unlock();
ActiveThread();
return TRUE;
}
void Logger::Error( const std::string& message, const std::string& location /* = "" */ )
{
std::cerr << "** " << GetTimestamp() << "\t" << message;
if ( location != "" ) { std::cerr << " @ " << location; }
std::cerr << "\t LINE " << __LINE__ << " FILE " << __FILE__ ;
std::cerr << std::endl;
std::string loc = location;
if ( loc == "" ) { loc = "-"; }
m_file << "<tr class='error'>"
<< "<td class='time'>" << GetFormattedTimestamp() << "</td>"
<< "<td class='location'>" << loc << "</td>"
<< "<td class='message'>" << message << "</td>"
<< "</tr>" << std::endl;
}
// -----------------------------------------------------------------------------
static void SendMotorSetpoints(void)
{
struct Setpoints {
int16_t motor_setpoints[MAX_MOTORS];
uint16_t timestamp;
} __attribute__((packed)) setpoints;
_Static_assert(((sizeof(struct Setpoints) + 2) / 3) * 4 + 6
< UART_TX_BUFFER_LENGTH, "Setpoints is too large for the UART TX buffer");
for (uint16_t i = MAX_MOTORS; i--; )
setpoints.motor_setpoints[i] = MotorSetpoint(i);
setpoints.timestamp = GetTimestamp();
MKSerialTx(1, 'I', (uint8_t *)&setpoints, sizeof(setpoints));
}
/*----------------------------------------------------------------------*
| SendRename |
*----------------------------------------------------------------------*/
void
SendRenameCommand(Window window, int commID, char *new_name)
{
Time time;
XtPointer msg, save;
CARD32 commandID = 0L;
CARD32 count = 1L;
CARD32 windowBit = 1<<30;
/* CARD32 iconBit = 1<<31;*/
char str[128];
int size=0;
commandID += commID;
time = GetTimestamp(dsp);
/*
* Fixup the window id receiving the client command.
* Top 2 bits:
* 10 - Icon only
* 01 - Window only
* 11 - Both Icon and Window
* 00 - Reserved (Don't Use!)
*
* Bottom 30 bits:
* 0x3fffffff - All windows and/or icons
* 0x00000000 - Root window menu
*/
size += sizeof(CARD32); /* command id */
size += sizeof(CARD16) + (strlen(new_name) * sizeof(CARD8));
size += sizeof(CARD32); /* count */
size += sizeof(CARD32); /* window */
save = msg = (XtPointer) XtMalloc(size);
window |= windowBit;
msg = PackCARD32(msg, (CARD32)commandID);
msg = PackString(msg, (String) new_name);
msg = PackCARD32(msg, (CARD32)count);
msg = PackCARD32(msg, (CARD32)window);
PRINT("RenameCommand #%d to %s on window %x (%u).\n", commandID,
new_name, window, window);
SendMessage(RENAME, save, size, time);
}
/*----------------------------------------------------------------------*
| SendRemove |
*----------------------------------------------------------------------*/
void
SendRemoveCommand(Window window, CARD32 commandID)
{
Time time;
XtPointer msg,save;
CARD32 selection = 0L;
CARD32 count = 1L;
CARD32 windowBit = 1<<30;
/* CARD32 iconBit = 1<<31;*/
char str[128];
int size=0;
time = GetTimestamp(dsp);
selection += atoms.own_selection;
/*
* Fixup the window id receiving the client command.
* Top 2 bits:
* 10 - Icon only
* 01 - Window only
* 11 - Both Icon and Window
* 00 - Reserved (Don't Use!)
*
* Bottom 30 bits:
* 0x3fffffff - All windows and/or icons
* 0x00000000 - Root window menu
*/
window |= windowBit;
size += sizeof(CARD32); /* command id */
size += sizeof(CARD32); /* count */
size += sizeof(CARD32); /* window id */
save = msg = (XtPointer) XtMalloc(size);
msg = PackCARD32(msg, (CARD32) commandID);
msg = PackCARD32(msg, (CARD32) count);
msg = PackCARD32(msg, (CARD32) window);
PRINT("RemoveCommand #%d on window %x (%u).\n", commandID, window, window);
SendMessage(REMOVE, save, size, time);
}
void CMediaStream::SetUpSRTPKeys (void)
{
if (GetBoolValue(STREAM_VIDEO_USE_SRTP)) {
srandom(GetTimestamp());
if (GetBoolValue(STREAM_VIDEO_SRTP_FIXED_KEYS)) {
CHECK_AND_FREE(m_video_key);
m_video_key = strdup(GetStringValue(STREAM_VIDEO_SRTP_KEY));
CHECK_AND_FREE(m_video_salt);
m_video_salt = strdup(GetStringValue(STREAM_VIDEO_SRTP_SALT));
} else {
if (m_video_key == NULL)
m_video_key = generate_random(16);
if(m_video_salt == NULL)
m_video_salt = generate_random(14);
}
}
if (GetBoolValue(STREAM_AUDIO_USE_SRTP)) {
if (GetBoolValue(STREAM_AUDIO_SRTP_FIXED_KEYS)) {
CHECK_AND_FREE(m_audio_key);
m_audio_key = strdup(GetStringValue(STREAM_AUDIO_SRTP_KEY));
CHECK_AND_FREE(m_audio_salt);
m_audio_salt = strdup(GetStringValue(STREAM_AUDIO_SRTP_SALT));
} else {
if (m_audio_key == NULL)
m_audio_key = generate_random(16);
if(m_audio_salt == NULL)
m_audio_salt = generate_random(14);
}
}
if (GetBoolValue(STREAM_TEXT_USE_SRTP)) {
if (GetBoolValue(STREAM_TEXT_SRTP_FIXED_KEYS)) {
CHECK_AND_FREE(m_text_key);
m_text_key = strdup(GetStringValue(STREAM_TEXT_SRTP_KEY));
CHECK_AND_FREE(m_text_salt);
m_text_salt = strdup(GetStringValue(STREAM_TEXT_SRTP_SALT));
} else {
if (m_text_key == NULL)
m_text_key = generate_random(16);
if(m_text_salt == NULL)
m_text_salt = generate_random(14);
}
}
}
void StepShm(int cntr)
{
if (!status.calibrated)
{
printf("ZLift is not calibrated. Please calibrate and run again. Exiting.\n");
endme(1);
}
SetTimestamp(GetTimestamp()); //Pass back timestamp as a heartbeat
joint_state_g.header.stamp = ros::Time::now();
joint_state_g.position[0] = status.position;
joint_state_g.velocity[0] = status.velocity;
joint_state_g.effort[0] = status.effort;
zlift_publisher_g.publish(joint_state_g);
/*if (cntr % 100 == 0)
{
if (1)
{
printf("********************************\n");
printf("timestamp: %ld\n", status.timestamp);
{
printf("------------------------------\n");
printf("position: %f\n", status.position);
printf("velocity: %f\n", status.velocity);
printf("effort: %f\n", status.effort);
printf("------------------------------\n");
printf("\n");
}
}
}*/
/*cmd.position = 600;
cmd.velocity = 2000;
cmd.stiffness = 1.0;
cmd.control_mode = JOINT_MODE_ROS_THETA_GC;
cmd.smoothing_mode = SMOOTHING_MODE_SLEW;*/
}
void AddLabel(const char* aFormat, ...)
{
TraceInfo* info = GetOrCreateTraceInfo();
ENSURE_TRUE_VOID(info);
va_list args;
va_start(args, aFormat);
nsAutoCString buffer;
buffer.AppendPrintf(aFormat, args);
va_end(args);
// Log format:
// [3 taskId "label"]
nsCString* log = info->AppendLog();
if (log) {
log->AppendPrintf("%d %lld %lld \"%s\"", ACTION_ADD_LABEL, info->mCurTaskId,
GetTimestamp(), buffer.get());
}
}
void CIhdParser::ActPostSuccAction(CChunk *pChunk, ONDEMANDPARAM *pOndemand, void *pExtraData)
{
IHD_METERING_DATA data;
ENDIENTRY *pEndi;
pEndi = m_pEndDeviceList->GetEndDeviceByID(&pOndemand->id);
memset(&data, 0, sizeof(IHD_METERING_DATA));
data.infoLength = sizeof(IHD_METERING_DATA);
data.hwVer = pEndi->hw;
data.swVer = pEndi->fw;
data.swBuild = pEndi->build;
GetTimestamp(&data.currentTime, NULL);
data.swBuild = HostToBigShort(data.swBuild);
data.currentTime.year = HostToBigShort(data.currentTime.year);
pChunk->Add((char *)&data, sizeof(IHD_METERING_DATA));
}
Boolean
WSMDefaultOwnSelection(Widget w, WSMClientType client_type,
WSMRequestCallbackFunc request_callback,
XtPointer request_data)
{
Time time;
Display *dpy = XtDisplay(w);
Atom own_selection =
_WSMGetSelectionAtom(dpy,
XScreenNumberOfScreen(XtScreen(w)), client_type);
if (XGetSelectionOwner(dpy, own_selection) != None) {
/*
* Someone out there already owns this selection, we should give
* up and return.
*/
printf("Error - Someone out there already owns this selection.\n");
return(False);
}
if (!XtIsRealized(w)) {
fprintf(stderr, "%s must be realized, and is not.\n",
"Programmer Error: Widget passed to WSMDefaultOwnSelection");
return(False);
}
WSMRegisterRequestCallback(dpy,
XScreenNumberOfScreen(XtScreen(w)),
request_callback, request_data);
time = GetTimestamp(dpy); /* CurrentTime or Zero is a no-no! */
/*
* NOTE - w MUST have its XmNconvertCallback set properly,
* otherwise, no conversions will be handled!
*/
XmeNamedSource(w, own_selection, time);
return(True);
}
int main(int argc, char **argv)
{
struct timeval start,stop,diff;
gettimeofday(&start,0);
//做你要做的事...
gettimeofday(&stop,0);
tim_subtract(&diff,&start,&stop);
printf("总计用时:%d毫秒\n",diff.tv_usec);
//struct timeval Now;
// gettimeofday(&Now, NULL);
//sprintf(TimeBuffer, "%.63s", Now.tv_usec+4);
// printf("saahdashdsa%u\n", Now.tv_usec);
// GetTimestamp();
while(1){
printf ("%s \n", GetTimestamp());
sleep(2);
}
}
void FlotillaDock::tick(){
if (state != Connected) return;
int channel_index;
mutex.lock();
while (command_queue.size() > 0){
sp_blocking_write(port, command_queue.front().c_str(), command_queue.front().length(), 0);
command_queue.pop();
std::this_thread::sleep_for(std::chrono::microseconds(100000));
}
mutex.unlock();
for (channel_index = 0; channel_index < MAX_CHANNELS; channel_index++){
if (module[channel_index].state != ModuleConnected) continue;
std::string update;
if (module[channel_index].get_next_update(update)){
std::ostringstream stream;
stream << "s " << (channel_index + 1) << " " << update;
update = stream.str();
#ifdef DEBUG_TRANSPORT
std::ostringstream msg;
msg << GetTimestamp() << "Sending to dock: " << update << std::endl;
std::cout << msg.str();
#endif
while (sp_output_waiting(port) > 0) {};
sp_blocking_write(port, update.c_str(), update.length(), 0);
sp_blocking_write(port, "\r", 1, 0);
std::this_thread::sleep_for(std::chrono::microseconds(10000));
}
}
while (sp_input_waiting(port) > 0){
process_command(sp_readline(port));
}
}
// -----------------------------------------------------------------------------
static void CopyUBloxMessage(uint8_t id)
{
// TODO: do this in a more efficient way
switch (id)
{
case UBX_ID_POS_LLH:
memcpy(&ubx_pos_llh_, &data_buffer_[0], sizeof(struct UBXPosLLH));
new_data_bits_ |= UBX_NEW_DATA_BIT_POS_LLH;
UpdatePositionToFlightCtrl();
#ifdef LOG_DEBUG_TO_SD
// SetNewDataCallback(LogUBXPosLLH);
#endif
break;
case UBX_ID_VEL_NED:
memcpy(&ubx_vel_ned_, &data_buffer_[0], sizeof(struct UBXVelNED));
new_data_bits_ |= UBX_NEW_DATA_BIT_VEL_NED;
UpdateVelocityToFlightCtrl();
#ifdef LOG_DEBUG_TO_SD
// SetNewDataCallback(LogUBXVelNED);
#endif
break;
case UBX_ID_SOL:
memcpy(&ubx_sol_, &data_buffer_[0], sizeof(struct UBXSol));
new_data_bits_ |= UBX_NEW_DATA_BIT_SOL;
#ifdef LOG_DEBUG_TO_SD
// SetNewDataCallback(LogUBXSol);
#endif
break;
case UBX_ID_TIME_UTC:
memcpy(&ubx_time_utc_, &data_buffer_[0], sizeof(struct UBXTimeUTC));
new_data_bits_ |= UBX_NEW_DATA_BIT_TIME_UTC;
#ifdef LOG_DEBUG_TO_SD
// SetNewDataCallback(LogUBXTimeUTC);
#endif
break;
}
last_reception_timestamp_ = GetTimestamp();
error_bits_ &= ~UBX_ERROR_BIT_STALE;
}
String UnixFileSystemBase::CreateTempFilename()
{
String prefix = Path::Combine(GetTempDirectory(), (String)(int)getpid() + "-");
Random rand(DefaultTraits::Hash(GetTimestamp()));
static const char* cs = "1234567890abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";
int cl = strlen(cs);
while (true)
{
String tmp = String::Create(8);
for (int i = 0; i < tmp.Length(); i++)
tmp[i] = cs[rand.NextInt(cl)];
String result = prefix + tmp;
struct stat attributes;
if (stat(result.Ptr(), &attributes) == -1)
return result;
}
}
void staterr(const char *str, ...)
{
va_list ar;
char buf[40000];
va_start(ar, str);
vsprintf(buf, str, ar);
va_end(ar);
fprintf(stdout, "%s\n", buf);
fflush(stdout);
if (LogFilename[0])
{
FILE *fp = fopen(LogFilename, "a+");
if (fp)
{
fprintf(fp, "%s << %s >>\n", GetTimestamp(), buf);
fclose(fp);
}
}
}
void RTPPacket::Dump()
{
int i;
printf("Payload type: %d\n",(int)GetPayloadType());
printf("Extended sequence number: 0x%08x\n",GetExtendedSequenceNumber());
printf("Timestamp: 0x%08x\n",GetTimestamp());
printf("SSRC: 0x%08x\n",GetSSRC());
printf("Marker: %s\n",HasMarker()?"yes":"no");
printf("CSRC count: %d\n",GetCSRCCount());
for (i = 0 ; i < GetCSRCCount() ; i++)
printf(" CSRC[%02d]: 0x%08x\n",i,GetCSRC(i));
printf("Payload: %s\n",GetPayloadData());
printf("Payload length: %d\n",GetPayloadLength());
printf("Packet length: %d\n",GetPacketLength());
printf("Extension: %s\n",HasExtension()?"yes":"no");
if (HasExtension())
{
printf(" Extension ID: 0x%04x\n",GetExtensionID());
printf(" Extension data: %s\n",GetExtensionData());
printf(" Extension length: %d\n",GetExtensionLength());
}
}
void FlotillaDock::disconnect(void){
if (state == Disconnected) return;
state = Disconnected;
int x;
for (x = 0; x < MAX_CHANNELS; x++){
if (module[x].state == ModuleConnected){
module[x].disconnect();
queue_module_event(x);
}
}
sp_flush(port, SP_BUF_OUTPUT);
sp_flush(port, SP_BUF_INPUT);
sp_close(port);
sp_free_port(port);
std::ostringstream msg;
msg << GetTimestamp() << "Dock Disconnected" << std::endl; // , serial " << serial << std::endl;
std::cout << msg.str();
}
// This function sends data that has been requested.
void SendPendingMKSerial(void)
{
// Handle only one request at a time.
if (tx_request_)
{
// A one-time request has higher priority than a periodic "stream" of data.
if (tx_request_ & MK_TX_VERSION) SendVersion();
}
else if (mk_stream_ && TimestampInPast(stream_timer_))
{
// A data stream is active and it is time for another transmission.
switch (mk_stream_)
{
case MK_STREAM_CONTROL:
SendControlData();
break;
case MK_STREAM_KALMAN:
SendKalmanData();
break;
case MK_STREAM_MOTOR_SETPOINTS:
SendMotorSetpoints();
break;
case MK_STREAM_SENSORS:
SendSensorData();
break;
default:
break;
}
stream_timer_ += stream_period_;
// Prevent timer rollover for small periods.
if (TimestampInPast(stream_timer_)) stream_timer_ = GetTimestamp();
// Disable the stream if no request has been renewing received in a while.
if (TimestampInPast(stream_timeout_)) mk_stream_ = MK_STREAM_NONE;
}
}
/*----------------------------------------------------------------------*
| SendDefineCommand |
*----------------------------------------------------------------------*/
void
SendDefineCommand (int parID, int commID, char *cmdName, char *cmdDefault)
{
Time time;
XtPointer msg,save;
int size=0;
CARD32 selection = 0L;
CARD32 commandID = 0L;
CARD32 parentID = 0L;
commandID += commID;
parentID += parID;
time = GetTimestamp(dsp);
selection += atoms.own_selection;
size += sizeof(CARD32); /* command id */
size += sizeof(CARD32); /* selection */
size += sizeof(CARD32); /* parent id */
size += sizeof(CARD16) + (strlen(cmdName) * sizeof(CARD8)); /* name */
size += sizeof(CARD16) + (strlen(cmdDefault) * sizeof(CARD8)); /* defalault label */
save = msg = (XtPointer) XtMalloc(size);
msg = PackCARD32(msg, (CARD32) commandID);
msg = PackCARD32(msg, (CARD32) selection);
msg = PackCARD32(msg, (CARD32) parentID);
msg = PackString(msg, (String) cmdName);
msg = PackString(msg, (String) cmdDefault);
PRINT("DefineCommand: %d, %d, %d, '%s', '%s'\n",
commandID, selection, parentID, cmdName, cmdDefault);
SendMessage(DEFINE, save, size, time);
}
//Convert from joint-space to actuator-space and do slew/min-jerk smoothing
void M3Joint::CalcThetaDesiredSmooth()
{
mReal q_des_jt,q_on, q_des;
//Ramp in from theta at switchover point
q_on=q_on_slew.Step(1.0,1.0/MODE_Q_ON_SLEW_TIME);
q_des_jt=command.q_desired()*q_on+(1.0-q_on)*q_switch;
//Clamp in range
q_des_jt=CLAMP(q_des_jt,param.min_q()+param.min_q_pad(), param.max_q()-param.max_q_pad());
if (command.ctrl_mode()==JOINT_MODE_THETA || command.ctrl_mode()==JOINT_MODE_THETA_GC)
q_des_jt=q_slew.Step(q_des_jt,CLAMP(command.q_slew_rate(),0.,param.max_q_slew_rate()));
//Min-Jerk
if (command.ctrl_mode()==JOINT_MODE_THETA_MJ || command.ctrl_mode()==JOINT_MODE_THETA_GC_MJ )
q_des_jt=jerk_joint.Step(GetTimestamp(),q_des_jt,(mReal)command.qdot_desired());
//Min-Jerk may make desired out of bounds so re-clamp
q_des_jt=CLAMP(q_des_jt,param.min_q()+param.min_q_pad(),param.max_q()-param.max_q_pad());
//Set result
trans->SetThetaDesJointDeg(q_des_jt);
//trans->SetThetaDotDesJointDeg(act->GetThetaDotDeg());
}
OMX_BOOL AvcDecoder_OMX::GetYuv(OMX_U8** aOutBuf, OMX_U32* aOutBufSize, OMX_TICKS* aOutTimestamp)
{
unsigned char* out_data_virt = NULL;
unsigned int out_size = 0;
out_data_virt = mfc_get_yuv(&out_size);
if(out_data_virt)
{
*aOutBuf = out_data_virt;
*aOutBufSize = out_size;
if(GetTimestamp(aOutTimestamp) == OMX_FALSE)
LOGE("GetTimestamp fail\n");
}
else
{
//*aOutBufSize = 0;
//*aOutTimestamp = 0;
return OMX_FALSE;
}
return OMX_TRUE;
}
bool MMDeviceAudioSource::GetNextBuffer(void **buffer, UINT *numFrames, QWORD *timestamp)
{
UINT captureSize = 0;
bool bFirstRun = true;
HRESULT hRes;
UINT64 devPosition, qpcTimestamp;
LPBYTE captureBuffer;
UINT32 numFramesRead;
DWORD dwFlags = 0;
while (true) {
if (inputBufferSize >= sampleWindowSize*GetChannelCount()) {
if (bFirstRun) {
lastQPCTimestamp += 10;
} else if (bIsMic && !bUseQPC) {
captureSize = 0;
mmCapture->GetNextPacketSize(&captureSize);
//throws away worthless mic data that's sampling faster than the desktop buffer.
//disgusting fix for stupid worthless mic issues.
if (captureSize > 0) {
++numTimesInARowNewDataSeen;
if (numTimesInARowNewDataSeen > angerThreshold) {
if (SUCCEEDED(mmCapture->GetBuffer(&captureBuffer, &numFramesRead, &dwFlags, &devPosition, &qpcTimestamp))) {
mmCapture->ReleaseBuffer(numFramesRead);
numTimesInARowNewDataSeen = 0;
}
}
} else {
numTimesInARowNewDataSeen = 0;
}
}
firstTimestamp = GetTimestamp(lastQPCTimestamp);
break;
}
//---------------------------------------------------------
hRes = mmCapture->GetNextPacketSize(&captureSize);
if (FAILED(hRes)) {
RUNONCE AppWarning(TEXT("MMDeviceAudioSource::GetBuffer: GetNextPacketSize failed, result = %08lX"), hRes);
return false;
}
if (!captureSize)
return false;
//---------------------------------------------------------
hRes = mmCapture->GetBuffer(&captureBuffer, &numFramesRead, &dwFlags, &devPosition, &qpcTimestamp);
if (FAILED(hRes)) {
RUNONCE AppWarning(TEXT("MMDeviceAudioSource::GetBuffer: GetBuffer failed, result = %08lX"), hRes);
return false;
}
UINT totalFloatsRead = numFramesRead*GetChannelCount();
if (bConvert) {
if (convertBuffer.Num() < totalFloatsRead)
convertBuffer.SetSize(totalFloatsRead);
short *shortBuffer = (short*)captureBuffer;
for (UINT i = 0; i < totalFloatsRead; i++)
convertBuffer[i] = float(shortBuffer[i])*(1.0f/32767.0f);
captureBuffer = (LPBYTE)convertBuffer.Array();
}
if (inputBufferSize) {
double timeAdjust = double(inputBufferSize/GetChannelCount());
timeAdjust /= (double(GetSamplesPerSec())*0.0000001);
qpcTimestamp -= UINT64(timeAdjust);
}
qpcTimestamp /= 10000;
lastQPCTimestamp = qpcTimestamp;
//---------------------------------------------------------
UINT newInputBufferSize = inputBufferSize + totalFloatsRead;
if (newInputBufferSize > inputBuffer.Num())
inputBuffer.SetSize(newInputBufferSize);
mcpy(inputBuffer.Array()+inputBufferSize, captureBuffer, totalFloatsRead*sizeof(float));
inputBufferSize = newInputBufferSize;
mmCapture->ReleaseBuffer(numFramesRead);
bFirstRun = false;
}
*numFrames = sampleWindowSize;
*buffer = (void*)inputBuffer.Array();
*timestamp = firstTimestamp;
/*if (bIsMic) {
static QWORD lastTimestamp = 0;
if (firstTimestamp != lastTimestamp+10)
Log(TEXT("A: %llu, difference: %llu"), firstTimestamp, firstTimestamp-lastTimestamp);
lastTimestamp = firstTimestamp;
}*/
return true;
}
