int main(int argc, char** argv)
{
configure_port();
write(fd, "Starting session...\n\r", 21);
while(command_received != 1)
{
sleep(100);
if (command_received > 1)
{
printf("%s\n", command);
if (command_received == 3)
{
close(fd);
system("sudo /sbin/getty ttyUSB0 9600 2>>/home/jeremy/NetBeansProjects/SerialCommunication/stderr");
configure_port();
}
command_received = 0;
fflush(stdout);
}
}
printf("Done.\n");
return 0;
}
int configure_server(spade_server* server, char* configuration_path,
unsigned int override_port) {
config_t configuration_struct, *configuration;
configuration = &configuration_struct;
config_init(configuration);
if (!config_read_file(configuration, configuration_path)) {
log4c_category_log(log4c_category_get("spade"), LOG4C_PRIORITY_ERROR,
"Configuration error: %s:%d - %s",
configuration_path,
config_error_line(configuration),
config_error_text(configuration));
config_destroy(configuration);
return(EXIT_FAILURE);
}
configure_hostname(server, configuration);
configure_port(server, override_port, configuration);
configure_reverse_lookups(server, configuration);
configure_static_file_path(server, configuration);
configure_dynamic_file_paths(server, configuration);
configure_dynamic_handlers(server, configuration);
config_destroy(configuration);
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "goal_planner");
ros::NodeHandle n_;
fd = open_port();
configure_port(fd);
//fd1 = open_port1();
//configure_port(fd1);
//send_hbt();
//Create an object of class GoalPlanner that will take care of everything
GoalPlanner GPObject;
ros::Publisher pub_local_goal_position_;
pub_local_goal_position_ = n_.advertise<geometry_msgs::PoseStamped>("/local_goal_position", 1);
ros::spin();
return 0;
}
int main(int argc, char* argv[])
{
if(argc<2)
{
printf("Argument is not correct\n");
return 0;
}
int fd = open_port(argv[1]);
int fd_tty = open_terminal();
configure_port(fd);
configure_terminal(fd_tty);
char rec=' ',rec_tty=' ';
while(fd!=-1 && fd_tty!=-1)
{
int data=read(fd,&rec,1);
int data_tty=read(fd_tty,&rec_tty,1);
if(data>0)
printf("%c",rec);
if(data_tty>0)
printf("%c",rec_tty);
if(rec_tty=='q')
break;
rec=' ';rec_tty=' ';
}
close(fd);
close(fd_tty);
return(0);
}
unsigned long SerialInterface::set_baud(unsigned long b)
{
baud = b;
configure_port();
return baud;
}
/** CONSTRUCTOR */
SerialPort::SerialPort(int connect) {
_connection = (!connect);
if(_connection) {
_port_fd = create_port();
configure_port();
}
}
int SerialInterface::set_stop(int s)
{
if (s >= 1 && s <= 2)
stop = s;
configure_port();
return stop;
}
SerialInterface::SerialParity SerialInterface::set_parity(SerialParity p)
{
if (p >= 0 && p <= 2)
parity = p;
configure_port();
return parity;
}
SerialInterface::SerialFlow SerialInterface::set_flow(SerialFlow f)
{
if (f >= 0 && f <= 2)
flow = f;
configure_port();
return flow;
}
int SerialInterface::set_bits(int b)
{
if (b >= 5 && b <= 8)
bits = b;
configure_port();
return bits;
}
int main(int argc, char **argv) {
/* misc. locals... */
int nfds;
fd_set fdset;
char tmpstr[64];
if(argc != 2) {
fprintf(stderr,"Usage: %s <IPv6/mask>\n",argv[0]);
return;
}
/* open the serial port */
serial_fd = open("/dev/ttyS0", O_RDWR | O_NOCTTY);
check(serial_fd != -1,"serial open() error");
ret = configure_port(serial_fd);
check(ret != -1,"serial configure_port() error");
/* TUN interface creation */
struct ifreq ifr;
tun_fd = open(DEVTUN, O_RDWR);
check(tun_fd != -1,"tun open() error");
memset(&ifr, 0, sizeof(ifr));
ifr.ifr_flags = IFF_TUN;
ret = ioctl(tun_fd, TUNSETIFF, (void *) &ifr);
check(ret >= 0,"tun ioctl error");
/* TUN attachement to an IP address */
snprintf((char*)tmpstr, 64, "ifconfig %s inet6 add %s",ifr.ifr_name,argv[1]);
ret = system((char*)tmpstr);
check(ret != -1,"system() ifocnfig add error");
snprintf((char*)tmpstr, 64, "ifconfig %s up",ifr.ifr_name);
ret = system((char*)tmpstr);
check(ret != -1,"system() ifconfig up error");
/* main loop */
nfds = MAX(tun_fd, serial_fd) + 1;
FD_ZERO(&fdset);
printf("Started\n");
while(1) {
FD_SET(tun_fd, &fdset);
FD_SET(serial_fd, &fdset);
ret = select(nfds, &fdset, NULL, NULL, NULL);
check(ret != -1,"select() error");
/* wait for something to read on tun_fd or socket_fd */
if(ret) {
if(FD_ISSET(tun_fd, &fdset)) {
read_from_tun();
}
if(FD_ISSET(serial_fd, &fdset)) {
read_from_serial();
}
}
}
return 0;
}
edison_serial::edison_serial(std::string port, speed_t baud)
: m_port("/dev/ttyMFD1"), m_fd(0), m_port_ready(false),
m_bt_connected(false)
{
m_port = port;
if (open_port()) {
configure_port(baud);
m_port_ready = true;
}
}
static void i2c_init(void)
{
int i;
/* Configure GPIOs */
gpio_config_module(MODULE_I2C, 1);
for (i = 0; i < i2c_ports_used; ++i)
configure_port(i2c_ports[i].port, i2c_ports[i].kbps);
}
static int open_interface(struct cp210x_transport *tr,
struct usb_device *dev, int ino,
int baud_rate)
{
#if defined(__linux__)
int drv;
char drName[256];
#endif
printc_dbg(__FILE__": Trying to open interface %d on %s\n",
ino, dev->filename);
tr->int_number = ino;
tr->handle = usb_open(dev);
if (!tr->handle) {
pr_error(__FILE__": can't open device");
return -1;
}
#if defined(__linux__)
drv = usb_get_driver_np(tr->handle, tr->int_number, drName,
sizeof(drName));
printc(__FILE__" : driver %d\n", drv);
if (drv >= 0) {
if (usb_detach_kernel_driver_np(tr->handle,
tr->int_number) < 0)
pr_error(__FILE__": warning: can't detach "
"kernel driver");
}
#endif
#ifdef __Windows__
if (usb_set_configuration(tr->handle, 1) < 0) {
pr_error(__FILE__": can't set configuration 1");
usb_close(tr->handle);
return -1;
}
#endif
if (usb_claim_interface(tr->handle, tr->int_number) < 0) {
pr_error(__FILE__": can't claim interface");
usb_close(tr->handle);
return -1;
}
if (configure_port(tr, baud_rate) < 0) {
printc_err("Failed to configure for V1 device\n");
usb_close(tr->handle);
return -1;
}
return 0;
}
static void
kms_alpha_blending_set_port_properties (KmsAlphaBlending * self,
GstStructure * properties)
{
gint port, z_order;
gfloat relative_x, relative_y, relative_width, relative_height;
KmsAlphaBlendingData *port_data;
gboolean fields_ok = TRUE;
GST_DEBUG ("setting port properties");
fields_ok = fields_ok
&& gst_structure_get (properties, "relative_x", G_TYPE_FLOAT, &relative_x,
NULL);
fields_ok = fields_ok
&& gst_structure_get (properties, "relative_y", G_TYPE_FLOAT, &relative_y,
NULL);
fields_ok = fields_ok
&& gst_structure_get (properties, "relative_width", G_TYPE_FLOAT,
&relative_width, NULL);
fields_ok = fields_ok
&& gst_structure_get (properties, "relative_height", G_TYPE_FLOAT,
&relative_height, NULL);
fields_ok = fields_ok
&& gst_structure_get (properties, "port", G_TYPE_INT, &port, NULL);
fields_ok = fields_ok
&& gst_structure_get (properties, "z_order", G_TYPE_INT, &z_order, NULL);
if (!fields_ok) {
GST_WARNING_OBJECT (self, "Invalid properties structure received");
return;
}
KMS_ALPHA_BLENDING_LOCK (self);
port_data = g_hash_table_lookup (self->priv->ports, GINT_TO_POINTER (port));
if (port_data == NULL) {
KMS_ALPHA_BLENDING_UNLOCK (self);
return;
}
port_data->relative_x = relative_x;
port_data->relative_y = relative_y;
port_data->relative_width = relative_width;
port_data->relative_height = relative_height;
port_data->z_order = z_order;
port_data->configured = TRUE;
configure_port (port_data);
KMS_ALPHA_BLENDING_UNLOCK (self);
}
int open_port(const char id)
{
int fd; // file description for the serial port
while(1)
for(int i = 0; i < 10; i++) {
char dev_name[100];
sprintf(dev_name, "/dev/ttyUSB%d", i);
fd = open(dev_name, O_RDWR | O_NOCTTY);
if(fd == -1) // if open is unsucessful
{
printf("Unable to open %s. \n", dev_name);
goto failed;
}
else
{
printf("port open: %s.\n", dev_name);
fd = configure_port(fd);
// Allow arduino to run past bootloader
sleep(3);
printf("Requesting identity...\n");
write_data(fd, "I", 1);
// Wait for arduino reply
sleep(1);
char response[100]; // Response will be of form "iX" where X is the
// arduino type character (e.g. 'F' for freduino)
if(read_data(fd, response, 100) >= 2) {
printf("Device type: %c\n", response[1]);
// Compare upper case because serial has a strange
// habit of changing message cases... weird
if(toupper(id) == toupper(response[1])) {
printf("Found correct device!\n");
return fd;
} else {
printf("Wrong device!\n");
goto failed;
}
} else {
printf("No device type found...");
goto failed;
}
break;
}
failed:
close(fd);
}
return(fd);
} //open_port
static void reset_port(int port)
{
int i;
MEC1322_I2C_CONFIG(port) |= 1 << 9;
udelay(100);
MEC1322_I2C_CONFIG(port) &= ~(1 << 9);
for (i = 0; i < i2c_ports_used; ++i)
if (port == i2c_ports[i].port) {
configure_port(i2c_ports[i].port, i2c_ports[i].kbps);
break;
}
}
static void
kms_alpha_blending_reconfigure_ports (KmsAlphaBlending * self)
{
GstCaps *filtercaps;
GList *l;
GList *values = g_hash_table_get_values (self->priv->ports);
values = g_list_sort (values, (GCompareFunc) compare_port_data);
for (l = values; l != NULL; l = l->next) {
KmsAlphaBlendingData *port_data = l->data;
if (port_data->input == FALSE) {
continue;
}
if (port_data->id == self->priv->master_port) {
filtercaps =
gst_caps_new_simple ("video/x-raw", "format", G_TYPE_STRING, "AYUV",
"width", G_TYPE_INT, self->priv->output_width, "height",
G_TYPE_INT, self->priv->output_height, NULL);
if (port_data->capsfilter != NULL) {
g_object_set (G_OBJECT (port_data->capsfilter), "caps", filtercaps,
NULL);
}
if (port_data->video_mixer_pad != NULL) {
g_object_set (port_data->video_mixer_pad, "xpos", 0, "ypos", 0, "alpha",
1.0, "zorder", self->priv->z_master, NULL);
}
} else {
configure_port (port_data);
}
}
g_list_free (values);
//reconfigure videotestsrc input
filtercaps =
gst_caps_new_simple ("video/x-raw", "format", G_TYPE_STRING, "AYUV",
"width", G_TYPE_INT, self->priv->output_width, "height",
G_TYPE_INT, self->priv->output_height, "framerate", GST_TYPE_FRACTION, 15,
1, NULL);
g_object_set (G_OBJECT (self->priv->videotestsrc_capsfilter), "caps",
filtercaps, NULL);
gst_caps_unref (filtercaps);
}
int open_port(char *port) {
char logmsg[255];
snprintf(logmsg, 255, "Opening port ...");
rblog(RBLOG_DEBUG, logmsg);
int fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
snprintf(logmsg, 255, "opened port, fd is: %d", fd);
rblog(RBLOG_DEBUG, logmsg);
// TODO: use errno?
if (fd == -1) {
// if open is unsucessful
snprintf(logmsg, 255, "Unable to open serial port, exiting");
rblog(RBLOG_ERROR, logmsg);
exit(1);
} else {
configure_port(fd, B38400);
fcntl(fd, F_SETFL, O_NONBLOCK);
}
return(fd);
}
SerialInterface::SerialStatus SerialInterface::open_port()
{
#ifdef _WIN32
COMMTIMEOUTS timeouts = {0};
#endif
close_port();
#ifdef __unix__
port_fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
#elif defined _WIN32
h_port = CreateFile(("\\\\.\\" + port).c_str(),
GENERIC_READ | GENERIC_WRITE,
0,
0,
OPEN_EXISTING,
//FILE_ATTRIBUTE_NORMAL,
FILE_FLAG_OVERLAPPED,
0);
#endif
if (!is_open())
{
std::cerr << "Error opening port " + port << std::endl;
return SS_Error;
}
#ifdef __unix__
tcgetattr(port_fd, &port_termios);
memcpy(&port_termios_saved, &port_termios, sizeof(struct termios));
configure_port();
tcflush(port_fd, TCOFLUSH);
tcflush(port_fd, TCIFLUSH);
#elif defined _WIN32
if (!SetCommMask(h_port, EV_RXCHAR))
{
std::cerr << "Error setting mask!" << std::endl;
close_port();
return SS_Error;
}
memset(&dcb_serial_params, 0, sizeof(DCB));
dcb_serial_params.DCBlength = sizeof(DCB);
if (!GetCommState(h_port, &dcb_serial_params)) {
std::cerr << "Error getting state!" << std::endl;
close_port();
return SS_Error;
}
memcpy(&dcb_serial_params_saved, &dcb_serial_params, sizeof(dcb_serial_params));
configure_port();
if(!GetCommTimeouts(h_port, &timeouts)){
std::cerr << "Error getting timeouts!" << std::endl;
close_port();
return SS_Error;
}
timeouts.ReadIntervalTimeout = MAXDWORD;
timeouts.ReadTotalTimeoutConstant = 0;
timeouts.ReadTotalTimeoutMultiplier = 0;
//timeouts.WriteTotalTimeoutConstant = 50;
//timeouts.WriteTotalTimeoutMultiplier = 10;
if(!SetCommTimeouts(h_port, &timeouts)){
std::cerr << "Error setting timeouts!" << std::endl;
close_port();
return SS_Error;
}
h_overlapped = CreateEvent(0, true, false, 0);
if (h_overlapped == INVALID_HANDLE_VALUE)
{
std::cerr << "Error creating event (1)!" << std::endl;
close_port();
return SS_Error;
}
h_overlapped_thread = CreateEvent(0, true, false, 0);
if (h_overlapped_thread == INVALID_HANDLE_VALUE)
{
std::cerr << "Error creating event (2)!" << std::endl;
close_port();
return SS_Error;
}
#endif
launch_select_thread();
if (debug)
std::cout << "Port opened." << std::endl;
m_port_opened.emit();
return SS_Success;
}
static GstPadProbeReturn
link_to_videomixer (GstPad * pad, GstPadProbeInfo * info,
KmsAlphaBlendingData * data)
{
GstPadTemplate *sink_pad_template;
KmsAlphaBlending *mixer = data->mixer;
if (GST_EVENT_TYPE (GST_PAD_PROBE_INFO_EVENT (info)) != GST_EVENT_CAPS) {
return GST_PAD_PROBE_PASS;
}
GST_DEBUG ("stream start detected");
KMS_ALPHA_BLENDING_LOCK (mixer);
data->link_probe_id = 0;
sink_pad_template =
gst_element_class_get_pad_template (GST_ELEMENT_GET_CLASS (mixer->
priv->videomixer), "sink_%u");
if (G_UNLIKELY (sink_pad_template == NULL)) {
GST_ERROR_OBJECT (mixer, "Error taking a new pad from videomixer");
KMS_ALPHA_BLENDING_UNLOCK (mixer);
return GST_PAD_PROBE_DROP;
}
if (mixer->priv->master_port == data->id) {
//master_port, reconfigurate the output_width and heigth_width
//and all the ports already created
GstEvent *event;
GstCaps *caps;
gint width, height;
const GstStructure *str;
event = gst_pad_probe_info_get_event (info);
gst_event_parse_caps (event, &caps);
GST_DEBUG ("caps %" GST_PTR_FORMAT, caps);
if (caps != NULL) {
str = gst_caps_get_structure (caps, 0);
if (gst_structure_get_int (str, "width", &width) &&
gst_structure_get_int (str, "height", &height)) {
mixer->priv->output_height = height;
mixer->priv->output_width = width;
}
}
}
if (mixer->priv->videotestsrc == NULL) {
GstCaps *filtercaps;
GstPad *pad;
mixer->priv->videotestsrc = gst_element_factory_make ("videotestsrc", NULL);
mixer->priv->videotestsrc_capsfilter =
gst_element_factory_make ("capsfilter", NULL);
g_object_set (mixer->priv->videotestsrc, "is-live", TRUE, "pattern",
/*black */ 2, NULL);
filtercaps =
gst_caps_new_simple ("video/x-raw", "format", G_TYPE_STRING, "AYUV",
"width", G_TYPE_INT, mixer->priv->output_width,
"height", G_TYPE_INT, mixer->priv->output_height,
"framerate", GST_TYPE_FRACTION, 15, 1, NULL);
g_object_set (G_OBJECT (mixer->priv->videotestsrc_capsfilter), "caps",
filtercaps, NULL);
gst_caps_unref (filtercaps);
gst_bin_add_many (GST_BIN (mixer), mixer->priv->videotestsrc,
mixer->priv->videotestsrc_capsfilter, NULL);
gst_element_link (mixer->priv->videotestsrc,
mixer->priv->videotestsrc_capsfilter);
/*link capsfilter -> videomixer */
pad = gst_element_request_pad (mixer->priv->videomixer,
sink_pad_template, NULL, NULL);
gst_element_link_pads (mixer->priv->videotestsrc_capsfilter, NULL,
mixer->priv->videomixer, GST_OBJECT_NAME (pad));
g_object_set (pad, "xpos", 0, "ypos", 0, "alpha", 0.0, "zorder", 0, NULL);
g_object_unref (pad);
gst_element_sync_state_with_parent (mixer->priv->videotestsrc_capsfilter);
gst_element_sync_state_with_parent (mixer->priv->videotestsrc);
}
data->videoscale = gst_element_factory_make ("videoscale", NULL);
data->capsfilter = gst_element_factory_make ("capsfilter", NULL);
data->videorate = gst_element_factory_make ("videorate", NULL);
data->queue = gst_element_factory_make ("queue", NULL);
data->videobox = gst_element_factory_make ("videobox", NULL);
data->input = TRUE;
gst_bin_add_many (GST_BIN (mixer), data->queue, data->videorate,
data->videoscale, data->capsfilter, data->videobox, NULL);
g_object_set (data->videorate, "average-period", 200 * GST_MSECOND, NULL);
g_object_set (data->queue, "flush-on-eos", TRUE, NULL);
gst_element_link_many (data->videorate, data->queue, data->videoscale,
data->capsfilter, data->videobox, NULL);
/*link capsfilter -> videomixer */
data->video_mixer_pad =
gst_element_request_pad (mixer->priv->videomixer,
sink_pad_template, NULL, NULL);
gst_element_link_pads (data->videobox, NULL,
mixer->priv->videomixer, GST_OBJECT_NAME (data->video_mixer_pad));
gst_element_link (data->videoconvert, data->videorate);
data->probe_id = gst_pad_add_probe (data->video_mixer_pad,
GST_PAD_PROBE_TYPE_EVENT_DOWNSTREAM,
(GstPadProbeCallback) cb_EOS_received,
KMS_ALPHA_BLENDING_REF (data), (GDestroyNotify) kms_ref_struct_unref);
gst_element_sync_state_with_parent (data->videoscale);
gst_element_sync_state_with_parent (data->capsfilter);
gst_element_sync_state_with_parent (data->videorate);
gst_element_sync_state_with_parent (data->queue);
gst_element_sync_state_with_parent (data->videobox);
/* configure videomixer pad */
mixer->priv->n_elems++;
if (mixer->priv->master_port == data->id) {
kms_alpha_blending_reconfigure_ports (mixer);
} else {
configure_port (data);
}
KMS_ALPHA_BLENDING_UNLOCK (mixer);
return GST_PAD_PROBE_REMOVE;
}
int main(int argc, char *argv[])
{
int ret = ERROR_OK;
int err_count = 0;
int rcv_err_count = 0;
bool msg_received = false;
char* device;
/* Para parsear el los mensajes se recorre el arreglo con un puntero
* a enteros de dos bytes.
*/
int16_t *ch_buff;
//char str[128]; // dbg
// check input arguments
if(argc<2)
{
err_log(HOW_TO);
return -1;
}
else device = argv[1];
struct timeval tv_in;
struct timeval tv_end;
struct timeval tv_diff;
#if DEBUG_TIMING_SBUSD
struct timeval tv_last;
#endif //#if DEBUG_TIMING_SBUSD
#if !PC_TEST
fd = open_port(device);
if (fd == -1)
{
return -1;
}
configure_port(fd);
ret = custom_baud(fd);
if (ret < 0)
{
err_log_stderr("custom_baud() failed!");
return ret;
}
#endif
/**
* Inherit priority from main.c for correct IPC.
*/
if(setpriority(PRIO_PROCESS, 0, -18) == -1) //requires being superuser
{
err_log_num("setpriority() failed!",errno);
return -1;
}
// Catch signals
prctl(PR_SET_PDEATHSIG, SIGHUP);
signal(SIGHUP, uquad_sig_handler);
signal(SIGINT, uquad_sig_handler);
signal(SIGQUIT, uquad_sig_handler);
#if PC_TEST
futaba_sbus_begin(); //para tiempo de start
#endif //PC_TEST
// Lleva a cero todos los canales y el mensaje sbus
futaba_sbus_set_channel(ROLL_CHANNEL, 1500); //init roll en cero
futaba_sbus_set_channel(PITCH_CHANNEL, 1500); //init pitch en cero
futaba_sbus_set_channel(YAW_CHANNEL, 1500); //init yaw en cero
futaba_sbus_set_channel(THROTTLE_CHANNEL, 950); //init throttle en minimo
futaba_sbus_set_channel(FLIGHTMODE_CHANNEL, 1500); //inint flight mode 2
futaba_sbus_update_msg();
sleep_ms(500); //Para ponerme a tiro con main
bool main_ready = false;
// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
// Loop
// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
for(;;)
{
gettimeofday(&tv_in,NULL);
//printf("errores: %d\n",err_count);//dbg
#if !PC_TEST
if (err_count > MAX_ERR_SBUSD)
{
err_log("error count exceded");
//err_count = 0;
quit();
}
#endif
ret = uquad_read(&rbuf);
if(ret == ERROR_OK)
{
if(!main_ready) main_ready = true;
//err_log("read ok!");
msg_received = true;
// Parse message. 2 bytes per channel.
ch_buff = (int16_t *)rbuf.mtext;
// send ack
ret = uquad_send_ack();
if(ret != ERROR_OK)
{
err_log("Failed to send ack!");
}
if (rcv_err_count > 0)
rcv_err_count = 0;
} else {
//err_log("Failed to read msg!");
msg_received = false;
rcv_err_count++;
if (main_ready && rcv_err_count > 3) {
err_count++;
rcv_err_count = 0;
}
}
if(msg_received)
{
futaba_sbus_set_channel(ROLL_CHANNEL, ch_buff[ROLL_CH_INDEX]);
futaba_sbus_set_channel(PITCH_CHANNEL, ch_buff[PITCH_CH_INDEX]);
futaba_sbus_set_channel(YAW_CHANNEL, ch_buff[YAW_CH_INDEX]);
futaba_sbus_set_channel(THROTTLE_CHANNEL, ch_buff[THROTTLE_CH_INDEX]);
//futaba_sbus_set_channel(7, ch_buff[FLIGHTMODE_CH_INDEX]); // flight mode no se modifica
// Comando para activar failsafe
if ( (ch_buff[FAILSAFE_CH_INDEX] == ACTIVATE_FAILSAFE) &&
(futaba_sbus_get_failsafe() == SBUS_SIGNAL_OK) )
futaba_sbus_set_failsafe(SBUS_SIGNAL_FAILSAFE);
// Comando para desactivar failsafe
if ( (ch_buff[FAILSAFE_CH_INDEX] == DEACTIVATE_FAILSAFE) &&
(futaba_sbus_get_failsafe() == SBUS_SIGNAL_FAILSAFE) )
futaba_sbus_set_failsafe(SBUS_SIGNAL_OK);
futaba_sbus_update_msg();
msg_received = false;
//print_sbus_data(); // dbg
}
#if !PC_TEST
ret = futaba_sbus_write_msg(fd);
if (ret < 0)
{
err_count++; // si fallo al enviar el mensaje se apagan los motores!!
}
else
{
/// This loop was fine
if(err_count > 0)
err_count--;
}
sleep_ms(5); //TODO revisar si hay que hacerlo siempre!!
#else
sleep_ms(5); //TODO revisar si hay que hacerlo siempre!!
#endif
// Escribe el mensaje a stdout - dbg
//convert_sbus_data(str);
//printf("%s",str);
/// Control de tiempo
gettimeofday(&tv_end,NULL);
ret = uquad_timeval_substract(&tv_diff, tv_end, tv_in);
if(ret > 0)
{
if(tv_diff.tv_usec < LOOP_T_US)
usleep(LOOP_T_US - (unsigned long)tv_diff.tv_usec);
#if DEBUG_TIMING_SBUSD
gettimeofday(&tv_end,NULL);
uquad_timeval_substract(&tv_diff, tv_end, tv_in);
printf("duracion loop sbusd (14ms): %lu\n",(unsigned long)tv_diff.tv_usec);
#endif
} else {
err_log("WARN: Absurd timing!");
err_count++; // si no cumplo el tiempo de loop falla la comunicacion sbus
}
//printf("rcv_err_count: %d\n", rcv_err_count);
//printf("err_count: %d\n", err_count);
} //for(;;)
return 0; //never gets here
}
/**
* \brief Main Application Routine \n
* - Initialize the system clocks \n
* NOTE: The clock should be configured in conf_clock.h \n
* - Configure port pins (PA14 and PA16) are used here \n
* - Enable Global Interrupt \n
* - Configure and enable USART \n
* - Configure and enable ADC \n
* - Configure and enable DMAC and EVSYS if DMAC mode is chosen \n
* - Start first ADC conversion \n
* - Count idle loop count in forever loop \n
*/
int main(void)
{
/* Initialize system clocks */
system_init();
#if defined(ENABLE_PORT_TOGGLE)
/* Configure PORT pins PA14 and PA16 are configured here
* NOTE: Use oscilloscope to probe the pin.
*/
configure_port();
#endif
/* ENable Global interrupt */
system_interrupt_enable_global();
/* Start SysTick Timer */
systick_init();
/* Configure SERCOM - USART */
configure_usart();
/* Configure and enable ADC */
configure_adc();
/* Configure and enable EVSYS */
configure_event();
/* Configure and enable DMA channel and descriptor */
configure_dma();
/* Get the time stamp 1 before starting ADC transfer */
time_stamp1 = SysTick->VAL;
/*
* Trigger first ADC conversion through software.
* NOTE: In case of using DMA, further conversions are triggered through
* event generated when previous ADC result is transferred to destination
* (can be USART DATA register [or] RAM buffer).
* When DMA is not used, further conversions are triggered via software in
* ADC handler after each result ready.
*/
adc_start_conversion(&adc_instance);
while (1){
#if defined (ENABLE_PORT_TOGGLE)
/* Use oscilloscope to probe the pin. */
port_base->OUTTGL.reg = (1UL << PIN_PA16 % 32 );
#endif
/* Increment idle count whenever application reached while(1) loop */
idle_loop_count++;
/*
* Check if 1024 bytes transfer is done in either case (I.e. with or without
* using DMA.
* 'adc_conv_done' flag is set to true in the ADC handler once
* 'adc_sample_count' reaches BLOCK_COUNT.
* 'adc_dma_transfer_is_done' is set to true once DMA transfer is done
* in DMA call back for channel zero when 'ADC_DMAC_USART' is chosen.
* When choosing ADC_DMAC_MEM_MEM_USART mode, 'adc_dma_transfer_is_done'
* is set to true in DMA channel call back for channel 2.
* DMA channel is disabled once reaching BLOCK_COUNT (with DMA cases).
* ADC is disabled once reaching BLOBK_COUNT samples (without DMA cases).
*/
if (adc_dma_transfer_is_done == true){
/*
* Calculate number of cycles taken from the time stamp
* taken before start of the conversion and after 1024 transfer
* is completed.
* NOTE: This value in relation to the idle_loop_count is
* used in calculating CPU usage.
*/
cycles_taken = calculate_cycles_taken(time_stamp1,time_stamp2);
/* Write the CPU cycles taken on USART */
usart_write_buffer_wait(&usart_instance, (uint8_t *)&cycles_taken, sizeof(cycles_taken));
/* Print idle loop count on USART */
usart_write_buffer_wait(&usart_instance,(uint8_t *)&idle_loop_count, sizeof(idle_loop_count));
/*
* Enter into forever loop as all transfers are completed and
* DMAC/ADC is disabled
*/
while(1);
}
}
}//end of main
static int parse_slot_config(int slot,
const unsigned char *buf,
struct eeprom_eisa_slot_info *es,
struct resource *io_parent,
struct resource *mem_parent)
{
int res=0;
int function_len;
unsigned int pos=0;
unsigned int maxlen;
int num_func=0;
u_int8_t flags;
int p0;
char *board;
int id_string_used=0;
if (NULL == (board = kmalloc(8, GFP_KERNEL))) {
return -1;
}
print_eisa_id(board, es->eisa_slot_id);
printk(KERN_INFO "EISA slot %d: %s %s ",
slot, board, es->flags&HPEE_FLAG_BOARD_IS_ISA ? "ISA" : "EISA");
maxlen = es->config_data_length < HPEE_MAX_LENGTH ?
es->config_data_length : HPEE_MAX_LENGTH;
while ((pos < maxlen) && (num_func <= es->num_functions)) {
pos+=configure_function(buf+pos, &function_len);
if (!function_len) {
break;
}
num_func++;
p0 = pos;
pos += configure_choise(buf+pos, &flags);
if (flags & HPEE_FUNCTION_INFO_F_DISABLED) {
/* function disabled, skip silently */
pos = p0 + function_len;
continue;
}
if (flags & HPEE_FUNCTION_INFO_CFG_FREE_FORM) {
/* I have no idea how to handle this */
printk("function %d have free-form confgiuration, skipping ",
num_func);
pos = p0 + function_len;
continue;
}
/* the ordering of the sections need
* more investigation.
* Currently I think that memory comaed before IRQ
* I assume the order is LSB to MSB in the
* info flags
* eg type, memory, irq, dma, port, HPEE_PORT_init
*/
if (flags & HPEE_FUNCTION_INFO_HAVE_TYPE) {
pos += configure_type_string(buf+pos);
}
if (flags & HPEE_FUNCTION_INFO_HAVE_MEMORY) {
id_string_used=1;
pos += configure_memory(buf+pos, mem_parent, board);
}
if (flags & HPEE_FUNCTION_INFO_HAVE_IRQ) {
pos += configure_irq(buf+pos);
}
if (flags & HPEE_FUNCTION_INFO_HAVE_DMA) {
pos += configure_dma(buf+pos);
}
if (flags & HPEE_FUNCTION_INFO_HAVE_PORT) {
id_string_used=1;
pos += configure_port(buf+pos, io_parent, board);
}
if (flags & HPEE_FUNCTION_INFO_HAVE_PORT_INIT) {
pos += configure_port_init(buf+pos);
}
if (p0 + function_len < pos) {
printk("\n" KERN_ERR "eisa_enumerator: function %d length mis-match "
"got %d, expected %d\n",
num_func, pos-p0, function_len);
res=-1;
break;
}
pos = p0 + function_len;
}
printk("\n");
if (!id_string_used) {
kfree(board);
}
if (pos != es->config_data_length) {
printk(KERN_ERR "eisa_enumerator: config data length mis-match got %d, expected %d\n",
pos, es->config_data_length);
res=-1;
}
if (num_func != es->num_functions) {
printk(KERN_ERR "eisa_enumerator: number of functions mis-match got %d, expected %d\n",
num_func, es->num_functions);
res=-2;
}
return res;
}