/***************************************************************
【函数功能】： 使能惯导模块的数据输出
【输 入】： bEnable：True使能输出；False失能输出
【输 出】： 执行结果：0正确；-1错误
【说 明】： ！经过实验验证！
***************************************************************/
int CGyro::Enable(bool bEnable)
{
char eBuf[18] = {'@','*',1,1, 0,0,0,0, 0,0,0,0, 0,0,0,0, -2,'*'};
char dBuf[18] = {'@','*',1,0, 0,0,0,0, 0,0,0,0, 0,0,0,0, -1,'*'};
if(!m_bPortOpened)
return -1;
//if (bEnable^m_bOutEnabled) // 0,1或1,0才输出。不同才操作
{
if (bEnable)
{
sio_write(m_iPort, eBuf, 18);
sleep(10);
sio_write(m_iPort, eBuf, 18);
m_bOutEnabled = true;
}
else
{
sio_write(m_iPort, dBuf, 18);
sleep(10);
sio_write(m_iPort, dBuf, 18);
m_bOutEnabled = false;
}
}
return 0;
}
static ALuint sndio_proc(ALvoid *ptr)
{
ALCdevice *device = ptr;
sndio_data *data = device->ExtraData;
ALsizei frameSize;
size_t wrote;
SetRTPriority();
frameSize = FrameSizeFromDevFmt(device->FmtChans, device->FmtType);
while(!data->killNow && device->Connected)
{
ALsizei len = data->data_size;
ALubyte *WritePtr = data->mix_data;
aluMixData(device, WritePtr, len/frameSize);
while(len > 0 && !data->killNow)
{
wrote = sio_write(data->sndHandle, WritePtr, len);
if(wrote == 0)
{
ERR("sio_write failed\n");
aluHandleDisconnect(device);
break;
}
len -= wrote;
WritePtr += wrote;
}
}
return 0;
}
void
SNDDMA_Submit(void)
{
struct pollfd pfd;
size_t count, todo, avail;
int n;
n = sio_pollfd(hdl, &pfd, POLLOUT);
while (poll(&pfd, n, 0) < 0 && errno == EINTR)
;
if (!(sio_revents(hdl, &pfd) & POLLOUT))
return;
avail = dma_buffer_size;
while (avail > 0) {
todo = dma_buffer_size - dma_ptr;
if (todo > avail)
todo = avail;
count = sio_write(hdl, dma_buffer + dma_ptr, todo);
if (count == 0)
break;
dma_ptr += count;
if (dma_ptr >= dma_buffer_size)
dma_ptr -= dma_buffer_size;
avail -= count;
}
}
/*
* fluid_sndio_audio_run
*/
void*
fluid_sndio_audio_run2(void* d)
{
fluid_sndio_audio_driver_t* dev = (fluid_sndio_audio_driver_t*) d;
short* buffer = (short*) dev->buffer;
float* left = dev->buffers[0];
float* right = dev->buffers[1];
int buffer_size = dev->buffer_size;
int dither_index = 0;
FLUID_LOG(FLUID_DBG, "Audio thread running");
/* it's as simple as that: */
while (dev->cont)
{
(*dev->callback)(dev->data, buffer_size, 0, NULL, 2, dev->buffers);
fluid_synth_dither_s16 (&dither_index, buffer_size, left, right,
buffer, 0, 2, buffer, 1, 2);
sio_write (dev->hdl, buffer, dev->buffer_byte_size);
}
FLUID_LOG(FLUID_DBG, "Audio thread finished");
pthread_exit(NULL);
return 0; /* not reached */
}
//---------------------------------------------------------------------------
bool __fastcall CSerialPort::WriteData(char *pBuffer)
{
if(sio_write(m_nPortNo,pBuffer,strlen(pBuffer))<0)
return false;
return true;
}
static uint16_t wbsio_get_spibase(uint16_t port)
{
uint8_t id;
uint16_t flashport = 0;
w836xx_ext_enter(port);
id = sio_read(port, 0x20);
if (id != 0xa0) {
msg_perr("\nW83627 not found at 0x%x, id=0x%02x want=0xa0.\n", port, id);
goto done;
}
if (0 == (sio_read(port, 0x24) & 2)) {
msg_perr("\nW83627 found at 0x%x, but SPI pins are not enabled. (CR[0x24] bit 1=0)\n", port);
goto done;
}
sio_write(port, 0x07, 0x06);
if (0 == (sio_read(port, 0x30) & 1)) {
msg_perr("\nW83627 found at 0x%x, but SPI is not enabled. (LDN6[0x30] bit 0=0)\n", port);
goto done;
}
flashport = (sio_read(port, 0x62) << 8) | sio_read(port, 0x63);
done:
w836xx_ext_leave(port);
return flashport;
}
/* write as many blocks as is currently possible */
static void
sndio_update(int threaded)
{
struct pollfd pfd;
nfds_t nfds;
int i, nblocks, nbytes;
/* make sure counters have been updated */
nfds = sio_pollfd(hdl, &pfd, POLLOUT);
poll(&pfd, nfds, 0);
if (!(sio_revents(hdl, &pfd) & POLLOUT))
return;
nblocks = (sndio_play_appbufsz - (sndio_playpos - sndio_realpos)) /
sndio_play_round;
/* we got POLLOUT, so we can write something. if we don't
* write anything, we could underrun.
*/
if (nblocks < 1)
nblocks = 1;
for (i = 0; i < nblocks; i++) {
sio_write(hdl, sndio_play_bufdata, sndio_play_bufsize);
sndio_playpos += sndio_play_round;
if (sio_eof(hdl)) {
/* print error message? */
return;
}
_mix_some_samples((uintptr_t) sndio_play_bufdata, 0, sndio_signed);
}
}
static u32_t
ppp_output_cb(ppp_pcb *pcb, u8_t *data, u32_t len, void *ctx)
{
LWIP_UNUSED_ARG(pcb);
LWIP_UNUSED_ARG(ctx);
return sio_write(ppp_sio, data, len);
}
void AudioSndio::run( void )
{
surroundSampleFrame * temp =
new surroundSampleFrame[mixer()->framesPerPeriod()];
int_sample_t * outbuf =
new int_sample_t[mixer()->framesPerPeriod() * channels()];
while( TRUE )
{
const fpp_t frames = getNextBuffer( temp );
if( !frames )
{
break;
}
uint bytes = convertToS16( temp, frames,
mixer()->masterGain(), outbuf, FALSE );
if( sio_write( m_hdl, outbuf, bytes ) != bytes )
{
break;
}
}
delete[] temp;
delete[] outbuf;
}
static gboolean
try_write (Stream *stream)
{
#define STEP 1024
int avail, step, rendered, written, bpf;
gboolean finish = FALSE;
avail = swfdec_playback_stream_avail_update (stream);
bpf = stream->par.bps * stream->par.pchan;
while (avail > 0 && !finish) {
gint16 data[2 * STEP];
step = MIN (avail, STEP);
rendered = swfdec_audio_render (stream->audio, data, stream->offset, step);
if (rendered < step) {
finish = TRUE;
}
written = sio_write (stream->hdl, data, rendered * bpf) / bpf;
avail -= written;
stream->offset += rendered;
stream->f_written += written;
}
if (finish) {
swfdec_playback_stream_remove_handlers (stream);
}
return TRUE;
#undef STEP
}
/***************************************************************
【函数功能】： 发送Set报文
【输 入】： 无
【输 出】： 执行结果：0正确；-1错误；-2发送报文失败；
【说 明】： 在timer中第6发送时刻被调用。直接通过串口发送。
***************************************************************/
int CSensor::SendSetFrame()
{
if(!m_bPortOpened)
return -1;
if (m_pSendQueue->QueueNData() < 10)
{
m_pSendQueue->QueueFlush();
return -1;
}
char buf[10];
m_pSendQueue->QueueNRead(buf, 10);
if (((BYTE)buf[0] != 0xAA) || ((BYTE)buf[1] != 0x55))
{
return -1;
}
/**********************************************/
WaitForSingleObject(m_hMutex,1000);
// 占用串口发送资源
m_sCmd.send = (BYTE)buf[3];
m_sCmd.stat = STAT_SENSOR_T2T;
m_sCmd.addr = (BYTE)buf[4];
m_sCmd.ack = CMD_IO_ACK;
// 直接通过串口发送，保证时间精度
int err = sio_write(m_iPort, buf, 10); // 持续时间大约10us
m_sCmd.stat = STAT_SENSOR_T2R;
ReleaseMutex(m_hMutex);
/**********************************************/
if (err != 10)
return -2;
return 0;
}
static PaError
BlockingWriteStream(PaStream* stream, const void *data, unsigned long numFrames)
{
PaSndioStream *s = (PaSndioStream *)stream;
unsigned n, res;
while (numFrames > 0) {
n = s->par.round;
if (n > numFrames)
n = numFrames;
PaUtil_SetOutputFrameCount(&s->bufproc, n);
PaUtil_SetInterleavedOutputChannels(&s->bufproc, 0, s->wbuf, s->par.pchan);
res = PaUtil_CopyOutput(&s->bufproc, &data, n);
if (res != n) {
DPR("BlockingWriteStream: copyOutput: %u != %u\n");
return paUnanticipatedHostError;
}
res = sio_write(s->hdl, s->wbuf, n * s->par.pchan * s->par.bps);
if (res == 0)
return paUnanticipatedHostError;
s->wpos += n;
numFrames -= n;
}
return paNoError;
}
static void
bufdump (struct xmp_context *ctx, int len)
{
void *buf;
if ((buf = xmp_smix_buffer (ctx)) != NULL)
sio_write (hdl, buf, len);
}
static int output_data(char *buf, int32 nbytes)
{
if (!sio_write(sndio_ctx, buf, nbytes)) {
ctl->cmsg(CMSG_WARNING, VERB_VERBOSE, "sio_write() failed");
return -1;
}
return 0;
}
/*
=======================================
RS232 数据发送
=======================================
*/
CR_API void_t
qst_rs232_send (
__CR_IN__ void_t* obj,
__CR_IN__ const void_t* data,
__CR_IN__ uint_t size
)
{
sio_write((uint_t)obj, data, size);
}
static int sndio_write(const char *buf, int cnt)
{
size_t rc;
rc = sio_write(hdl, buf, cnt);
if (rc == 0)
return -OP_ERROR_INTERNAL;
return rc;
}
/***************************************************************
【函数功能】： 输入校正坐标数据
【输 入】： iFlag：校正哪个数据；fValue：数据值
【输 出】： 执行结果：0正确；-1错误。
【说 明】： ！经过实验验证！ 角度信息的单位必须是弧度
***************************************************************/
int CGyro::Reset(int iFlag, double fValue)
{
if(!m_bPortOpened)
return -1;
UGyroFrame uFrame = {0};
uFrame.st.head[0] = '@';
uFrame.st.head[1] = '*';
uFrame.st.cmd[0] = -1;
if (iFlag > 7)
{
//AfxMessageBox("惯导：复位数据的flag错误！");
return -1;
}
switch (iFlag)
{
case GYRO_RST_POSX:
uFrame.st.posx = fValue;
break;
case GYRO_RST_POSY:
uFrame.st.posy = fValue;
break;
case GYRO_RST_ANGLE:
fValue -= M_PI/2.0f;
while (fValue > MAX_GYRO_ANGLE)
{
fValue -= 2*M_PI;
}
while(fValue <= MIN_GYRO_ANGLE)
{
fValue += 2*M_PI;
}
fValue = fValue * (-1.0f); // 还原成顺正逆负
if (fValue < 0)
{
fValue += 2*M_PI;
}
uFrame.st.angle = fValue;
break;
default:
//AfxMessageBox("惯导：复位数据的flag错误！");
return -1;
}
uFrame.st.cmd[1] = (BYTE)iFlag;
char sum = 0;
for (int i=0; i<14; i++)
{
sum += uFrame.buf[2+i];
}
uFrame.st.checksum = (BYTE)(-sum);
uFrame.st.tail = '*';
sio_write(m_iPort,uFrame.buf, 18);
return 0;
}
// internal: handle message
// return 1 to return to user, 0 to eat, othere to return this error
int _sio_handlemsg(std::string & msg) {
// system handler
if (msg.substr(0, 10)=="SYS-CPING\t") {
std::string r2 = msg.substr(10);
if (sio_write(SIO_DATA, "SYS-CPONG\t%s", r2.c_str())==-1) {
return -1;
};
return 0;
};
int rv = 1; // return to user
// message match handler
int plen = 0;
if (sio_matchpref(msg.c_str(), "SYS-SET\t", &plen)>0) {
int evar = msg.find('\t', plen) + 1; // skip appname
evar = msg.find('\t', evar) + 1; // skip varname
evar = msg.find('\t', evar) + 1; // skip key
if (evar<plen) evar=plen;
std::string pref = msg.substr(plen, evar-plen-1);
int mcount = 0;
for (std::map<std::string,SIOVarRecord*>::iterator i=varlist.find(pref); i!=varlist.end(); i++) {
//printf("key = [%s], val=[0x%X]\n", i->first.c_str(), i->second);
if (i->first == pref) {
SIOVarRecord * r = i->second;
if (r) {
rv = r->parse_value_set(msg, evar);
mcount++;
};
//mcount += 100;
};
//mcount+=1000;
};
//printf("got sys-get to [%s] from [%s], matched %d\n", pref.c_str(), msg.c_str(), mcount);
};
sio_mmatch m;
// other stuff
for (std::list<SIOMessageHandler*>::iterator ll=matchlist.begin(); ll != matchlist.end(); ll++) {
m.pnum = sio_matchpref(msg.c_str(), (*ll)->prefix, &m.plen);
if (m.pnum > 0) {
m.message = msg;
int r = ((*ll)->func)(&m, (*ll)->userarg);
if (r < rv) rv = r;
};
};
return rv;
};
/**
* Asynchronously prints a null-terminated string to the serial console.
*/
void sio_puts( char *buffer ) {
char *temp = buffer;
unsigned int len = 0;
//poor man's strlen
while( *buffer++ != '\0' ) {
++len;
}
sio_write(temp, len);
}
LOCAL void actually_flush_buffer(void)
{
int l,i;
if (idx)
{
total += idx * dsize;
sio_write(hdl, buffer, dsize * idx);
}
idx = 0;
}
/*
* play given number of bytes until sio_write() blocks
*/
static int play(struct ao *ao, void **data, int samples, int flags)
{
struct priv *p = ao->priv;
int n;
n = sio_write(p->hdl, data[0], samples * ao->sstride);
p->delay += n;
if (flags & AOPLAY_FINAL_CHUNK)
reset(ao);
return n / ao->sstride;
}
/*
* play given number of samples until sio_write() blocks
*/
static int play(struct ao *ao, void **data, int samples, int flags)
{
struct priv *p = ao->priv;
int n;
n = sio_write(p->hdl, data[0], samples * ao->sstride) / ao->sstride;
p->delay += n;
p->playing = true;
/* on AOPLAY_FINAL_CHUNK, just let it underrun */
return n;
}
/**
* @brief Tests for Assertion
*
* This function tests the assertion of a given expression and
* if the expression fails, a message is printed. This function
* is implemented similar to the C library function except
* that the processing will not be aborted if the assertion fails.
*
* @param expression To be tested for assertion
* @param message Data to be printed over SIO
* @file file File in which assertion has to be tested.
* @line line Line number on which assertion has to be tested.
*/
void pal_assert(bool expression,
FLASH_STRING_T message,
int8_t *file,
uint16_t line)
{
/*
* Assert for the expression. This expression should be true always,
* false indicates that something went wrong
*/
if (!expression)
{
#ifdef TEST_HARNESS
assert_t *assert_details = (assert_t *)&assert_msg[0];
uint8_t assert_string[ASSERT_BUFFER_SIZE];
/* Only used for test environment */
assert_details->assert_cmdcode = ASSERT_INDICATION;
/* Copy the assertion message to RAM */
PGM_STRCPY((char *)assert_string, message);
/*
* Put total length of message to be printed in data[0] of echo
* indication. Data will be copied from failure_msg->data[1]
* The function snprintf is used for copying variable
* number of charaters into failure_msg->data
*/
assert_details->data[0] = snprintf((char *)&(assert_details->data[1]),
ASSERT_MESSAGE_LENGTH,"%s, line %d: assertion %s failed -",
file, line, assert_string);
/*
* The function snprintf returns the number of characters that
* would have been printed if there was enough room.
* Take the actual number of bytes that are printed
*/
assert_details->data[0] =
strlen((const char *)&(assert_details->data[1]));
/* Total size of echo indication */
assert_details->size = sizeof(assert_t) + assert_details->data[0] -
sizeof(assert_details->size);
/* Write into UART/USB */
sio_write((uint8_t*)assert_details);
#else
/* Standard for all applications */
PGM_STRCPY(tmpbuf, message);
tmpbuf[PGM_STRLEN(message)+1] = '\0';
PRINTF("Assertion Failed on File %s, line %d, expression %s\n",
file, line, tmpbuf);
#endif /* TEST_HARNESS */
}
}
/**
* Sends a chat message to the modem. Gets the send string via a callback.
* @param chat Chat
* @param item Item to send
*/
static void chat_send_cb(struct chat *chat, const struct chat_item *item)
{
LWIP_ASSERT("chat_poll: chat->send_cb != NULL", chat->send_cb != NULL);
// Get the command to send
chat->send_cb(chat, (int) item->arg, chat->buf, BUFSIZE, chat->arg);
chat->buf[BUFSIZE] = '\0';
// Send command
sio_write(chat->sd, (u8_t *) chat->buf, strlen(chat->buf));
chat->recvd = 0;
}
unsigned int CAESinkSNDIO::AddPackets(uint8_t **data, unsigned int frames, unsigned int offset)
{
if (!m_hdl)
return INT_MAX;
unsigned int frameSize = m_par.bps * m_par.pchan;
size_t size = frames * frameSize;
void *buffer = data[0] + offset * frameSize;
size_t wrote = sio_write(m_hdl, buffer, size);
m_written += wrote;
return wrote / frameSize;
}
/***************************************************************
【函数功能】： 输入校正数据
【输 入】： 坐标值 fAngle弧度值；
【输 出】： 执行结果：0正确；-1错误
【说 明】： ！经过实验验证！
***************************************************************/
int CGyro::Reset(double fPosX, double fPosY, double fAngle)
{
if(!m_bPortOpened)
return -1;
UGyroFrame uFrame = {0};
uFrame.st.head[0] = '@';
uFrame.st.head[1] = '*';
uFrame.st.cmd[0] = -1;
uFrame.st.cmd[1] = GYRO_RST_POSX | GYRO_RST_POSY | GYRO_RST_ANGLE;
uFrame.st.posx = fPosX;
uFrame.st.posy = fPosY;
fAngle -= M_PI/2.0f;
while (fAngle > MAX_GYRO_ANGLE)
{
fAngle -= 2*M_PI;
}
while(fAngle <= MIN_GYRO_ANGLE)
{
fAngle += 2*M_PI;
}
fAngle = fAngle * (-1.0f); // 还原成顺正逆负
if (fAngle < 0)
{
fAngle += 2*M_PI;
}
uFrame.st.angle = fAngle;
char sum = 0;
for (int i=0; i<14; i++)
{
sum += uFrame.buf[2+i];
}
uFrame.st.checksum = (BYTE)(-sum);
uFrame.st.tail = '*';
sio_write(m_iPort,uFrame.buf, 18);
sleep(10);
sio_write(m_iPort,uFrame.buf, 18);
return 0;
}
static void
sndio_play (int argc, char *argv [])
{ struct sio_hdl *hdl ;
struct sio_par par ;
short buffer [BUFFER_LEN] ;
SNDFILE *sndfile ;
SF_INFO sfinfo ;
int k, readcount ;
for (k = 1 ; k < argc ; k++)
{ printf ("Playing %s\n", argv [k]) ;
if (! (sndfile = sf_open (argv [k], SFM_READ, &sfinfo)))
{ puts (sf_strerror (NULL)) ;
continue ;
} ;
if (sfinfo.channels < 1 || sfinfo.channels > 2)
{ printf ("Error : channels = %d.\n", sfinfo.channels) ;
continue ;
} ;
if ((hdl = sio_open (NULL, SIO_PLAY, 0)) == NULL)
{ fprintf (stderr, "open sndio device failed") ;
return ;
} ;
sio_initpar (&par) ;
par.rate = sfinfo.samplerate ;
par.pchan = sfinfo.channels ;
par.bits = 16 ;
par.sig = 1 ;
par.le = SIO_LE_NATIVE ;
if (! sio_setpar (hdl, &par) || ! sio_getpar (hdl, &par))
{ fprintf (stderr, "set sndio params failed") ;
return ;
} ;
if (! sio_start (hdl))
{ fprintf (stderr, "sndio start failed") ;
return ;
} ;
while ((readcount = sf_read_short (sndfile, buffer, BUFFER_LEN)))
sio_write (hdl, buffer, readcount * sizeof (short)) ;
sio_close (hdl) ;
} ;
return ;
} /* sndio_play */
static int ao_sndio_write(ao_driver_t *this_gen, int16_t *data,
uint32_t num_frames)
{
sndio_driver_t *this = (sndio_driver_t *) this_gen;
size_t ret, size = num_frames * this->bytes_per_frame;
ret = sio_write(this->hdl, data, size);
if (ret == 0)
return 0;
this->playpos += num_frames;
return 1;
}
static void SNDIO_PlayAudio(_THIS)
{
int written;
/* Write the audio data */
written = sio_write(hdl, mixbuf, mixlen);
/* If we couldn't write, assume fatal error for now */
if ( written == 0 ) {
this->enabled = 0;
}
#ifdef DEBUG_AUDIO
fprintf(stderr, "Wrote %d bytes of audio data\n", written);
#endif
}
void SndioAudioDriver::write()
{
unsigned size = sndio_driver_bufferSize * 2;
for (unsigned i = 0; i < (unsigned)sndio_driver_bufferSize; ++i) {
audioBuffer[i * 2] = (short)(out_L[i] * 32768.0);
audioBuffer[i * 2 + 1] = (short)(out_R[i] * 32768.0);
}
unsigned long written = sio_write(hdl, audioBuffer, size * 2);
if (written != (size * 2) ) {
ERRORLOG("SndioAudioDriver: Error writing samples to audio device.");
}
}
