/*
* This is a wrapper around select(). It uses poll() when a fine
* poll() is available, in order to avoid limits with FD_SETSIZE,
* otherwise select() is used. An error is returned if select() is
* being used and a the number of file descriptors is larger than
* FD_SETSIZE. A NULL timeout pointer makes this function wait
* indefinitely, unles no valid file descriptor is given, when this
* happens the NULL timeout is ignored and the function times out
* immediately. When compiled with CURL_ACKNOWLEDGE_EINTR defined,
* EINTR condition is honored and function might exit early without
* awaiting timeout, otherwise EINTR will be ignored.
*
* Return values:
* -1 = system call error or nfds > FD_SETSIZE
* 0 = timeout
* N = number of file descriptors kept in file descriptor sets.
*/
int Curl_select(int nfds,
fd_set *fds_read, fd_set *fds_write, fd_set *fds_excep,
struct timeval *timeout)
{
struct timeval initial_tv;
int timeout_ms;
int pending_ms;
int error;
int r;
#ifdef HAVE_POLL_FINE
struct pollfd small_fds[SMALL_POLLNFDS];
struct pollfd *poll_fds;
int ix;
int fd;
int poll_nfds = 0;
#else
struct timeval pending_tv;
struct timeval *ptimeout;
#endif
int ret = 0;
if ((nfds < 0) ||
((nfds > 0) && (!fds_read && !fds_write && !fds_excep))) {
SET_SOCKERRNO(EINVAL);
return -1;
}
if (timeout) {
if ((timeout->tv_sec < 0) ||
(timeout->tv_usec < 0) ||
(timeout->tv_usec >= 1000000)) {
SET_SOCKERRNO(EINVAL);
return -1;
}
timeout_ms = (int)(timeout->tv_sec * 1000) + (int)(timeout->tv_usec / 1000);
}
else {
timeout_ms = -1;
}
if ((!nfds) || (!fds_read && !fds_write && !fds_excep)) {
r = wait_ms(timeout_ms);
return r;
}
pending_ms = timeout_ms;
initial_tv = curlx_tvnow();
#ifdef HAVE_POLL_FINE
if (fds_read || fds_write || fds_excep) {
fd = nfds;
while (fd--) {
if ((fds_read && (0 != FD_ISSET(fd, fds_read))) ||
(fds_write && (0 != FD_ISSET(fd, fds_write))) ||
(fds_excep && (0 != FD_ISSET(fd, fds_excep))))
poll_nfds++;
}
}
if (!poll_nfds)
poll_fds = NULL;
else if (poll_nfds <= SMALL_POLLNFDS)
poll_fds = small_fds;
else {
poll_fds = calloc((size_t)poll_nfds, sizeof(struct pollfd));
if (!poll_fds) {
SET_SOCKERRNO(ENOBUFS);
return -1;
}
}
if (poll_fds) {
ix = 0;
fd = nfds;
while (fd--) {
poll_fds[ix].events = 0;
if (fds_read && (0 != FD_ISSET(fd, fds_read)))
poll_fds[ix].events |= (POLLRDNORM|POLLIN);
if (fds_write && (0 != FD_ISSET(fd, fds_write)))
poll_fds[ix].events |= (POLLWRNORM|POLLOUT);
if (fds_excep && (0 != FD_ISSET(fd, fds_excep)))
poll_fds[ix].events |= (POLLRDBAND|POLLPRI);
if (poll_fds[ix].events) {
poll_fds[ix].fd = fd;
poll_fds[ix].revents = 0;
ix++;
}
}
}
do {
if (timeout_ms < 0)
pending_ms = -1;
r = poll(poll_fds, poll_nfds, pending_ms);
} while ((r == -1) && (error = SOCKERRNO) &&
(error != EINVAL) && error_not_EINTR &&
((timeout_ms < 0) || ((pending_ms = timeout_ms - elapsed_ms) > 0)));
if (r < 0)
ret = -1;
if (r > 0) {
ix = poll_nfds;
while (ix--) {
if (poll_fds[ix].revents & POLLNVAL) {
SET_SOCKERRNO(EBADF);
ret = -1;
break;
}
}
}
if (!ret) {
ix = poll_nfds;
while (ix--) {
if (fds_read && (0 != FD_ISSET(poll_fds[ix].fd, fds_read))) {
if (0 == (poll_fds[ix].revents & (POLLRDNORM|POLLERR|POLLHUP|POLLIN)))
FD_CLR(poll_fds[ix].fd, fds_read);
else
ret++;
}
if (fds_write && (0 != FD_ISSET(poll_fds[ix].fd, fds_write))) {
if (0 == (poll_fds[ix].revents & (POLLWRNORM|POLLERR|POLLHUP|POLLOUT)))
FD_CLR(poll_fds[ix].fd, fds_write);
else
ret++;
}
if (fds_excep && (0 != FD_ISSET(poll_fds[ix].fd, fds_excep))) {
if (0 == (poll_fds[ix].revents & (POLLRDBAND|POLLERR|POLLHUP|POLLPRI)))
FD_CLR(poll_fds[ix].fd, fds_excep);
else
ret++;
}
}
}
if (poll_fds && (poll_nfds > SMALL_POLLNFDS))
free(poll_fds);
#else /* HAVE_POLL_FINE */
VERIFY_NFDS(nfds);
ptimeout = (timeout_ms < 0) ? NULL : &pending_tv;
do {
if (ptimeout) {
pending_tv.tv_sec = pending_ms / 1000;
pending_tv.tv_usec = (pending_ms % 1000) * 1000;
}
r = select(nfds, fds_read, fds_write, fds_excep, ptimeout);
} while ((r == -1) && (error = SOCKERRNO) &&
(error != EINVAL) && (error != EBADF) && error_not_EINTR &&
((timeout_ms < 0) || ((pending_ms = timeout_ms - elapsed_ms) > 0)));
if (r < 0)
ret = -1;
else
ret = r;
#endif /* HAVE_POLL_FINE */
return ret;
}
void delay_ms(int ms)
{
wait_ms(ms);
}
/*
* This is an internal function used for waiting for read or write
* events on a pair of file descriptors. It uses poll() when a fine
* poll() is available, in order to avoid limits with FD_SETSIZE,
* otherwise select() is used. An error is returned if select() is
* being used and a file descriptor is too large for FD_SETSIZE.
* A negative timeout value makes this function wait indefinitely,
* unles no valid file descriptor is given, when this happens the
* negative timeout is ignored and the function times out immediately.
* When compiled with CURL_ACKNOWLEDGE_EINTR defined, EINTR condition
* is honored and function might exit early without awaiting timeout,
* otherwise EINTR will be ignored.
*
* Return values:
* -1 = system call error or fd >= FD_SETSIZE
* 0 = timeout
* CURL_CSELECT_IN | CURL_CSELECT_OUT | CURL_CSELECT_ERR
*/
int Curl_socket_ready(curl_socket_t readfd, curl_socket_t writefd,
int timeout_ms)
{
#ifdef HAVE_POLL_FINE
struct pollfd pfd[2];
int num;
#else
struct timeval pending_tv;
struct timeval *ptimeout;
fd_set fds_read;
fd_set fds_write;
fd_set fds_err;
curl_socket_t maxfd;
#endif
struct timeval initial_tv = {0,0};
int pending_ms = 0;
int error;
int r;
int ret;
if((readfd == CURL_SOCKET_BAD) && (writefd == CURL_SOCKET_BAD)) {
r = wait_ms(timeout_ms);
return r;
}
/* Avoid initial timestamp, avoid curlx_tvnow() call, when elapsed
time in this function does not need to be measured. This happens
when function is called with a zero timeout or a negative timeout
value indicating a blocking call should be performed. */
if(timeout_ms > 0) {
pending_ms = timeout_ms;
initial_tv = curlx_tvnow();
}
#ifdef HAVE_POLL_FINE
num = 0;
if(readfd != CURL_SOCKET_BAD) {
pfd[num].fd = readfd;
pfd[num].events = POLLRDNORM|POLLIN|POLLRDBAND|POLLPRI;
pfd[num].revents = 0;
num++;
}
if(writefd != CURL_SOCKET_BAD) {
pfd[num].fd = writefd;
pfd[num].events = POLLWRNORM|POLLOUT;
pfd[num].revents = 0;
num++;
}
do {
if(timeout_ms < 0)
pending_ms = -1;
else if(!timeout_ms)
pending_ms = 0;
r = poll(pfd, num, pending_ms);
if(r != -1)
break;
error = SOCKERRNO;
if(error && error_not_EINTR)
break;
if(timeout_ms > 0) {
pending_ms = timeout_ms - elapsed_ms;
if(pending_ms <= 0)
break;
}
} while(r == -1);
if(r < 0)
return -1;
if(r == 0)
return 0;
ret = 0;
num = 0;
if(readfd != CURL_SOCKET_BAD) {
if(pfd[num].revents & (POLLRDNORM|POLLIN|POLLERR|POLLHUP))
ret |= CURL_CSELECT_IN;
if(pfd[num].revents & (POLLRDBAND|POLLPRI|POLLNVAL))
ret |= CURL_CSELECT_ERR;
num++;
}
if(writefd != CURL_SOCKET_BAD) {
if(pfd[num].revents & (POLLWRNORM|POLLOUT))
ret |= CURL_CSELECT_OUT;
if(pfd[num].revents & (POLLERR|POLLHUP|POLLNVAL))
ret |= CURL_CSELECT_ERR;
}
return ret;
#else /* HAVE_POLL_FINE */
FD_ZERO(&fds_err);
maxfd = (curl_socket_t)-1;
FD_ZERO(&fds_read);
if(readfd != CURL_SOCKET_BAD) {
VERIFY_SOCK(readfd);
FD_SET(readfd, &fds_read);
FD_SET(readfd, &fds_err);
maxfd = readfd;
}
FD_ZERO(&fds_write);
if(writefd != CURL_SOCKET_BAD) {
VERIFY_SOCK(writefd);
FD_SET(writefd, &fds_write);
FD_SET(writefd, &fds_err);
if(writefd > maxfd)
maxfd = writefd;
}
ptimeout = (timeout_ms < 0) ? NULL : &pending_tv;
do {
if(timeout_ms > 0) {
pending_tv.tv_sec = pending_ms / 1000;
pending_tv.tv_usec = (pending_ms % 1000) * 1000;
}
else if(!timeout_ms) {
pending_tv.tv_sec = 0;
pending_tv.tv_usec = 0;
}
r = select((int)maxfd + 1, &fds_read, &fds_write, &fds_err, ptimeout);
if(r != -1)
break;
error = SOCKERRNO;
if(error && error_not_EINTR)
break;
if(timeout_ms > 0) {
pending_ms = timeout_ms - elapsed_ms;
if(pending_ms <= 0)
break;
}
} while(r == -1);
if(r < 0)
return -1;
if(r == 0)
return 0;
ret = 0;
if(readfd != CURL_SOCKET_BAD) {
if(FD_ISSET(readfd, &fds_read))
ret |= CURL_CSELECT_IN;
if(FD_ISSET(readfd, &fds_err))
ret |= CURL_CSELECT_ERR;
}
if(writefd != CURL_SOCKET_BAD) {
if(FD_ISSET(writefd, &fds_write))
ret |= CURL_CSELECT_OUT;
if(FD_ISSET(writefd, &fds_err))
ret |= CURL_CSELECT_ERR;
}
return ret;
#endif /* HAVE_POLL_FINE */
}
static void usb_hub_port_connect_change(struct usb_hub *hubstate, int port,
u16 portstatus, u16 portchange)
{
struct usb_device *hub = hubstate->dev;
struct usb_device *dev;
unsigned int delay = HUB_SHORT_RESET_TIME;
int i;
dbg("port %d, portstatus %x, change %x, %s",
port + 1, portstatus, portchange, portspeed (portstatus));
/* Clear the connection change status */
usb_clear_port_feature(hub, port + 1, USB_PORT_FEAT_C_CONNECTION);
/* Disconnect any existing devices under this port */
if (hub->children[port])
usb_disconnect(&hub->children[port]);
/* Return now if nothing is connected */
if (!(portstatus & USB_PORT_STAT_CONNECTION)) {
if (portstatus & USB_PORT_STAT_ENABLE)
usb_hub_port_disable(hub, port);
return;
}
if (usb_hub_port_debounce(hub, port) < 0) {
err("connect-debounce failed, port %d disabled", port+1);
usb_hub_port_disable(hub, port);
return;
}
down(&usb_address0_sem);
for (i = 0; i < HUB_PROBE_TRIES; i++) {
struct usb_device *pdev;
int len;
/* Allocate a new device struct */
dev = usb_alloc_dev(hub, hub->bus);
if (!dev) {
err("couldn't allocate usb_device");
break;
}
/* Reset the device */
if (usb_hub_port_reset(hub, port, dev, delay)) {
usb_free_dev(dev);
break;
}
/* Find a new device ID for it */
usb_connect(dev);
/* Set up TT records, if needed */
if (hub->tt) {
dev->tt = hub->tt;
dev->ttport = hub->ttport;
} else if (dev->speed != USB_SPEED_HIGH
&& hub->speed == USB_SPEED_HIGH) {
dev->tt = &hubstate->tt;
dev->ttport = port + 1;
}
/* Save readable and stable topology id, distinguishing
* devices by location for diagnostics, tools, etc. The
* string is a path along hub ports, from the root. Each
* device's id will be stable until USB is re-cabled, and
* hubs are often labeled with these port numbers.
*
* Initial size: ".NN" times five hubs + NUL = 16 bytes max
* (quite rare, since most hubs have 4-6 ports).
*/
pdev = dev->parent;
if (pdev->devpath [0] != '0') /* parent not root? */
len = snprintf (dev->devpath, sizeof dev->devpath,
"%s.%d", pdev->devpath, port + 1);
/* root == "0", root port 2 == "2", port 3 that hub "2.3" */
else
len = snprintf (dev->devpath, sizeof dev->devpath,
"%d", port + 1);
if (len == sizeof dev->devpath)
warn ("devpath size! usb/%03d/%03d path %s",
dev->bus->busnum, dev->devnum, dev->devpath);
info("new USB device %s-%s, assigned address %d",
dev->bus->bus_name, dev->devpath, dev->devnum);
/* !!TB - fix by lly: check for devices running slower than they could */
if (dev->speed == USB_SPEED_FULL || dev->speed == USB_SPEED_LOW)
{
dbg("USB 1.1 device - waiting 20ms");
wait_ms(20);
}
/* Run it through the hoops (find a driver, etc) */
if (!usb_new_device(dev)) {
hub->children[port] = dev;
goto done;
}
/* Free the configuration if there was an error */
usb_free_dev(dev);
/* Switch to a long reset time */
delay = HUB_LONG_RESET_TIME;
}
usb_hub_port_disable(hub, port);
done:
up(&usb_address0_sem);
}
void angle_na_calibration(int dir)
{
adns6010_encoders_t adns_zero;
adns6010_encoders_t adns_sample;
double heading;
int offset;
int nsamples = 20;
double samples[6];
int i,j,k;
const int16_t timings[6] = { 100, 200, 300, 400, 500, 600};
// Zero compass
compass_set_heading_rad(&compass,0.0);
adns6010_encoders_get_value(&adns_zero);
NOTICE(0,"ADNS zero values = (%ld,%ld,%ld,%ld,%ld,%ld)",
adns_zero.vectors[0],adns_zero.vectors[1],adns_zero.vectors[2],
adns_zero.vectors[3],adns_zero.vectors[4],adns_zero.vectors[5]);
for(i=0;i<6;i++)
{
NOTICE(0,"Robot turns for %d ms",timings[i]);
setmotors_rotation(dir);
wait_ms(timings[i]);
setmotors_rotation(0);
wait_ms(1000);
for(k=0;k<6;k++)
samples[k] = 0.0;
for(j=0;j<nsamples;j++)
{
wait_ms(1);
adns6010_encoders_get_value(&adns_sample);
for(k=0;k<6;k++)
samples[k] += adns_sample.vectors[k] - adns_zero.vectors[k];
}
offset = (dir<0)?1:0;
for(k=0;k<6;k++)
{
DEBUG(0,"saving cal_data[%d][%d] = %3.3f",i+offset*6,k,samples[k]);
samples[k] /= nsamples;
calibration_data[i+offset*6][k] = samples[k];
}
NOTICE(0,"ADNS values : (%3.1f,%3.1f,%3.1f,%3.1f,%3.1f,%3.1f)",
samples[0],samples[1],samples[2],
samples[3],samples[4],samples[5]);
NOTICE(0,"P(%d,%d), key to go",dir,i);
// set ADNS6010 zero
adns6010_encoders_get_value(&adns_zero);
while(!cli_getkey());
}
}
/******************************************************************************
t_rsto()
asserts reset to target for 50ms then waits 300ms to return.
******************************************************************************/
void t_rsto(void){
tRSTO = 1; // assert RESET signal
wait_ms(50); // wait 50ms
tRSTO = 0; // de-assert RESET signal
wait_ms(300); // wait 300ms for reset pin to go high
}
int filterBack()
{
while (z < 500) {
z++;
grnLED = 0;
wait_ms(1);
filter_IN1= 1;
filter_IN2= 0;
filter_IN3= 0;
filter_IN4= 1;
wait_ms(mDelay);
filter_IN1= 1;
filter_IN2= 0;
filter_IN3= 0;
filter_IN4= 0;
wait_ms(mDelay);
filter_IN1= 1;
filter_IN2= 1;
filter_IN3= 0;
filter_IN4= 0;
wait_ms(mDelay);
filter_IN1= 0;
filter_IN2= 1;
filter_IN3= 0;
filter_IN4= 0;
wait_ms(mDelay);
filter_IN1= 0;
filter_IN2= 1;
filter_IN3= 1;
filter_IN4= 0;
wait_ms(mDelay);
filter_IN1= 0;
filter_IN2= 0;
filter_IN3= 1;
filter_IN4= 0;
wait_ms(mDelay);
filter_IN1= 0;
filter_IN2= 0;
filter_IN3= 1;
filter_IN4= 1;
wait_ms(mDelay);
filter_IN1= 0;
filter_IN2= 0;
filter_IN3= 0;
filter_IN4= 1;
wait_ms(mDelay);
filter_IN1= 0;
filter_IN2= 0;
filter_IN3= 0;
filter_IN4= 0;
}
z = 0;
return(0);
}
bool process_record_user(uint16_t keycode, keyrecord_t *record) {
switch (keycode) {
case QWERTY:
if (record->event.pressed) {
#ifdef AUDIO_ENABLE
PLAY_NOTE_ARRAY(tone_qwerty, false, 0);
#endif
persistant_default_layer_set(1UL<<_QWERTY);
}
return false;
break;
case COLEMAK:
if (record->event.pressed) {
#ifdef AUDIO_ENABLE
PLAY_NOTE_ARRAY(tone_colemak, false, 0);
#endif
persistant_default_layer_set(1UL<<_COLEMAK);
}
return false;
break;
case DVORAK:
if (record->event.pressed) {
#ifdef AUDIO_ENABLE
PLAY_NOTE_ARRAY(tone_dvorak, false, 0);
#endif
persistant_default_layer_set(1UL<<_DVORAK);
}
return false;
break;
case LOWER:
if (record->event.pressed) {
layer_on(_LOWER);
update_tri_layer(_LOWER, _RAISE, _ADJUST);
} else {
layer_off(_LOWER);
update_tri_layer(_LOWER, _RAISE, _ADJUST);
}
return false;
break;
case RAISE:
if (record->event.pressed) {
layer_on(_RAISE);
update_tri_layer(_LOWER, _RAISE, _ADJUST);
} else {
layer_off(_RAISE);
update_tri_layer(_LOWER, _RAISE, _ADJUST);
}
return false;
break;
case BACKLIT:
if (record->event.pressed) {
register_code(KC_RSFT);
#ifdef BACKLIGHT_ENABLE
backlight_step();
#endif
} else {
unregister_code(KC_RSFT);
}
return false;
break;
case PLOVER:
if (record->event.pressed) {
#ifdef AUDIO_ENABLE
stop_all_notes();
PLAY_NOTE_ARRAY(tone_plover, false, 0);
#endif
layer_off(_RAISE);
layer_off(_LOWER);
layer_off(_ADJUST);
layer_on(_PLOVER);
if (!eeconfig_is_enabled()) {
eeconfig_init();
}
keymap_config.raw = eeconfig_read_keymap();
keymap_config.nkro = 1;
eeconfig_update_keymap(keymap_config.raw);
}
return false;
break;
case EXT_PLV:
if (record->event.pressed) {
#ifdef AUDIO_ENABLE
PLAY_NOTE_ARRAY(tone_plover_gb, false, 0);
#endif
layer_off(_PLOVER);
}
return false;
break;
case DFU:
if (record->event.pressed) {
clear_keyboard();
#if defined(MIDI_ENABLE) && defined(MIDI_BASIC)
process_midi_all_notes_off();
#endif
#if defined(AUDIO_ENABLE) && !defined(NO_MUSIC_MODE)
music_all_notes_off();
uint16_t timer_start = timer_read();
PLAY_NOTE_ARRAY(tone_goodbye, false, 0);
shutdown_user();
while(timer_elapsed(timer_start) < 250)
wait_ms(1);
stop_all_notes();
#else
wait_ms(250);
#endif
// this is also done later in bootloader.c - not sure if it's neccesary here
#ifdef BOOTLOADER_CATERINA
*(uint16_t *)0x0800 = 0x7777; // these two are a-star-specific
#endif
bootloader_jump();
}
return false;
break;
}
return true;
}
void startup_user()
{
rgblight_mode(RGB_MATRIX_CYCLE_ALL);
wait_ms(20); // gets rid of tick
PLAY_NOTE_ARRAY(tone_startup, false, 0);
}
/*
* Send the requested file.
*/
static void sendtftp(struct testcase *test, struct formats *pf)
{
int size;
ssize_t n;
/* This is volatile to live through a siglongjmp */
volatile unsigned short sendblock; /* block count */
struct tftphdr *sdp; /* data buffer */
struct tftphdr *sap; /* ack buffer */
sendblock = 1;
#if defined(HAVE_ALARM) && defined(SIGALRM)
mysignal(SIGALRM, timer);
#endif
sdp = r_init();
sap = &ackbuf.hdr;
do {
size = readit(test, &sdp, pf->f_convert);
if (size < 0) {
nak(errno + 100);
return;
}
sdp->th_opcode = htons((unsigned short)opcode_DATA);
sdp->th_block = htons(sendblock);
timeout = 0;
#ifdef HAVE_SIGSETJMP
(void) sigsetjmp(timeoutbuf, 1);
#endif
if(test->writedelay) {
logmsg("Pausing %d seconds before %d bytes", test->writedelay,
size);
wait_ms(1000*test->writedelay);
}
send_data:
if (swrite(peer, sdp, size + 4) != size + 4) {
logmsg("write");
return;
}
read_ahead(test, pf->f_convert);
for ( ; ; ) {
#ifdef HAVE_ALARM
alarm(rexmtval); /* read the ack */
#endif
n = sread(peer, &ackbuf.storage[0], sizeof(ackbuf.storage));
#ifdef HAVE_ALARM
alarm(0);
#endif
if(got_exit_signal)
return;
if (n < 0) {
logmsg("read: fail");
return;
}
sap->th_opcode = ntohs((unsigned short)sap->th_opcode);
sap->th_block = ntohs(sap->th_block);
if (sap->th_opcode == opcode_ERROR) {
logmsg("got ERROR");
return;
}
if (sap->th_opcode == opcode_ACK) {
if (sap->th_block == sendblock) {
break;
}
/* Re-synchronize with the other side */
(void) synchnet(peer);
if (sap->th_block == (sendblock-1)) {
goto send_data;
}
}
}
sendblock++;
} while (size == SEGSIZE);
}
static void expect(FIL *file){
enum {
INIT,
BEFORE_SEND,
SENDING,
SENDING_ESCAPE,
SENDING_ESCAPE_HEX1,
SENDING_ESCAPE_HEX2,
BEFORE_EXPECT,
EXPECTING,
EXPECT_FAILED,
EXPECT_TIMEOUT,
BEFORE_WAIT,
WAITING,
IGNORE_LINE,
} state = INIT;
char c;
char buf[8];
u8 buf_length, expect_index;
u16 read_size;
telemeter_ready = 0;
while((f_read(file, &c, sizeof(c), &read_size) == FR_OK)
&& (read_size > 0)){
unsigned char is_endline = ((c == '\r') || (c == '\n'));
switch(state){
case INIT:
if(c == '$'){
while(uart1_read(buf, sizeof(buf)) > 0); // clear RX buffer
state = BEFORE_SEND;
}else if(c == '>'){
state = BEFORE_EXPECT;
buf_length = expect_index = 0;
do{
u8 timeout = FALSE;
timeout_10ms = 0;
while(!uart1_read(&c, sizeof(c))){
if(timeout_10ms >= 100){
timeout = TRUE;
break;
}
}
if(timeout){
buf_length = 0;
break;
}else if(c == '\r' || c == '\n'){
if(buf_length > 0){break;}
}else if(buf_length < sizeof(buf)){
buf[buf_length++] = c;
}
}while(1);
}else if(c == '@'){
state = BEFORE_WAIT;
}else if(!isspace(c)){
state = IGNORE_LINE;
}
break;
case BEFORE_SEND:
if(isspace(c)){
if(is_endline){state = INIT;}
break;
}
state = SENDING;
case SENDING:
if(is_endline){
state = INIT;
}else if(c == '\\'){ // escape char
state = SENDING_ESCAPE;
}else{
uart1_write(&c, sizeof(c));
}
break;
case SENDING_ESCAPE:
if(c == 'x'){
state = SENDING_ESCAPE_HEX1;
break;
}
switch(c){
case 'r': c = '\r'; break;
case 'n': c = '\n'; break;
case '\\': c = '\\'; break;
}
uart1_write(&c, sizeof(c));
state = SENDING;
break;
case SENDING_ESCAPE_HEX1:
buf[0] = ((c & 0x0F) + (c >= 'A' ? 10 : 0)) << 4;
state = SENDING_ESCAPE_HEX2;
break;
case SENDING_ESCAPE_HEX2:
buf[0] += ((c & 0x0F) + (c >= 'A' ? 10 : 0));
uart1_write(buf, 1);
state = SENDING;
break;
case BEFORE_EXPECT:
if(isspace(c)){
if(is_endline){state = INIT;} // match any including timeout(buf_length == 0)
break;
}else if(buf_length == 0){
state = EXPECT_TIMEOUT;
return;
}
state = EXPECTING;
case EXPECTING:
if(is_endline){
state = INIT;
}else if(expect_index < buf_length){
if(c != buf[expect_index++]){
state = EXPECT_FAILED;
return;
}
}
break;
case BEFORE_WAIT:
if(isspace(c)){
if(is_endline){state = INIT;}
break;
}
buf_length = 0;
state = WAITING;
case WAITING:
if(is_endline){
buf[buf_length] = '\0';
wait_ms((unsigned int)atol(buf));
state = INIT;
}else if(buf_length < (sizeof(buf) - 1)){
buf[buf_length++] = c;
}
break;
case IGNORE_LINE:
if(is_endline){state = INIT;}
break;
}
}
telemeter_ready = 1;
}
int main(void)
{
int i7, i3, max;
//int16_t capt[16]; /* variable recevant la valeur venant de l'adc */
//int16_t i; /* */
// int16_t val;
char ret;
uint16_t i15,i75;
DDRB=0xFF; /* PORT B en sortie */
DDRA=0x00; /* PORT A en entrée */
PORTB=0xFF; /* LEDS éteintes */
uart_init(); /* initialisation de l'UART, si besoin de communication uart */
/* truc bizarre pour afficher les printf par uart */
/* NDJD: C'est pas bizarre, c'est des pointeurs de fctns... */
fdevopen( uart0_send, uart0_recv);
sei(); /* Autorisation des interruptions */
adc_init(); /* Initialisation de l'adc, si utilisation de l'adc */
gp2d12 g;
printf("\n\n\n");
printf("Programme d'etalonnage du module GP2D12.\n");
do
{
printf("Sur quel port est situe le GP2 ? (1 à 7)\n");
ret = uart0_recv();
printf("%d\n",ret);
}
while( (ret<'1') || (ret>'7') );
g.pin = ret - 48;
printf("Pin %d choisi.\n",g.pin);
printf("\nMettre un obstacle à 15cm du GP2.\n");
printf("Then press a touche.\n");
while( !uart0_recv() )
wait_ms(100);
i15 = gp2d12_adc(&g,100,1);
printf("\nMettre un obstacle à 75cm du GP2.\n");
printf("Then press a touche.\n");
while( !uart0_recv() )
wait_ms(100);
i75 = gp2d12_adc(&g,100,1);
printf("\nMettre un obstacle a 7 cm\n");
printf("Then press a touche.\n");
while( !uart0_recv() )
wait_ms(100);
i7 = gp2d12_adc(&g,100,1);
printf("\nMettre un obstacle a 3 cm\n");
printf("Then press a touche.\n");
while( !uart0_recv() )
wait_ms(100);
i3 = gp2d12_adc(&g,100,1);
printf("\nC'est le moment d'avoir le max\n");
printf("Avancer l'obstacle lentement de 30 a 0 cm plusieurs fois\n");
printf("Then press a touche.\n");
g.max=0;
r=0;
uart0_register_rx_event(recu);
while(!r)
{
printf("%d \r",g.max);
max=gp2d12_adc(&g,100,1);
if (g.max <max)
{
g.max=max;
}
}
//--------------
//printf("i15=%u i75=%u\n\n",i15,i75);
gp2d12_etalon(&g,(double)i15,(double)i75,(double) i3, (double) i7);
printf("\nResultats :\n pin=%u \n a=%f\n b=%f\n ac=%f \n bc=%f \n ",g.pin,g.a,g.b,g.ac,g.bc);
printf("max=%d \n zone=0 \n zonei=0 \n cmp=0 \n\n ",g.max);
g.zone=0;
g.zonei=0;
g.cmpt=0;
while(1)
{
gp2d12_update(&g);
printf("\r zone : %d, zonei : %d, distance : %5.d cm",g.zone,g.zonei,g.d);
PORTB++;
wait_ms(1);
}
return 0;
}
void ledDisplay::draw_pattern(int pattern)
{
switch(pattern) {
case 5: {
int i=0;
do {
cs = 0;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_a[k+1]);
}
wait_ms(ms);
cs = 1;
wait(wt);
i++;
} while(i<freq);
break;
}
case 6: {
int i=0;
do {
cs = 0;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_b[k+1]);
}
wait_ms(ms);
cs = 1;
wait(wt);
i++;
} while(i<freq);
break;
}
case 7: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_c[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 8: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_d[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 9: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_e[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 10: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_f[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 11: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_g[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 12: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_h[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 13: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_i[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 14: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_j[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 15: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_k[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 16: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_l[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 17: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_m[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 18: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_n[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 19: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_o[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 20: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_p[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 21: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_q[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 22: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_r[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 23: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_s[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 24: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_t[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 25: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_u[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 26: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_v[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 27: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_w[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 28: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_x[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 29: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_y[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 30: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_z[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
case 31: {
int i=0;
do {
cs = 0;
//cs1 = 0;
////////myled = 1;
wait_ms(ms);
for(int k=0; k<65; k++) {
spi.write(letter_blank[k+1]);
//spi1.write(image[k+1]);
}
wait_ms(ms);
cs = 1;
////////myled=0;
wait(wt);
i++;
} while(i<freq);
break;
}
}//ACTUAL END
}
void IconMenu(void) {
unsigned char *videosavepage;
u32 COUNT_start;
u32 temp=1;
ICON *iconPtr=NULL;
int i;
extern int nTempCursorMbrX, nTempCursorMbrY;
int nTempCursorResumeX, nTempCursorResumeY ;
int nTempCursorX, nTempCursorY;
int nModeDependentOffset=(vmode.width-640)/2;
nTempCursorResumeX=nTempCursorMbrX;
nTempCursorResumeY=nTempCursorMbrY;
nTempCursorX=VIDEO_CURSOR_POSX;
nTempCursorY=vmode.height-80;
// We save the complete framebuffer to memory (we restore at exit)
videosavepage = malloc(FB_SIZE);
memcpy(videosavepage,(void*)FB_START,FB_SIZE);
VIDEO_CURSOR_POSX=((252+nModeDependentOffset)<<2);
VIDEO_CURSOR_POSY=nTempCursorY-100;
#ifndef SILENT_MODE
//In silent mode, don't draw the menu the first time.
//If we get a left/right xpad event, it will be registered,
//and the menu will 'appear'. Otherwise, it will proceed quietly
//and boot the default boot item
VIDEO_ATTR=0xffc8c8c8;
printk("Select from Menu\n");
VIDEO_ATTR=0xffffffff;
IconMenuDraw(nModeDependentOffset, nTempCursorY);
#endif
COUNT_start = IoInputDword(0x8008);
//Main menu event loop.
while(1)
{
int changed=0;
wait_ms(75);
if (risefall_xpad_BUTTON(TRIGGER_XPAD_PAD_RIGHT) == 1)
{
if (selectedIcon->nextIcon!=NULL) {
//A bit ugly, but need to find the last visible icon, and see if
//we are moving further right from it.
lastVisibleIcon=firstVisibleIcon;
for (i=0; i<2; i++) {
if (lastVisibleIcon->nextIcon==NULL) {
break;
}
lastVisibleIcon = lastVisibleIcon->nextIcon;
}
if (selectedIcon == lastVisibleIcon) {
//We are moving further right, so slide all the icons along.
if(lastVisibleIcon->nextIcon != NULL) {
firstVisibleIcon = firstVisibleIcon->nextIcon;
}
//As all the icons have moved, we need to refresh the entire page.
memcpy((void*)FB_START,videosavepage,FB_SIZE);
}
memcpy((void*)FB_START,videosavepage,FB_SIZE);
selectedIcon = selectedIcon->nextIcon;
changed=1;
}
temp=0;
}
else if (risefall_xpad_BUTTON(TRIGGER_XPAD_PAD_LEFT) == 1)
{
if (selectedIcon->previousIcon!=NULL) {
if (selectedIcon == firstVisibleIcon) {
//We are moving further left, so slide all the icons along.
if(firstVisibleIcon->previousIcon != NULL) {
firstVisibleIcon = firstVisibleIcon->previousIcon;
}
//As all the icons have moved, we need to refresh the entire page.
memcpy((void*)FB_START,videosavepage,FB_SIZE);
}
selectedIcon = selectedIcon->previousIcon;
changed=1;
}
temp=0;
}
//If anybody has toggled the xpad left/right, disable the timeout.
if (temp!=0) {
temp = IoInputDword(0x8008) - COUNT_start;
}
if ((risefall_xpad_BUTTON(TRIGGER_XPAD_KEY_A) == 1) || risefall_xpad_BUTTON(TRIGGER_XPAD_KEY_START) == 1 ||
(u32)(temp>(0x369E99*BOOT_TIMEWAIT))) {
memcpy((void*)FB_START,videosavepage,FB_SIZE);
VIDEO_CURSOR_POSX=nTempCursorResumeX;
VIDEO_CURSOR_POSY=nTempCursorResumeY;
if (temp>(0x369E99*BOOT_TIMEWAIT)) timedOut=1;
//Icon selected - invoke function pointer.
if (selectedIcon->functionPtr!=NULL) selectedIcon->functionPtr(selectedIcon->functionDataPtr);
//If we come back to this menu, make sure we are redrawn, and that we replace the saved video page
changed=1;
memcpy((void*)FB_START,videosavepage,FB_SIZE);
}
if (changed) {
BootVideoClearScreen(&jpegBackdrop, nTempCursorY, VIDEO_CURSOR_POSY+1);
IconMenuDraw(nModeDependentOffset, nTempCursorY);
changed=0;
}
}
}
int RadioManagerOld::openRadioAtChannel(int frequencySection,int channel,char searchFlag)
{
if(status ==RM_STATUS_LISTEN || status ==RM_STATUS_SEARCH)
{
if(status ==RM_STATUS_SEARCH)
{
killTimer(timeId);
//printf("[radio][%4d] ~~~~~~~~~~~~~~~~~searchEnd cause[%d] \n", __LINE__, RM_SEARCH_R_CANCEL);
emit searchChannelChanged(currentSearchChannel,0);
emit searchEnd(RM_SEARCH_R_CANCEL);
}
status =RM_STATUS_OPENED;
}
printf("-++++++++++++++ open Radio start +++++++++++++++-\n");
int rssi;
int snr;
if(frequencySection==RM_FREQ_SECTION_FM)
{
rssi = 20;
snr = 3;
readParameterConfig(RM_FREQ_SECTION_FM, &rssi, &snr);
si47xxAMRX_powerdown();
printf("------->[%s] FM rssi:%d snr:%d +++++++++++++++-\n",__func__,rssi,snr);
si47xxFMRX_initialize( rssi, snr );
si47xxFMRX_set_volume(30); // full volume, turn off mute
}
else
{
rssi = 25;
snr = 5;
readParameterConfig(RM_FREQ_SECTION_AM, &rssi, &snr);
si47xxFMRX_powerdown();
printf("------->[%s] AM rssi:%d snr:%d +++++++++++++++-\n",__func__,rssi,snr);
si47xxAMRX_initialize( rssi, snr );
si47xxAMRX_set_volume(30); // full volume, turn off mute
}
wait_ms(RM_SEARCHED_INTERVAL);
if(frequencySection==RM_FREQ_SECTION_FM)
{
freqSection =RM_FREQ_SECTION_FM;
printf("~~~~~~~~~~~~~~~~~~~~~~~~ select one FM chanel ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n");
selectFMChannel(channel,searchFlag);
}
else
{
freqSection =RM_FREQ_SECTION_AM;
printf("~~~~~~~~~~~~~~~~~~~~~~~~ select one AM chanel start ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n");
selectAMChannel(channel,searchFlag );
printf("~~~~~~~~~~~~~~~~~~~~~~~~ select one AM chanel over ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n");
}
status =RM_STATUS_LISTEN;
return RM_R_OK;
}
void shutdown_user()
{
PLAY_NOTE_ARRAY(tone_goodbye, false, 0);
wait_ms(150);
stop_all_notes();
}
/******************************************************************************
Blink_Led()
blinks Status LED
******************************************************************************/
void Blink_Led(byte rate){
for(;;){
STATUS_LED ^= 1; // toggle Status LED
wait_ms(rate); // wait for rate ms
}
}
int main(int argc, char **argv)
{
int i;
int xi = 0;
/* in/out variables */
double f1 = 0.0;
uint32_t f_init = 0; /* in Hz */
uint32_t f_start = 0; /* in Hz */
uint16_t loop_cnt = 0;
int8_t rssi_target_dBm = -80;
uint16_t scan_time_us = 128;
uint32_t timestamp;
uint8_t rssi_value;
int8_t rssi_offset = DEFAULT_SX127X_RSSI_OFFSET;
int32_t val, val2;
int channel;
uint32_t freq_offset;
/* parse command line options */
while ((i = getopt (argc, argv, "h:f:s:r:o:")) != -1) {
switch (i) {
case 'h':
usage();
return EXIT_FAILURE;
break;
case 'f':
i = sscanf(optarg, "%lf", &f1);
if ((i != 1) || (f1 < 30.0) || (f1 > 3000.0)) {
MSG("ERROR: Invalid LBT start frequency\n");
usage();
return EXIT_FAILURE;
} else {
f_start = (uint32_t)((f1*1e6) + 0.5);/* .5 Hz offset to get rounding instead of truncating */
}
break;
case 's':
i = sscanf(optarg, "%i", &xi);
if ((i != 1) || ((xi != 128) && (xi != 5000))) {
MSG("ERROR: scan_time_us must be 128 or 5000 \n");
usage();
return EXIT_FAILURE;
} else {
scan_time_us = xi;
}
break;
case 'r':
i = sscanf(optarg, "%i", &xi);
if ((i != 1) || ((xi < -128) && (xi > 0))) {
MSG("ERROR: rssi_target must be b/w -128 & 0 \n");
usage();
return EXIT_FAILURE;
} else {
rssi_target_dBm = xi;
}
break;
case 'o': /* -o <int> SX127x RSSI offset [-128..127] */
i = sscanf(optarg, "%i", &xi);
if((i != 1) || (xi < -128) || (xi > 127)) {
MSG("ERROR: rssi_offset must be b/w -128 & 127\n");
usage();
return EXIT_FAILURE;
} else {
rssi_offset = (int8_t)xi;
}
break;
default:
MSG("ERROR: argument parsing use -h option for help\n");
usage();
return EXIT_FAILURE;
}
}
MSG("INFO: Starting LoRa Gateway v1.5 LBT test\n");
/* configure signal handling */
sigemptyset(&sigact.sa_mask);
sigact.sa_flags = 0;
sigact.sa_handler = sig_handler;
sigaction(SIGQUIT, &sigact, NULL);
sigaction(SIGINT, &sigact, NULL);
sigaction(SIGTERM, &sigact, NULL);
/* Connect to concentrator */
i = lgw_connect(false, LGW_DEFAULT_NOTCH_FREQ);
if (i != LGW_REG_SUCCESS) {
MSG("ERROR: lgw_connect() did not return SUCCESS\n");
return EXIT_FAILURE;
}
/* Check if FPGA supports LBT */
lgw_fpga_reg_r(LGW_FPGA_FEATURE, &val);
if (TAKE_N_BITS_FROM((uint8_t)val, 2, 1) != true) {
MSG("ERROR: LBT is not supported (0x%x)\n", (uint8_t)val);
return EXIT_FAILURE;
}
/* Get FPGA lowest frequency for LBT channels */
lgw_fpga_reg_r(LGW_FPGA_LBT_INITIAL_FREQ, &val);
switch (val) {
case 0:
f_init = 915000000;
break;
case 1:
f_init = 863000000;
break;
default:
MSG("ERROR: LBT start frequency %d is not supported\n", val);
return EXIT_FAILURE;
}
/* Initialize 1st LBT channel freq if not given by user */
if (f_start == 0) {
f_start = f_init;
} else if (f_start < f_init) {
MSG("ERROR: LBT start frequency %u is not supported (f_init=%u)\n", f_start, f_init);
return EXIT_FAILURE;
}
MSG("FREQ: %u\n", f_start);
/* Configure SX127x and read few RSSI points */
lgw_setup_sx127x(f_init, MOD_FSK, LGW_SX127X_RXBW_100K_HZ, rssi_offset); /* 200KHz LBT channels */
for (i = 0; i < 100; i++) {
lgw_sx127x_reg_r(0x11, &rssi_value); /* 0x11: RegRssiValue */
MSG("SX127x RSSI:%i dBm\n", -(rssi_value/2));
wait_ms(10);
}
/* Configure LBT */
val = -2*(rssi_target_dBm);
lgw_fpga_reg_w(LGW_FPGA_RSSI_TARGET, val);
for (i = 0; i < LBT_CHANNEL_FREQ_NB; i++) {
freq_offset = (f_start - f_init)/100E3 + i*2; /* 200KHz between each channel */
lgw_fpga_reg_w(LGW_FPGA_LBT_CH0_FREQ_OFFSET+i, (int32_t)freq_offset);
if (scan_time_us == 5000) { /* configured to 128 by default */
lgw_fpga_reg_w(LGW_FPGA_LBT_SCAN_TIME_CH0+i, 1);
}
}
lgw_fpga_reg_r(LGW_FPGA_RSSI_TARGET, &val);
MSG("RSSI_TARGET = %d\n", val);
if (val != (-2*rssi_target_dBm)) {
MSG("ERROR: failed to read back RSSI target register value\n");
return EXIT_FAILURE;
}
for (i = 0; i < LBT_CHANNEL_FREQ_NB; i++) {
lgw_fpga_reg_r(LGW_FPGA_LBT_CH0_FREQ_OFFSET+i, &val);
lgw_fpga_reg_r(LGW_FPGA_LBT_SCAN_TIME_CH0+i, &val2);
MSG("CH_%i: freq=%u (offset=%i), scan_time=%u (%i)\n", i, (uint32_t)((val*100E3)+f_init), val, (val2==1)?5000:128, val2);
}
lgw_fpga_reg_r(LGW_FPGA_VERSION, &val);
MSG("FPGA VERSION = %d\n", val);
/* Enable LBT FSM */
lgw_fpga_reg_w(LGW_FPGA_CTRL_FEATURE_START, 1);
/* Start test */
while ((quit_sig != 1) && (exit_sig != 1)) {
MSG("~~~~\n");
for (channel = 0; channel < LBT_CHANNEL_FREQ_NB; channel++) {
/* Select LBT channel */
lgw_fpga_reg_w(LGW_FPGA_LBT_TIMESTAMP_SELECT_CH, channel);
/* Get last instant when the selected channel was free */
lgw_fpga_reg_r(LGW_FPGA_LBT_TIMESTAMP_CH, &val);
timestamp = (uint32_t)(val & 0x0000FFFF) * 256; /* 16bits (1LSB = 256µs) */
MSG(" TIMESTAMP_CH%u = %u\n", channel, timestamp);
}
loop_cnt += 1;
wait_ms(400);
}
/* close SPI link */
i = lgw_disconnect();
if (i != LGW_REG_SUCCESS) {
MSG("ERROR: lgw_disconnect() did not return SUCCESS\n");
return EXIT_FAILURE;
}
MSG("INFO: Exiting LoRa Gateway v1.5 LBT test successfully\n");
return EXIT_SUCCESS;
}
int closeTray()
{
while (z < 500) {
z++;
grnLED = 1;
wait_ms(1);
cuvette_IN1= 1;
cuvette_IN2= 0;
cuvette_IN3= 0;
cuvette_IN4= 1;
wait_ms(mDelay);
cuvette_IN1= 1;
cuvette_IN2= 0;
cuvette_IN3= 0;
cuvette_IN4= 0;
wait_ms(mDelay);
cuvette_IN1= 1;
cuvette_IN2= 1;
cuvette_IN3= 0;
cuvette_IN4= 0;
wait_ms(mDelay);
cuvette_IN1= 0;
cuvette_IN2= 1;
cuvette_IN3= 0;
cuvette_IN4= 0;
wait_ms(mDelay);
cuvette_IN1= 0;
cuvette_IN2= 1;
cuvette_IN3= 1;
cuvette_IN4= 0;
wait_ms(mDelay);
cuvette_IN1= 0;
cuvette_IN2= 0;
cuvette_IN3= 1;
cuvette_IN4= 0;
wait_ms(mDelay);
cuvette_IN1= 0;
cuvette_IN2= 0;
cuvette_IN3= 1;
cuvette_IN4= 1;
wait_ms(mDelay);
cuvette_IN1= 0;
cuvette_IN2= 0;
cuvette_IN3= 0;
cuvette_IN4= 1;
wait_ms(mDelay);
cuvette_IN1= 0;
cuvette_IN2= 0;
cuvette_IN3= 0;
cuvette_IN4= 0;
}
trayOpen = 0;
z = 0;
return(0);
}
int main(int argc, char **argv)
{
int i; /* temporary variable(s) */
int32_t reg_val;
/* user chosen parameters */
double f1 = 0.0;
double f2 = 0.0;
double fs = 0.0;
/* frequency scan parameters (default values) */
uint32_t f_start = rf_rx_lowfreq_[RF_CHAIN]; /* in Hz */
uint32_t f_stop = rf_rx_upfreq_[RF_CHAIN]; /* in Hz */
uint32_t f_step = 200000; /* 200 kHz step by default */
/* RSSI measurement results */
int8_t rssi_max; /* max RSSI during X measurements */
int high_count; /* nb of measurements above a threshold defined by maximum RSSI */
/* clock, log file and log rotation management */
FILE * log_file = NULL;
char log_file_name[64];
char iso_date[20];
time_t now_time;
/* variables for PLL register calculation */
uint32_t f_target; /* loop variable */
uint32_t freq_hz;
uint32_t part_int;
uint32_t part_frac;
int cpt_attempts = 0;
/* parse command line options */
while ((i = getopt (argc, argv, "hf:")) != -1) {
switch (i) {
case 'h':
usage();
return EXIT_SUCCESS;
case 'f':
sscanf(optarg, "%lf:%lf:%lf", &f1, &f2, &fs);
/* check configuration sanity */
if (f2 < f1) {
MSG("ERROR: stop frequency must be bigger than start frequency\n");
return EXIT_FAILURE;
}
if ((f1 < 30.0) || (f1 > 3000.0)) {
MSG("ERROR: invalid start frequency %f MHz\n", f1);
return EXIT_FAILURE;
}
if ((f2 < 30.0) || (f2 > 3000.0)) {
MSG("ERROR: invalid stop frequency %f MHz\n", f2);
return EXIT_FAILURE;
}
f_start = (uint32_t)((f1*1e6) + 0.5); /* .5 Hz offset to get rounding instead of truncating */
f_stop = (uint32_t)((f2*1e6) + 0.5);
if (fs > 0.01) {
f_step = (uint32_t)((fs*1e6) + 0.5);
}
break;
default:
MSG("ERROR: argument parsing use -h option for help\n");
usage();
return EXIT_FAILURE;
}
}
printf("Scanning from %u Hz to %u Hz with a %u Hz frequency step\n", f_start, f_stop, f_step);
/* configure signal handling */
sigemptyset(&sigact.sa_mask);
sigact.sa_flags = 0;
sigact.sa_handler = sig_handler;
sigaction(SIGQUIT, &sigact, NULL);
sigaction(SIGINT, &sigact, NULL);
sigaction(SIGTERM, &sigact, NULL);
/* establish connection with concentrator and reset it */
if (lgw_connect() == LGW_REG_ERROR) {
MSG("ERROR: fail to connect to concentrator board\n");
return EXIT_FAILURE;
}
lgw_soft_reset();
/* Ungate concentrator clock, switch on the radios and reset them */
lgw_reg_w(LGW_GLOBAL_EN, 1);
lgw_reg_w(LGW_RADIO_A_EN,1);
lgw_reg_w(LGW_RADIO_B_EN,1);
wait_ms(500);
lgw_reg_w(LGW_RADIO_RST,1);
wait_ms(5);
lgw_reg_w(LGW_RADIO_RST,0);
wait_ms(5);
/* enter basic parameters for the radio */
sx125x_write_(RF_CHAIN, 0x10, SX125x_TX_DAC_CLK_SEL + SX125x_CLK_OUT*2);
sx125x_write_(RF_CHAIN, 0x0C, 0 + SX125x_RX_BB_GAIN*2 + SX125x_RX_LNA_GAIN*32); /* not required, firmware should take care of that */
sx125x_write_(RF_CHAIN, 0x0D, SX125x_RXBB_BW + SX125x_RX_ADC_TRIM*4 + SX125x_RX_ADC_BW*32);
/* configure the IF and concentrator parameters */
lgw_reg_w(LGW_IF_FREQ_0, -282); /* default -384 */
lgw_reg_w(LGW_IF_FREQ_1, -128); /* default -128 */
lgw_reg_w(LGW_RSSI_BB_FILTER_ALPHA,9); /* default 7 */
lgw_reg_w(LGW_RSSI_DEC_FILTER_ALPHA,7); /* default 5 */
lgw_reg_w(LGW_RSSI_CHANN_FILTER_ALPHA,3); /* default 8 */
lgw_reg_w(LGW_RSSI_CHANN_DEFAULT_VALUE,90); /* default 100 */
lgw_reg_w(LGW_RSSI_DEC_DEFAULT_VALUE,90); /* default 100 */
/* Load firmware */
load_firmware_(MCU_AGC, rssi_firmware, MCU_AGC_FW_BYTE);
lgw_reg_w(LGW_FORCE_HOST_FE_CTRL,0);
lgw_reg_w(LGW_FORCE_DEC_FILTER_GAIN,0);
/* open log file */
time(&now_time);
strftime(iso_date,ARRAY_SIZE(iso_date),"%Y%m%dT%H%M%SZ",gmtime(&now_time)); /* format yyyymmddThhmmssZ */
sprintf(log_file_name, "band_survey_%s.csv", iso_date);
log_file = fopen(log_file_name, "a"); /* create log file, append if file already exist */
if (log_file == NULL) {
MSG("ERROR: impossible to create log file %s\n", log_file_name);
return EXIT_FAILURE;
}
i = fprintf(log_file, "\"Frequency (Hz)\",\"RSSI (dB)\",\"high meas (nb)\"\n");
if (i < 0) {
MSG("ERROR: impossible to write to log file %s\n", log_file_name);
return EXIT_FAILURE;
}
/* main loop */
f_target = f_start;
while ((quit_sig != 1) && (exit_sig != 1) && (f_target <= f_stop)) {
/* set PLL to target frequency */
freq_hz = f_target - MEAS_IF;
#if (CFG_RADIO_1257 == 1)
part_int = freq_hz / (SX125x_32MHz_FRAC << 8); /* integer part, gives the MSB */
part_frac = ((freq_hz % (SX125x_32MHz_FRAC << 8)) << 8) / SX125x_32MHz_FRAC; /* fractional part, gives middle part and LSB */
#elif (CFG_RADIO_1255 == 1)
part_int = freq_hz / (SX125x_32MHz_FRAC << 7); /* integer part, gives the MSB */
part_frac = ((freq_hz % (SX125x_32MHz_FRAC << 7)) << 9) / SX125x_32MHz_FRAC; /* fractional part, gives middle part and LSB */
#endif
sx125x_write_(RF_CHAIN, 0x01,0xFF & part_int); /* Most Significant Byte */
sx125x_write_(RF_CHAIN, 0x02,0xFF & (part_frac >> 8)); /* middle byte */
sx125x_write_(RF_CHAIN, 0x03,0xFF & part_frac); /* Least Significant Byte */
/* start and PLL lock */
cpt_attempts = 0;
do {
if (cpt_attempts >= PLL_LOCK_MAX_ATTEMPTS) {
MSG("ERROR: fail to lock PLL\n");
return -1;
}
sx125x_write_(RF_CHAIN, 0x00, 1); /* enable Xtal oscillator */
sx125x_write_(RF_CHAIN, 0x00, 3); /* Enable RX (PLL+FE) */
++cpt_attempts;
wait_ms(1);
} while((sx125x_read_(RF_CHAIN, 0x11) & 0x02) == 0);
/* give control of the radio to the MCU and get it out of reset */
lgw_reg_w(LGW_FORCE_HOST_RADIO_CTRL,0);
lgw_reg_w(LGW_MCU_RST_1, 0);
/* wait for the firmware to finish running and fetch the result */
do {
wait_ms(1);
lgw_reg_r(LGW_MCU_AGC_STATUS, ®_val);
} while (reg_val != 1);
lgw_reg_w(LGW_DBG_AGC_MCU_RAM_ADDR,0x20);
lgw_reg_r(LGW_DBG_AGC_MCU_RAM_DATA, ®_val);
rssi_max = (int8_t)reg_val;
lgw_reg_w(LGW_DBG_AGC_MCU_RAM_ADDR,0x21);
lgw_reg_r(LGW_DBG_AGC_MCU_RAM_DATA, ®_val);
high_count = reg_val;
lgw_reg_w(LGW_DBG_AGC_MCU_RAM_ADDR,0x22);
lgw_reg_r(LGW_DBG_AGC_MCU_RAM_DATA, ®_val);
high_count += (reg_val << 8);
/* log the measurement */
i = fprintf(log_file, "%u, %i, %u\n", f_target, rssi_max, high_count);
if (i < 0) {
MSG("ERROR: impossible to write to log file %s\n", log_file_name);
return EXIT_FAILURE;
}
/* reset MCU and take back control of radio */
lgw_reg_w(LGW_MCU_RST_1, 1);
lgw_reg_w(LGW_FORCE_HOST_RADIO_CTRL,1);
/* iterate loop */
f_target += f_step;
}
fclose(log_file);
lgw_soft_reset();
lgw_disconnect();
printf("Exiting band survey program\n");
return EXIT_SUCCESS;
}
/*
* WARNING - If a driver calls usb_reset_device, you should simulate a
* disconnect() and probe() for other interfaces you doesn't claim. This
* is left up to the driver writer right now. This insures other drivers
* have a chance to re-setup their interface.
*
* Take a look at proc_resetdevice in devio.c for some sample code to
* do this.
*/
int usb_reset_device(struct usb_device *dev)
{
struct usb_device *parent = dev->parent;
struct usb_device_descriptor *descriptor;
int i, ret, port = -1;
if (!parent) {
err("attempting to reset root hub!");
return -EINVAL;
}
for (i = 0; i < parent->maxchild; i++)
if (parent->children[i] == dev) {
port = i;
break;
}
if (port < 0)
return -ENOENT;
down(&usb_address0_sem);
/* Send a reset to the device */
if (usb_hub_port_reset(parent, port, dev, HUB_SHORT_RESET_TIME)) {
usb_hub_port_disable(parent, port);
up(&usb_address0_sem);
return(-ENODEV);
}
/* Reprogram the Address */
ret = usb_set_address(dev);
if (ret < 0) {
err("USB device not accepting new address (error=%d)", ret);
usb_hub_port_disable(parent, port);
up(&usb_address0_sem);
return ret;
}
/* Let the SET_ADDRESS settle */
wait_ms(10);
up(&usb_address0_sem);
/*
* Now we fetch the configuration descriptors for the device and
* see if anything has changed. If it has, we dump the current
* parsed descriptors and reparse from scratch. Then we leave
* the device alone for the caller to finish setting up.
*
* If nothing changed, we reprogram the configuration and then
* the alternate settings.
*/
descriptor = kmalloc(sizeof *descriptor, GFP_NOIO);
if (!descriptor) {
return -ENOMEM;
}
ret = usb_get_descriptor(dev, USB_DT_DEVICE, 0, descriptor,
sizeof(*descriptor));
if (ret < 0) {
kfree(descriptor);
return ret;
}
le16_to_cpus(&descriptor->bcdUSB);
le16_to_cpus(&descriptor->idVendor);
le16_to_cpus(&descriptor->idProduct);
le16_to_cpus(&descriptor->bcdDevice);
if (memcmp(&dev->descriptor, descriptor, sizeof(*descriptor))) {
kfree(descriptor);
usb_destroy_configuration(dev);
ret = usb_get_device_descriptor(dev);
if (ret < sizeof(dev->descriptor)) {
if (ret < 0)
err("unable to get device descriptor (error=%d)", ret);
else
err("USB device descriptor short read (expected %Zi, got %i)", sizeof(dev->descriptor), ret);
clear_bit(dev->devnum, &dev->bus->devmap.devicemap);
dev->devnum = -1;
return -EIO;
}
ret = usb_get_configuration(dev);
if (ret < 0) {
err("unable to get configuration (error=%d)", ret);
usb_destroy_configuration(dev);
clear_bit(dev->devnum, &dev->bus->devmap.devicemap);
dev->devnum = -1;
return 1;
}
dev->actconfig = dev->config;
usb_set_maxpacket(dev);
return 1;
}
kfree(descriptor);
ret = usb_set_configuration(dev, dev->actconfig->bConfigurationValue);
if (ret < 0) {
err("failed to set active configuration (error=%d)", ret);
return ret;
}
for (i = 0; i < dev->actconfig->bNumInterfaces; i++) {
struct usb_interface *intf = &dev->actconfig->interface[i];
struct usb_interface_descriptor *as = &intf->altsetting[intf->act_altsetting];
ret = usb_set_interface(dev, as->bInterfaceNumber, as->bAlternateSetting);
if (ret < 0) {
err("failed to set active alternate setting for interface %d (error=%d)", i, ret);
return ret;
}
}
return 0;
}
int main() {
//float angle_x, angle_y, angle_z; // for testing
// xbee received data
uint8_t *rx_data;
uint8_t rx_len;
uint8_t msg_type;
uint8_t *msg_data;
uint8_t mode_value;
//send.attach(&sendPackage,0.1); // feedback configuration
// xbee initialize
xbee.baud(57600);
rst = 0;
myled = 0;
wait_ms(1);
rst = 1;
wait_ms(1);
myled = 1;
myled = 0;
pc.baud(57600);
imu_mpu6050.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
imu_mpu6050.setGyroRange(MPU6050_GYRO_RANGE_250);
bool success_flag;
// definition of motor speed
float frontCmdSpeed = 0, backCmdSpeed = 0, leftCmdSpeed = 0, rightCmdSpeed = 0;
float des_roll, des_pitch, des_yaw, des_thrust;
success_flag = imu_quadrotor.testCommunication();
while(!success_flag)
{
myled = 1;
}
flag_recv = 0;
estop.start();
estop.reset();
motor_front.motorInit();
motor_back.motorInit();
motor_left.motorInit();
motor_right.motorInit();
wait(2);
while(1) {
led2 = 1;
//mode_value = 0;
//des_roll = 0;
//des_pitch = 0;
//des_yaw = 0;
//des_thrust = 0;
msgIface.getBytes();
//IMUFeedback msg_imu_feedback;
/*
msg_imu_feedback.id = 0;
imu_quadrotor.getAngleXYZ(¤tRoll, ¤tPitch, ¤tYaw);
msg_imu_feedback.current_roll = currentRoll;
msg_imu_feedback.current_pitch = currentPitch;
msg_imu_feedback.current_yaw = currentYaw;
msg_imu_feedback.current_thrust = 0;
*/
// Handle incoming messages
if (msgIface.peekPacket( &rx_data, &rx_len ) ) { // check if I received some data
got_packet = !got_packet;
msg_type = rx_data[0]; // get the ID
msg_data = rx_data+1; // get the message
//pc.printf("ID: %d\n\r",msg_type);
switch(msg_type) // check the ID
{
case MsgTypeMode:
//got_packet = !got_packet;
msg_mode = (MsgMode*)msg_data; // receive the data
pid_quadrotor.Kp = msg_mode->Kp_recv;
pid_quadrotor.Ki = msg_mode->Ki_recv;
pid_quadrotor.Kd = msg_mode->Kd_recv;
pid_quadrotor.Kd_yaw = msg_mode->Kd_yaw_recv;
pc.printf("Kp: %f\n\r", pid_quadrotor.Kp);
pc.printf("Ki: %f\n\r", pid_quadrotor.Ki);
pc.printf("Kd: %f\n\r", pid_quadrotor.Kd);
pc.printf("Kd_yaw: %f\n\r", pid_quadrotor.Kd_yaw);
break;
case MsgTypeQuadrotor:
//got_packet = !got_packet;
msg_quadrotor = (MsgQuadrotor*)msg_data;
des_roll = msg_quadrotor->desired_roll_recv;
des_pitch = msg_quadrotor->desired_pitch_recv;
des_yaw = msg_quadrotor->desired_yaw_recv;
des_thrust = msg_quadrotor->desired_thrust_recv;
if(des_thrust < 5)
{
flag_recv = 0;
}
else
{
flag_recv = 1;
}
break;
}
msgIface.dropPacket();
}
/*
if(des_roll > 5)
{
myled = !myled;
}
*/
//pc.printf("roll: %f\n\r", des_roll);
//pc.printf("pitch: %f\n\r", des_pitch);
//pc.printf("yaw: %f\n\r", des_yaw);
//pc.printf("thrust: %f\n\r", des_thrust);
//pc.printf("P: %f\n\r", Kp);
//pc.printf("I: %f\n\r", Ki);
//pc.printf("D: %f\n\r", Kd);
//pc.printf("Croll: %f\n\r", currentRoll);
//pc.printf("Cpitch: %f\n\r", currentPitch);
//pc.printf("Cyaw: %f\n\r", currentYaw);
//motor_front.motorSpeedControl(10);
//motor_back.motorSpeedControl(1);
//motor_left.motorSpeedControl(0);
//motor_right.motorSpeedControl(5);
// DEBUG: resist all motion
//des_roll = 0;
//des_pitch = 0;
//des_yaw = 0;
//des_thrust = 0;
flag_recv = 1;
if(flag_recv) {
pid_quadrotor.setAngleGoal(des_roll, des_pitch, des_yaw, des_thrust);
if(pid_quadrotor.updateSpeed(&pc, &frontCmdSpeed, &leftCmdSpeed, &backCmdSpeed, &rightCmdSpeed))
{
//motor_front.motorSpeedControl(frontCmdSpeed);
//motor_back.motorSpeedControl(backCmdSpeed);
//motor_left.motorSpeedControl(leftCmdSpeed);
//motor_right.motorSpeedControl(rightCmdSpeed);
//pc.printf("front: %f\n\r", frontCmdSpeed);
//pc.printf("back: %f\n\r", backCmdSpeed);
//pc.printf("left: %f\n\r", leftCmdSpeed);
//pc.printf("right: %f\n\r", rightCmdSpeed);
}
}
else {
motor_front.motorSpeedControl(0);
motor_back.motorSpeedControl(0);
motor_left.motorSpeedControl(0);
motor_right.motorSpeedControl(0);
}
//pid_quadrotor.return_currentAngle(¤tRoll, ¤tPitch, ¤tYaw);
//msgIface.sendNow();
}
}
static void usb_hub_events(void)
{
unsigned long flags;
struct list_head *tmp;
struct usb_device *dev;
struct usb_hub *hub;
struct usb_hub_status *hubsts;
u16 hubstatus;
u16 hubchange;
u16 portstatus;
u16 portchange;
int i, ret;
/*
* We restart the list everytime to avoid a deadlock with
* deleting hubs downstream from this one. This should be
* safe since we delete the hub from the event list.
* Not the most efficient, but avoids deadlocks.
*/
while (1) {
spin_lock_irqsave(&hub_event_lock, flags);
if (list_empty(&hub_event_list))
break;
/* Grab the next entry from the beginning of the list */
tmp = hub_event_list.next;
hub = list_entry(tmp, struct usb_hub, event_list);
dev = hub->dev;
list_del(tmp);
INIT_LIST_HEAD(tmp);
down(&hub->khubd_sem); /* never blocks, we were on list */
spin_unlock_irqrestore(&hub_event_lock, flags);
if (unlikely(hub->disconnected))
goto loop;
if (hub->error) {
dbg("resetting hub %d for error %d", dev->devnum, hub->error);
if (usb_hub_reset(hub)) {
err("error resetting hub %d - disconnecting", dev->devnum);
up(&hub->khubd_sem);
usb_hub_disconnect(dev);
continue;
}
hub->nerrors = 0;
hub->error = 0;
}
for (i = 0; i < hub->descriptor->bNbrPorts; i++) {
ret = usb_hub_port_status(dev, i, &portstatus, &portchange);
if (ret < 0) {
continue;
}
if (portchange & USB_PORT_STAT_C_CONNECTION) {
dbg("port %d connection change", i + 1);
usb_hub_port_connect_change(hub, i, portstatus, portchange);
} else if (portchange & USB_PORT_STAT_C_ENABLE) {
dbg("port %d enable change, status %x", i + 1, portstatus);
usb_clear_port_feature(dev, i + 1, USB_PORT_FEAT_C_ENABLE);
/*
* EM interference sometimes causes bad shielded USB devices to
* be shutdown by the hub, this hack enables them again.
* Works at least with mouse driver.
*/
if (!(portstatus & USB_PORT_STAT_ENABLE) &&
(portstatus & USB_PORT_STAT_CONNECTION) && (dev->children[i])) {
err("already running port %i disabled by hub (EMI?), re-enabling...",
i + 1);
usb_hub_port_connect_change(hub, i, portstatus, portchange);
}
}
if (portchange & USB_PORT_STAT_C_SUSPEND) {
dbg("port %d suspend change", i + 1);
usb_clear_port_feature(dev, i + 1, USB_PORT_FEAT_C_SUSPEND);
}
if (portchange & USB_PORT_STAT_C_OVERCURRENT) {
err("port %d over-current change", i + 1);
usb_clear_port_feature(dev, i + 1, USB_PORT_FEAT_C_OVER_CURRENT);
usb_hub_power_on(hub);
}
if (portchange & USB_PORT_STAT_C_RESET) {
dbg("port %d reset change", i + 1);
usb_clear_port_feature(dev, i + 1, USB_PORT_FEAT_C_RESET);
}
} /* end for i */
/* deal with hub status changes */
hubsts = kmalloc(sizeof *hubsts, GFP_KERNEL);
if (!hubsts) {
err("couldn't allocate hubsts");
} else {
if (usb_get_hub_status(dev, hubsts) < 0)
err("get_hub_status failed");
else {
hubstatus = le16_to_cpup(&hubsts->wHubStatus);
hubchange = le16_to_cpup(&hubsts->wHubChange);
if (hubchange & HUB_CHANGE_LOCAL_POWER) {
dbg("hub power change");
usb_clear_hub_feature(dev, C_HUB_LOCAL_POWER);
}
if (hubchange & HUB_CHANGE_OVERCURRENT) {
dbg("hub overcurrent change");
wait_ms(500); /* Cool down */
usb_clear_hub_feature(dev, C_HUB_OVER_CURRENT);
usb_hub_power_on(hub);
}
}
kfree(hubsts);
}
loop:
up(&hub->khubd_sem);
} /* end while (1) */
spin_unlock_irqrestore(&hub_event_lock, flags);
}
int setup_sx1276_FSK(uint32_t frequency) {
uint64_t freq_reg;
uint8_t ModulationShaping = 0;
uint8_t PllHop = 1;
uint8_t LnaGain = 1;
uint8_t LnaBoost = 3;
uint8_t AdcBwAuto = 0;
uint8_t AdcBw = 7;
uint8_t AdcLowPwr = 0;
uint8_t AdcTrim = 6;
uint8_t AdcTest = 0;
uint8_t RxBwExp = 2;
uint8_t RxBwMant = 1;
uint8_t RssiSmoothing = 5;
uint8_t RssiOffset = 3;
uint8_t reg_val;
int x;
/* Set in FSK mode */
x = lgw_sx127x_reg_w(SX1276_REG_OPMODE, 0);
wait_ms(100);
x |= lgw_sx127x_reg_w(SX1276_REG_OPMODE, 0 | (ModulationShaping << 3)); /* Sleep mode, no FSK shaping */
wait_ms(100);
x |= lgw_sx127x_reg_w(SX1276_REG_OPMODE, 1 | (ModulationShaping << 3)); /* Standby mode, no FSK shaping */
wait_ms(100);
/* Set RF carrier frequency */
x |= lgw_sx127x_reg_w(SX1276_REG_PLLHOP, PllHop << 7);
freq_reg = ((uint64_t)frequency << 19) / (uint64_t)32000000;
x |= lgw_sx127x_reg_w(SX1276_REG_FRFMSB, (freq_reg >> 16) & 0xFF);
x |= lgw_sx127x_reg_w(SX1276_REG_FRFMID, (freq_reg >> 8) & 0xFF);
x |= lgw_sx127x_reg_w(SX1276_REG_FRFLSB, (freq_reg >> 0) & 0xFF);
/* Config */
x |= lgw_sx127x_reg_w(SX1276_REG_LNA, LnaBoost | (LnaGain << 5)); /* Improved sensitivity, highest gain */
x |= lgw_sx127x_reg_w(0x57, AdcBw | (AdcBwAuto << 3));
x |= lgw_sx127x_reg_w(0x58, AdcTest | (AdcTrim << 4) | (AdcLowPwr << 7));
/* set BR and FDEV for 200 kHz bandwidth*/
x |= lgw_sx127x_reg_w(SX1276_REG_BITRATEMSB, 125);
x |= lgw_sx127x_reg_w(SX1276_REG_BITRATELSB, 0);
x |= lgw_sx127x_reg_w(SX1276_REG_FDEVMSB, 2);
x |= lgw_sx127x_reg_w(SX1276_REG_FDEVLSB, 225);
/* Config continues... */
x |= lgw_sx127x_reg_w(SX1276_REG_RXCONFIG, 0); /* Disable AGC */
x |= lgw_sx127x_reg_w(SX1276_REG_RSSICONFIG, RssiSmoothing | (RssiOffset << 3)); /* Set RSSI smoothing to 64 samples, RSSI offset 3dB */
x |= lgw_sx127x_reg_w(SX1276_REG_RXBW, RxBwExp | (RxBwMant << 3)); /* RX BW = 100kHz, Mant=20, Exp=2 */
x |= lgw_sx127x_reg_w(SX1276_REG_RXDELAY, 2);
x |= lgw_sx127x_reg_w(SX1276_REG_PLL, 0x10); /* PLL BW set to 75 KHz */
x |= lgw_sx127x_reg_w(0x43, 1); /* optimize PLL start-up time */
if (x != LGW_REG_SUCCESS) {
DEBUG_MSG("ERROR: Failed to configure SX1276\n");
return x;
}
/* set Rx continuous mode */
x = lgw_sx127x_reg_w(SX1276_REG_OPMODE, 5 | (ModulationShaping << 3)); /* Receiver Mode, no FSK shaping */
wait_ms(500);
x |= lgw_sx127x_reg_r(SX1276_REG_IRQFLAGS1, ®_val);
/* Check if RxReady and ModeReady */
if ((TAKE_N_BITS_FROM(reg_val, 6, 1) == 0) || (TAKE_N_BITS_FROM(reg_val, 7, 1) == 0) || (x != LGW_REG_SUCCESS)) {
DEBUG_MSG("ERROR: SX1276 failed to enter RX continuous mode\n");
return LGW_REG_ERROR;
}
wait_ms(500);
DEBUG_MSG("INFO: Successfully configured SX1276 for FSK modulation\n");
return LGW_REG_SUCCESS;
}
int main(void)
{
// ADNS configuration
adns6010_configuration_t adns_config;
//--------------------------------------------------------------------------
// Booting
// Turn interruptions ON
sei();
// Initialize UART
uart_init();
fdevopen(uart1_dev_send, uart1_dev_recv);
//--------------------------------------------------------
// Error configuration
error_register_emerg(log_event);
error_register_error(log_event);
error_register_warning(log_event);
error_register_notice(log_event);
error_register_debug(log_event);
log_level = ERROR_SEVERITY_NOTICE;
//log_level = ERROR_SEVERITY_DEBUG;
// Clear screen
printf("%c[2J",0x1B);
printf("%c[0;0H",0x1B);
// Some advertisment :p
NOTICE(0,"Robotter 2009 - Galipeur - UNIOC-NG ADNS6010 CALIBRATION");
NOTICE(0,"Compiled "__DATE__" at "__TIME__".");
//--------------------------------------------------------
// Initialize scheduler
scheduler_init();
//--------------------------------------------------------
// Initialize time
time_init(160);
//--------------------------------------------------------
// Initialize FPGA
fpga_init();
// turn off led
_SFR_MEM8(0x1800) = 1;
//--------------------------------------------------------
// ADNS6010
//--------------------------------------------------------
NOTICE(0,"Initializing ADNS6010s");
adns6010_init();
#ifndef ADNS_OVERRIDE
NOTICE(0,"Checking ADNS6010s firmware");
adns6010_checkFirmware();
// ADNS CONFIGURATION
adns_config.res = ADNS6010_RES_2000;
adns_config.shutter = ADNS6010_SHUTTER_OFF;
adns_config.power = 0x11;
NOTICE(0,"Checking ADNS6010s SPI communication");
adns6010_checkSPI();
NOTICE(0,"Booting ADNS6010s");
adns6010_boot(&adns_config);
NOTICE(0,"Checking ADNS6010s");
adns6010_checks();
NOTICE(0,"ADNS6010s are GO");
#endif
//--------------------------------------------------------
NOTICE(0,"Initializing ADCs");
adc_init();
// For ploting purposes
NOTICE(0,"<PLOTMARK>");
// Set ADNS6010 system to automatic
adns6010_setMode(ADNS6010_BHVR_MODE_AUTOMATIC);
// Safe key event
scheduler_add_periodical_event_priority(&safe_key_pressed, NULL, 100, 50);
//----------------------------------------------------------------------
NOTICE(0,"Any key to go");
char cal_name = 'x';
uint8_t c;
while(1)
{
c = cli_getkey();
if(c != -1)
break;
}
//----------------------------------------------------------------------
//----------------------------------------------------------------------
int i,k;
NOTICE(0,"Go");
// Initialize control systems
cs_initialize();
cs_vx=0;
cs_vy=0;
cs_omega=0;
// remove break
motor_cs_break(0);
event_cs =
scheduler_add_periodical_event_priority(&cs_update, NULL, 25, 100);
NOTICE(0,"Press 'r' for auto line calibration / 'l' for ADNS line calibration / 'm' for motor encoders line calibration / 'a' for angle calibration / 'n' non-auto angle calibration / 'k' motor non-auto angle calibration");
c = cli_getkey();
//-----------------------------------------------------------------------------
// MOTOR ENCODERS LINE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='m')
{
NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3");
c = cli_getkey();
switch(c)
{
case '1': cal_name = 'A'; robot_angle = 0; break;
case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break;
case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break;
default : cal_name = 'A'; robot_angle = 0; break;
}
NOTICE(0,"ME ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
motor_encoders_get_value(&motor_zero);
wait_ms(200);
NOTICE(0,"ME zero values = (%ld,%ld,%ld)",
motor_zero.vectors[0],motor_zero.vectors[1],motor_zero.vectors[2]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
cs_vx=0;
cs_vy=0;
cs_omega=0;
// perform calibration, positive direction
motor_line_calibration(1);
NOTICE(0,"ME ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
motor_encoders_get_value(&motor_zero);
wait_ms(200);
NOTICE(0,"ME zero values = (%ld,%ld,%ld)",
motor_zero.vectors[0],motor_zero.vectors[1],motor_zero.vectors[2]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
// perform calibration, negative direction
motor_line_calibration(-1);
// output calibration data
NOTICE(0,"CALIBRATION DONE, printint scilab matrix :");
printf("%c=[\n",cal_name);
for(i=0;i<12;i++)
for(k=0;k<3;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==2)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
//-----------------------------------------------------------------------------
// ADNS LINE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='r')
{
NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3");
c = cli_getkey();
switch(c)
{
case '1': cal_name = 'A'; robot_angle = 0; break;
case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break;
case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break;
default : cal_name = 'A'; robot_angle = 0; break;
}
for(i=0;i<20;i++)
{
setmotors_course(robot_angle, 1);
wait_ms(1000);
setmotors_course(robot_angle, -1);
wait_ms(1000);
adns6010_encoders_get_value(&adns_zero);
}
}
//-----------------------------------------------------------------------------
// ADNS LINE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='l')
{
NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3");
c = cli_getkey();
switch(c)
{
case '1': cal_name = 'A'; robot_angle = 0; break;
case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break;
case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break;
default : cal_name = 'A'; robot_angle = 0; break;
}
NOTICE(0,"ADNS ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
adns6010_encoders_get_value(&adns_zero);
wait_ms(200);
NOTICE(0,"ADNS zero values = (%ld,%ld,%ld,%ld,%ld,%ld)",
adns_zero.vectors[0],adns_zero.vectors[1],adns_zero.vectors[2],
adns_zero.vectors[3],adns_zero.vectors[4],adns_zero.vectors[5]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
cs_vx=0;
cs_vy=0;
cs_omega=0;
// perform calibration, positive direction
line_calibration(1);
NOTICE(0,"ADNS ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
adns6010_encoders_get_value(&adns_zero);
wait_ms(200);
NOTICE(0,"ADNS zero values = (%ld,%ld,%ld,%ld,%ld,%ld)",
adns_zero.vectors[0],adns_zero.vectors[1],adns_zero.vectors[2],
adns_zero.vectors[3],adns_zero.vectors[4],adns_zero.vectors[5]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
// perform calibration, negative direction
line_calibration(-1);
// output calibration data
NOTICE(0,"CALIBRATION DONE, printint scilab matrix :");
printf("%c=[\n",cal_name);
for(i=0;i<12;i++)
for(k=0;k<6;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==5)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
//-----------------------------------------------------------------------------
// ANGLE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='a' )
{
NOTICE(0,"Angle calibration is fully automatic, robot will do approx. 3 turns");
NOTICE(0,"Press a key to start, calibration will start ~5s after.");
while(!cli_getkey());
NOTICE(0,"Starting in 5s...");
wait_ms(5000);
angle_calibration(1);
angle_calibration(-1);
NOTICE(0,"Angle calibration done, press a key for results matrix : ");
while(!cli_getkey());
printf("R=[\n");
for(i=0;i<12;i++)
for(k=0;k<6;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==5)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
int i,k;
for(i=0;i<12;i++)
{
printf("u_r%d = [0 0 %7.1f];\n",i,calibration_data_compass[i]);
}
printf("// ");
for(i=0;i<12;i++)
if(i==11)
printf("u_r11];\n");
else
printf("u_r%d;",i);
}
//-----------------------------------------------------------------------------
// MOTOR NON-AUTO ANGLE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='k' )
{
NOTICE(0,"key to go");
while(!cli_getkey());
motor_angle_na_calibration(1);
motor_angle_na_calibration(-1);
NOTICE(0,"Angle calibration done, press a key for results matrix : ");
while(!cli_getkey());
printf("R=[\n");
for(i=0;i<12;i++)
for(k=0;k<3;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==2)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
//-----------------------------------------------------------------------------
// NON-AUTO ANGLE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='n' )
{
NOTICE(0,"key to go");
while(!cli_getkey());
angle_na_calibration(1);
angle_na_calibration(-1);
NOTICE(0,"Angle calibration done, press a key for results matrix : ");
while(!cli_getkey());
printf("R=[\n");
for(i=0;i<12;i++)
for(k=0;k<6;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==5)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
EMERG(0,"Program ended");
while(1);
return 0;
}
int setup_sx125x(uint8_t rf_chain, uint8_t rf_clkout, bool rf_enable, uint8_t rf_radio_type, uint32_t freq_hz) {
uint32_t part_int = 0;
uint32_t part_frac = 0;
int cpt_attempts = 0;
if (rf_chain >= LGW_RF_CHAIN_NB) {
DEBUG_MSG("ERROR: INVALID RF_CHAIN\n");
return -1;
}
/* Get version to identify SX1255/57 silicon revision */
DEBUG_PRINTF("Note: SX125x #%d version register returned 0x%02x\n", rf_chain, sx125x_read(rf_chain, 0x07));
/* General radio setup */
if (rf_clkout == rf_chain) {
sx125x_write(rf_chain, 0x10, SX125x_TX_DAC_CLK_SEL + 2);
DEBUG_PRINTF("Note: SX125x #%d clock output enabled\n", rf_chain);
} else {
sx125x_write(rf_chain, 0x10, SX125x_TX_DAC_CLK_SEL);
DEBUG_PRINTF("Note: SX125x #%d clock output disabled\n", rf_chain);
}
switch (rf_radio_type) {
case LGW_RADIO_TYPE_SX1255:
sx125x_write(rf_chain, 0x28, SX125x_XOSC_GM_STARTUP + SX125x_XOSC_DISABLE*16);
break;
case LGW_RADIO_TYPE_SX1257:
sx125x_write(rf_chain, 0x26, SX125x_XOSC_GM_STARTUP + SX125x_XOSC_DISABLE*16);
break;
default:
DEBUG_PRINTF("ERROR: UNEXPECTED VALUE %d FOR RADIO TYPE\n", rf_radio_type);
break;
}
if (rf_enable == true) {
/* Tx gain and trim */
sx125x_write(rf_chain, 0x08, SX125x_TX_MIX_GAIN + SX125x_TX_DAC_GAIN*16);
sx125x_write(rf_chain, 0x0A, SX125x_TX_ANA_BW + SX125x_TX_PLL_BW*32);
sx125x_write(rf_chain, 0x0B, SX125x_TX_DAC_BW);
/* Rx gain and trim */
sx125x_write(rf_chain, 0x0C, SX125x_LNA_ZIN + SX125x_RX_BB_GAIN*2 + SX125x_RX_LNA_GAIN*32);
sx125x_write(rf_chain, 0x0D, SX125x_RX_BB_BW + SX125x_RX_ADC_TRIM*4 + SX125x_RX_ADC_BW*32);
sx125x_write(rf_chain, 0x0E, SX125x_ADC_TEMP + SX125x_RX_PLL_BW*2);
/* set RX PLL frequency */
switch (rf_radio_type) {
case LGW_RADIO_TYPE_SX1255:
part_int = freq_hz / (SX125x_32MHz_FRAC << 7); /* integer part, gives the MSB */
part_frac = ((freq_hz % (SX125x_32MHz_FRAC << 7)) << 9) / SX125x_32MHz_FRAC; /* fractional part, gives middle part and LSB */
break;
case LGW_RADIO_TYPE_SX1257:
part_int = freq_hz / (SX125x_32MHz_FRAC << 8); /* integer part, gives the MSB */
part_frac = ((freq_hz % (SX125x_32MHz_FRAC << 8)) << 8) / SX125x_32MHz_FRAC; /* fractional part, gives middle part and LSB */
break;
default:
DEBUG_PRINTF("ERROR: UNEXPECTED VALUE %d FOR RADIO TYPE\n", rf_radio_type);
break;
}
sx125x_write(rf_chain, 0x01,0xFF & part_int); /* Most Significant Byte */
sx125x_write(rf_chain, 0x02,0xFF & (part_frac >> 8)); /* middle byte */
sx125x_write(rf_chain, 0x03,0xFF & part_frac); /* Least Significant Byte */
/* start and PLL lock */
do {
if (cpt_attempts >= PLL_LOCK_MAX_ATTEMPTS) {
DEBUG_MSG("ERROR: FAIL TO LOCK PLL\n");
return -1;
}
sx125x_write(rf_chain, 0x00, 1); /* enable Xtal oscillator */
sx125x_write(rf_chain, 0x00, 3); /* Enable RX (PLL+FE) */
++cpt_attempts;
DEBUG_PRINTF("Note: SX125x #%d PLL start (attempt %d)\n", rf_chain, cpt_attempts);
wait_ms(1);
} while((sx125x_read(rf_chain, 0x11) & 0x02) == 0);
} else {
/* returns -1 on failure */
static int send_doc(curl_socket_t sock, struct httprequest *req)
{
ssize_t written;
size_t count;
const char *buffer;
char *ptr=NULL;
FILE *stream;
char *cmd=NULL;
size_t cmdsize=0;
FILE *dump;
bool persistant = TRUE;
bool sendfailure = FALSE;
size_t responsesize;
int error = 0;
int res;
static char weare[256];
char partbuf[80]="data";
logmsg("Send response number %ld part %ld", req->testno, req->partno);
switch(req->rcmd) {
default:
case RCMD_NORMALREQ:
break; /* continue with business as usual */
case RCMD_STREAM:
#define STREAMTHIS "a string to stream 01234567890\n"
count = strlen(STREAMTHIS);
for(;;) {
written = swrite(sock, STREAMTHIS, count);
if(got_exit_signal)
return -1;
if(written != (ssize_t)count) {
logmsg("Stopped streaming");
break;
}
}
return -1;
case RCMD_IDLE:
/* Do nothing. Sit idle. Pretend it rains. */
return 0;
}
req->open = FALSE;
if(req->testno < 0) {
size_t msglen;
char msgbuf[64];
switch(req->testno) {
case DOCNUMBER_QUIT:
logmsg("Replying to QUIT");
buffer = docquit;
break;
case DOCNUMBER_WERULEZ:
/* we got a "friends?" question, reply back that we sure are */
logmsg("Identifying ourselves as friends");
snprintf(msgbuf, sizeof(msgbuf), "RTSP_SERVER WE ROOLZ: %ld\r\n",
(long)getpid());
msglen = strlen(msgbuf);
snprintf(weare, sizeof(weare),
"HTTP/1.1 200 OK\r\nContent-Length: %zu\r\n\r\n%s",
msglen, msgbuf);
buffer = weare;
break;
case DOCNUMBER_INTERNAL:
logmsg("Bailing out due to internal error");
return -1;
case DOCNUMBER_CONNECT:
logmsg("Replying to CONNECT");
buffer = docconnect;
break;
case DOCNUMBER_BADCONNECT:
logmsg("Replying to a bad CONNECT");
buffer = docbadconnect;
break;
case DOCNUMBER_404:
default:
logmsg("Replying to with a 404");
if(req->protocol == RPROT_HTTP) {
buffer = doc404_HTTP;
}
else {
buffer = doc404_RTSP;
}
break;
}
count = strlen(buffer);
}
else {
char *filename = test2file(req->testno);
if(0 != req->partno)
snprintf(partbuf, sizeof(partbuf), "data%ld", req->partno);
stream=fopen(filename, "rb");
if(!stream) {
error = errno;
logmsg("fopen() failed with error: %d %s", error, strerror(error));
logmsg("Error opening file: %s", filename);
logmsg("Couldn't open test file");
return 0;
}
else {
error = getpart(&ptr, &count, "reply", partbuf, stream);
fclose(stream);
if(error) {
logmsg("getpart() failed with error: %d", error);
return 0;
}
buffer = ptr;
}
if(got_exit_signal) {
free(ptr);
return -1;
}
/* re-open the same file again */
stream=fopen(filename, "rb");
if(!stream) {
error = errno;
logmsg("fopen() failed with error: %d %s", error, strerror(error));
logmsg("Error opening file: %s", filename);
logmsg("Couldn't open test file");
free(ptr);
return 0;
}
else {
/* get the custom server control "commands" */
error = getpart(&cmd, &cmdsize, "reply", "postcmd", stream);
fclose(stream);
if(error) {
logmsg("getpart() failed with error: %d", error);
free(ptr);
return 0;
}
}
}
if(got_exit_signal) {
free(ptr);
free(cmd);
return -1;
}
/* If the word 'swsclose' is present anywhere in the reply chunk, the
connection will be closed after the data has been sent to the requesting
client... */
if(strstr(buffer, "swsclose") || !count) {
persistant = FALSE;
logmsg("connection close instruction \"swsclose\" found in response");
}
if(strstr(buffer, "swsbounce")) {
prevbounce = TRUE;
logmsg("enable \"swsbounce\" in the next request");
}
else
prevbounce = FALSE;
dump = fopen(RESPONSE_DUMP, "ab");
if(!dump) {
error = errno;
logmsg("fopen() failed with error: %d %s", error, strerror(error));
logmsg("Error opening file: %s", RESPONSE_DUMP);
logmsg("couldn't create logfile: " RESPONSE_DUMP);
free(ptr);
free(cmd);
return -1;
}
responsesize = count;
do {
/* Ok, we send no more than 200 bytes at a time, just to make sure that
larger chunks are split up so that the client will need to do multiple
recv() calls to get it and thus we exercise that code better */
size_t num = count;
if(num > 200)
num = 200;
written = swrite(sock, buffer, num);
if(written < 0) {
sendfailure = TRUE;
break;
}
else {
logmsg("Sent off %zd bytes", written);
}
/* write to file as well */
fwrite(buffer, 1, (size_t)written, dump);
if(got_exit_signal)
break;
count -= written;
buffer += written;
} while(count>0);
/* Send out any RTP data */
if(req->rtp_buffer) {
logmsg("About to write %zu RTP bytes", req->rtp_buffersize);
count = req->rtp_buffersize;
do {
size_t num = count;
if(num > 200)
num = 200;
written = swrite(sock, req->rtp_buffer + (req->rtp_buffersize - count),
num);
if(written < 0) {
sendfailure = TRUE;
break;
}
count -= written;
} while(count > 0);
free(req->rtp_buffer);
req->rtp_buffersize = 0;
}
do {
res = fclose(dump);
} while(res && ((error = errno) == EINTR));
if(res)
logmsg("Error closing file %s error: %d %s",
RESPONSE_DUMP, error, strerror(error));
if(got_exit_signal) {
free(ptr);
free(cmd);
return -1;
}
if(sendfailure) {
logmsg("Sending response failed. Only (%zu bytes) of "
"(%zu bytes) were sent",
responsesize-count, responsesize);
free(ptr);
free(cmd);
return -1;
}
logmsg("Response sent (%zu bytes) and written to " RESPONSE_DUMP,
responsesize);
free(ptr);
if(cmdsize > 0) {
char command[32];
int quarters;
int num;
ptr=cmd;
do {
if(2 == sscanf(ptr, "%31s %d", command, &num)) {
if(!strcmp("wait", command)) {
logmsg("Told to sleep for %d seconds", num);
quarters = num * 4;
while(quarters > 0) {
quarters--;
res = wait_ms(250);
if(got_exit_signal)
break;
if(res) {
/* should not happen */
error = errno;
logmsg("wait_ms() failed with error: (%d) %s",
error, strerror(error));
break;
}
}
if(!quarters)
logmsg("Continuing after sleeping %d seconds", num);
}
else
logmsg("Unknown command in reply command section");
}
ptr = strchr(ptr, '\n');
if(ptr)
ptr++;
else
ptr = NULL;
} while(ptr && *ptr);
}
free(cmd);
req->open = persistant;
prevtestno = req->testno;
prevpartno = req->partno;
return 0;
}
static bool command_common(uint8_t code)
{
#ifdef KEYBOARD_LOCK_ENABLE
static host_driver_t *host_driver = 0;
#endif
switch (code) {
#ifdef SLEEP_LED_ENABLE
case KC_Z:
// test breathing sleep LED
print("Sleep LED test\n");
sleep_led_toggle();
led_set(host_keyboard_leds());
break;
#endif
#ifdef BOOTMAGIC_ENABLE
case KC_E:
print("eeconfig:\n");
print_eeconfig();
break;
#endif
#ifdef KEYBOARD_LOCK_ENABLE
case KC_CAPSLOCK:
if (host_get_driver()) {
host_driver = host_get_driver();
clear_keyboard();
host_set_driver(0);
print("Locked.\n");
} else {
host_set_driver(host_driver);
print("Unlocked.\n");
}
break;
#endif
case KC_H:
case KC_SLASH: /* ? */
command_common_help();
break;
case KC_C:
debug_matrix = false;
debug_keyboard = false;
debug_mouse = false;
debug_enable = false;
command_console_help();
print("C> ");
command_state = CONSOLE;
break;
case KC_PAUSE:
clear_keyboard();
print("\n\nbootloader... ");
wait_ms(1000);
bootloader_jump(); // not return
break;
case KC_D:
if (debug_enable) {
print("\ndebug: off\n");
debug_matrix = false;
debug_keyboard = false;
debug_mouse = false;
debug_enable = false;
} else {
print("\ndebug: on\n");
debug_enable = true;
}
break;
case KC_X: // debug matrix toggle
debug_matrix = !debug_matrix;
if (debug_matrix) {
print("\nmatrix: on\n");
debug_enable = true;
} else {
print("\nmatrix: off\n");
}
break;
case KC_K: // debug keyboard toggle
debug_keyboard = !debug_keyboard;
if (debug_keyboard) {
print("\nkeyboard: on\n");
debug_enable = true;
} else {
print("\nkeyboard: off\n");
}
break;
case KC_M: // debug mouse toggle
debug_mouse = !debug_mouse;
if (debug_mouse) {
print("\nmouse: on\n");
debug_enable = true;
} else {
print("\nmouse: off\n");
}
break;
case KC_V: // print version & information
print("\n\t- Version -\n");
print("DESC: " STR(DESCRIPTION) "\n");
print("VID: " STR(VENDOR_ID) "(" STR(MANUFACTURER) ") "
"PID: " STR(PRODUCT_ID) "(" STR(PRODUCT) ") "
"VER: " STR(DEVICE_VER) "\n");
print("BUILD: " STR(VERSION) " (" __TIME__ " " __DATE__ ")\n");
/* build options */
print("OPTIONS:"
#ifdef PROTOCOL_PJRC
" PJRC"
#endif
#ifdef PROTOCOL_LUFA
" LUFA"
#endif
#ifdef PROTOCOL_VUSB
" VUSB"
#endif
#ifdef PROTOCOL_CHIBIOS
" CHIBIOS"
#endif
#ifdef BOOTMAGIC_ENABLE
" BOOTMAGIC"
#endif
#ifdef MOUSEKEY_ENABLE
" MOUSEKEY"
#endif
#ifdef EXTRAKEY_ENABLE
" EXTRAKEY"
#endif
#ifdef CONSOLE_ENABLE
" CONSOLE"
#endif
#ifdef COMMAND_ENABLE
" COMMAND"
#endif
#ifdef NKRO_ENABLE
" NKRO"
#endif
#ifdef KEYMAP_SECTION_ENABLE
" KEYMAP_SECTION"
#endif
" " STR(BOOTLOADER_SIZE) "\n");
print("GCC: " STR(__GNUC__) "." STR(__GNUC_MINOR__) "." STR(__GNUC_PATCHLEVEL__)
#if defined(__AVR__)
" AVR-LIBC: " __AVR_LIBC_VERSION_STRING__
" AVR_ARCH: avr" STR(__AVR_ARCH__) "\n");
#elif defined(__arm__)
// TODO
);
#endif
break;
case KC_S:
print("\n\t- Status -\n");
print_val_hex8(host_keyboard_leds());
print_val_hex8(keyboard_protocol);
print_val_hex8(keyboard_idle);
#ifdef NKRO_ENABLE
print_val_hex8(keyboard_nkro);
#endif
print_val_hex32(timer_read32());
#ifdef PROTOCOL_PJRC
print_val_hex8(UDCON);
print_val_hex8(UDIEN);
print_val_hex8(UDINT);
print_val_hex8(usb_keyboard_leds);
print_val_hex8(usb_keyboard_idle_count);
#endif
#ifdef PROTOCOL_PJRC
# if USB_COUNT_SOF
print_val_hex8(usbSofCount);
# endif
#endif
break;
#ifdef NKRO_ENABLE
case KC_N:
clear_keyboard(); //Prevents stuck keys.
keyboard_nkro = !keyboard_nkro;
if (keyboard_nkro) {
print("NKRO: on\n");
} else {
print("NKRO: off\n");
}
break;
#endif
case KC_ESC:
case KC_GRV:
case KC_0:
case KC_F10:
switch_default_layer(0);
break;
case KC_1 ... KC_9:
switch_default_layer((code - KC_1) + 1);
break;
case KC_F1 ... KC_F9:
switch_default_layer((code - KC_F1) + 1);
break;
default:
print("?");
return false;
}
/*
* This is a wrapper around poll(). If poll() does not exist, then
* select() is used instead. An error is returned if select() is
* being used and a file descriptor is too large for FD_SETSIZE.
* A negative timeout value makes this function wait indefinitely,
* unles no valid file descriptor is given, when this happens the
* negative timeout is ignored and the function times out immediately.
* When compiled with CURL_ACKNOWLEDGE_EINTR defined, EINTR condition
* is honored and function might exit early without awaiting timeout,
* otherwise EINTR will be ignored.
*
* Return values:
* -1 = system call error or fd >= FD_SETSIZE
* 0 = timeout
* N = number of structures with non zero revent fields
*/
int Curl_poll(struct pollfd ufds[], unsigned int nfds, int timeout_ms)
{
#ifndef HAVE_POLL_FINE
struct timeval pending_tv;
struct timeval *ptimeout;
fd_set fds_read;
fd_set fds_write;
fd_set fds_err;
curl_socket_t maxfd;
#endif
struct timeval initial_tv = {0,0};
bool fds_none = TRUE;
unsigned int i;
int pending_ms = 0;
int error;
int r;
if(ufds) {
for (i = 0; i < nfds; i++) {
if(ufds[i].fd != CURL_SOCKET_BAD) {
fds_none = FALSE;
break;
}
}
}
if(fds_none) {
r = wait_ms(timeout_ms);
return r;
}
/* Avoid initial timestamp, avoid curlx_tvnow() call, when elapsed
time in this function does not need to be measured. This happens
when function is called with a zero timeout or a negative timeout
value indicating a blocking call should be performed. */
if(timeout_ms > 0) {
pending_ms = timeout_ms;
initial_tv = curlx_tvnow();
}
#ifdef HAVE_POLL_FINE
do {
if(timeout_ms < 0)
pending_ms = -1;
else if(!timeout_ms)
pending_ms = 0;
r = poll(ufds, nfds, pending_ms);
if(r != -1)
break;
error = SOCKERRNO;
if(error && error_not_EINTR)
break;
if(timeout_ms > 0) {
pending_ms = timeout_ms - elapsed_ms;
if(pending_ms <= 0)
break;
}
} while(r == -1);
if(r < 0)
return -1;
if(r == 0)
return 0;
for (i = 0; i < nfds; i++) {
if(ufds[i].fd == CURL_SOCKET_BAD)
continue;
if(ufds[i].revents & POLLHUP)
ufds[i].revents |= POLLIN;
if(ufds[i].revents & POLLERR)
ufds[i].revents |= (POLLIN|POLLOUT);
}
#else /* HAVE_POLL_FINE */
FD_ZERO(&fds_read);
FD_ZERO(&fds_write);
FD_ZERO(&fds_err);
maxfd = (curl_socket_t)-1;
for (i = 0; i < nfds; i++) {
ufds[i].revents = 0;
if(ufds[i].fd == CURL_SOCKET_BAD)
continue;
VERIFY_SOCK(ufds[i].fd);
if(ufds[i].events & (POLLIN|POLLOUT|POLLPRI|
POLLRDNORM|POLLWRNORM|POLLRDBAND)) {
if(ufds[i].fd > maxfd)
maxfd = ufds[i].fd;
if(ufds[i].events & (POLLRDNORM|POLLIN))
FD_SET(ufds[i].fd, &fds_read);
if(ufds[i].events & (POLLWRNORM|POLLOUT))
FD_SET(ufds[i].fd, &fds_write);
if(ufds[i].events & (POLLRDBAND|POLLPRI))
FD_SET(ufds[i].fd, &fds_err);
}
}
ptimeout = (timeout_ms < 0) ? NULL : &pending_tv;
do {
if(timeout_ms > 0) {
pending_tv.tv_sec = pending_ms / 1000;
pending_tv.tv_usec = (pending_ms % 1000) * 1000;
}
else if(!timeout_ms) {
pending_tv.tv_sec = 0;
pending_tv.tv_usec = 0;
}
r = select((int)maxfd + 1, &fds_read, &fds_write, &fds_err, ptimeout);
if(r != -1)
break;
error = SOCKERRNO;
if(error && error_not_EINTR)
break;
if(timeout_ms > 0) {
pending_ms = timeout_ms - elapsed_ms;
if(pending_ms <= 0)
break;
}
} while(r == -1);
if(r < 0)
return -1;
if(r == 0)
return 0;
r = 0;
for (i = 0; i < nfds; i++) {
ufds[i].revents = 0;
if(ufds[i].fd == CURL_SOCKET_BAD)
continue;
if(FD_ISSET(ufds[i].fd, &fds_read))
ufds[i].revents |= POLLIN;
if(FD_ISSET(ufds[i].fd, &fds_write))
ufds[i].revents |= POLLOUT;
if(FD_ISSET(ufds[i].fd, &fds_err))
ufds[i].revents |= POLLPRI;
if(ufds[i].revents != 0)
r++;
}
#endif /* HAVE_POLL_FINE */
return r;
}
/*
* This is a wrapper around poll(). If poll() does not exist, then
* select() is used instead. An error is returned if select() is
* being used and a file descriptor is too large for FD_SETSIZE.
* A negative timeout value makes this function wait indefinitely,
* unles no valid file descriptor is given, when this happens the
* negative timeout is ignored and the function times out immediately.
* When compiled with CURL_ACKNOWLEDGE_EINTR defined, EINTR condition
* is honored and function might exit early without awaiting timeout,
* otherwise EINTR will be ignored.
*
* Return values:
* -1 = system call error or fd >= FD_SETSIZE
* 0 = timeout
* N = number of structures with non zero revent fields
*/
int Curl_poll(struct pollfd ufds[], unsigned int nfds, int timeout_ms)
{
#ifndef HAVE_POLL_FINE
struct timeval pending_tv;
struct timeval *ptimeout;
fd_set fds_read;
fd_set fds_write;
fd_set fds_err;
curl_socket_t maxfd;
#endif
struct timeval initial_tv;
bool fds_none = TRUE;
unsigned int i;
int pending_ms;
int error;
int r;
if (ufds) {
for (i = 0; i < nfds; i++) {
if (ufds[i].fd != CURL_SOCKET_BAD) {
fds_none = FALSE;
break;
}
}
}
if (fds_none) {
r = wait_ms(timeout_ms);
return r;
}
pending_ms = timeout_ms;
initial_tv = curlx_tvnow();
#ifdef HAVE_POLL_FINE
do {
if (timeout_ms < 0)
pending_ms = -1;
r = poll(ufds, nfds, pending_ms);
} while ((r == -1) && (error = SOCKERRNO) &&
(error != EINVAL) && error_not_EINTR &&
((timeout_ms < 0) || ((pending_ms = timeout_ms - elapsed_ms) > 0)));
#else /* HAVE_POLL_FINE */
FD_ZERO(&fds_read);
FD_ZERO(&fds_write);
FD_ZERO(&fds_err);
maxfd = (curl_socket_t)-1;
for (i = 0; i < nfds; i++) {
ufds[i].revents = 0;
if (ufds[i].fd == CURL_SOCKET_BAD)
continue;
VERIFY_SOCK(ufds[i].fd);
if (ufds[i].events & (POLLIN|POLLOUT|POLLPRI|
POLLRDNORM|POLLWRNORM|POLLRDBAND)) {
if (ufds[i].fd > maxfd)
maxfd = ufds[i].fd;
if (ufds[i].events & (POLLRDNORM|POLLIN))
FD_SET(ufds[i].fd, &fds_read);
if (ufds[i].events & (POLLWRNORM|POLLOUT))
FD_SET(ufds[i].fd, &fds_write);
if (ufds[i].events & (POLLRDBAND|POLLPRI))
FD_SET(ufds[i].fd, &fds_err);
}
}
ptimeout = (timeout_ms < 0) ? NULL : &pending_tv;
do {
if (ptimeout) {
pending_tv.tv_sec = pending_ms / 1000;
pending_tv.tv_usec = (pending_ms % 1000) * 1000;
}
r = select((int)maxfd + 1, &fds_read, &fds_write, &fds_err, ptimeout);
} while ((r == -1) && (error = SOCKERRNO) &&
(error != EINVAL) && (error != EBADF) && error_not_EINTR &&
((timeout_ms < 0) || ((pending_ms = timeout_ms - elapsed_ms) > 0)));
if (r < 0)
return -1;
if (r == 0)
return 0;
r = 0;
for (i = 0; i < nfds; i++) {
ufds[i].revents = 0;
if (ufds[i].fd == CURL_SOCKET_BAD)
continue;
if (FD_ISSET(ufds[i].fd, &fds_read))
ufds[i].revents |= POLLIN;
if (FD_ISSET(ufds[i].fd, &fds_write))
ufds[i].revents |= POLLOUT;
if (FD_ISSET(ufds[i].fd, &fds_err))
ufds[i].revents |= POLLPRI;
if (ufds[i].revents != 0)
r++;
}
#endif /* HAVE_POLL_FINE */
return r;
}
