static
void accept_control(struct evconnlistener *listener,
evutil_socket_t fd,
struct sockaddr *address,
int socklen,
void *_)
{
assert(address->sa_family == AF_INET);
memcpy(&context.control_address, address, sizeof(context.control_address));
char buffer[INET_ADDRSTRLEN];
debug( "control: from %s",
inet_ntop( AF_INET,
&context.control_address.sin_addr,
buffer,
INET_ADDRSTRLEN));
int32_t one = 1;
setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
context.control_buffers = bufferevent_socket_new(
context.events,
fd,
BEV_OPT_CLOSE_ON_FREE | BEV_OPT_DEFER_CALLBACKS);
if(!context.control_buffers)
{
perror("bufferevent_socket_new");
event_base_loopexit(context.events, NULL);
return;
}
bufferevent_setcb( context.control_buffers,
authenticate_control,
NULL,
error_on_control,
NULL);
bufferevent_setwatermark( context.control_buffers,
EV_READ,
sizeof(struct Message), BUFFER_LIMIT);
bufferevent_enable(context.control_buffers, EV_READ | EV_WRITE);
// disable new connections while authenticating
evconnlistener_set_cb(listener, NULL, NULL);
setup_control(context.control_buffers);
}
int main (void)
{
int i=0;
tmp_debug = 0;
//Setup oscillator/ports/pins first
setup_oscillator();
setup_ports();
setup_peripheral_pin_select();
setup_interrupt_priorities();
//init_temperature_model();
//Blinking HB LED and Timestamp
#ifdef USE_DIO
setup_dio();
#endif
//ToDo Used?
#ifdef USE_TIMER1
setup_timer1(); //do before setup_pwm
#endif //ToDo: there was a missing #endif, maybe that's why disabling Timer1 was breaking the code!
//Motor control PWM
#ifdef USE_PWM
//setup_pwm();
#endif
//Brushless motor
#ifdef USE_BLDC
setup_bldc();
#endif
//Torque PID and Current PID
#ifdef USE_CONTROL
setup_control();
#endif
//Analog to Digital
#ifdef USE_ADC
setup_adc();
#endif
//Torso external ADC
#ifdef USE_ADC_SPI
setup_adc_spi();
#endif
//Current measurment, control and limitation
#ifdef USE_CURRENT
setup_current();
#endif
//ToDo: Remvove?
#ifdef USE_TIMER3
setup_timer3();
#endif
//VerteX SPI Encoder
#ifdef USE_ENCODER_VERTX
setup_vertx();
#endif
//Torso motor brake
#ifdef USE_BRAKE
setup_brake();
#endif
//EtherCAT Communication
#ifdef USE_ETHERCAT
while (!eeprom_loaded()) //Wait until ESC is ready
SetHeartbeatLED;
setup_ethercat();
ClrHeartbeatLED;
#endif
setup_interrupt_priorities();
if (RCONbits.WDTO)
SetHeartbeatLED;
//RCONbits.SWDTEN = 1;//enable watchdog timer
if (RCON & 0x03 == 0x03)
{
RCONbits.BOR = 0;
RCONbits.POR = 0;
}
rcon_reg = RCON;
while(1)
{
if (i++%10001==0)
{
//ToggleHeartbeatLED();
#if defined USE_ETHERCAT
step_ethercat();
#endif
}
}
}
int main (void)
{
int dummy;
//int matrix; //only for test of ledmdrv
int i;
//int temp_current_val; //temporary
//int first_ec=1;
//Setup oscillator/ports/pins first
setup_oscillator();
setup_ports();
setup_peripheral_pin_select();
setup_interrupt_priorities();
#ifdef USE_DIO
setup_dio();
#endif
#ifdef USE_CONTROL
setup_control();
#endif
#ifdef USE_ADC_SPI
setup_adc_spi();
#endif
#ifdef USE_ADC
setup_adc();
initDma0(); // Initialise the DMA controller to buffer ADC data in conversion order
#endif
#ifdef USE_TIMER3
//setup_timer3();
#endif
#ifdef USE_UART
setup_uart();
#endif
ecat_isr_running = 0;
#ifdef USE_ETHERCAT
while (!eeprom_loaded()) //Wait until ESC is ready
//ToggleHeartbeatLED(); //Success
SetHeartbeatLED;
//#ifdef ECAT_DMA
cfgDma0SpiTx();
cfgDma1SpiRx();
//#endif
setup_ethercat();
ClrHeartbeatLED;
#endif
#if defined PWR_0_2 || defined PWR_0_3 || defined PWR_0_4 || defined PWR_0_5
ClrEnableMotor;
#endif
setup_interrupt_priorities();
dummy = U1RXREG;
while(1){
if (i++%20001==0)
{
//ToggleHeartbeatLED();
}
#if defined USE_ETHERCAT
if (!IEC0bits.DMA1IE)
{
DISABLE_AL_EVENT_INT;
step_ethercat();
ENABLE_AL_EVENT_INT;
}
#endif
}
}
void CLG_(check_command)()
{
/* check for dumps needed */
static Char buf[512];
static Char cmdBuffer[512];
Char *cmdPos = 0, *cmdNextLine = 0;
Int fd, bytesRead = 0, do_kill = 0;
SysRes res;
Int currentPID;
static Int check_counter = 0;
/* Check for PID change, i.e. whether we run as child after a fork.
* If yes, we setup interactive control for the new process
*/
currentPID = VG_(getpid)();
if (thisPID != currentPID) {
thisPID = currentPID;
setup_control();
}
/* Toggle between 2 command files, with/without ".pid" postfix
* (needed for compatibility with KCachegrind, which wants to trigger
* a dump by writing into a command file without the ".pid" postfix)
*/
check_counter++;
if (check_counter % 2) {
current_command_file = command_file;
current_result_file = result_file;
}
else {
current_command_file = command_file2;
current_result_file = result_file2;
}
res = VG_(open)(current_command_file, VKI_O_RDONLY,0);
if (!res.isError) {
fd = (Int) res.res;
bytesRead = VG_(read)(fd,cmdBuffer,500);
cmdBuffer[500] = 0; /* no command overrun please */
VG_(close)(fd);
/* don't delete command file on read error (e.g. EAGAIN) */
if (bytesRead>0) {
cmdPos = cmdBuffer;
}
}
/* force creation of result file if needed */
fd = -2;
while((bytesRead>0) && *cmdPos) {
/* Calculate pointer for next line */
cmdNextLine = cmdPos+1;
while((bytesRead>0) && *cmdNextLine && (*cmdNextLine != '\n')) {
cmdNextLine++;
bytesRead--;
}
if ((bytesRead>0) && (*cmdNextLine == '\n')) {
*cmdNextLine = 0;
cmdNextLine++;
bytesRead--;
}
/* Command with integer option */
if ((*cmdPos >= '0') && (*cmdPos <='9')) {
int value = *cmdPos-'0';
cmdPos++;
while((*cmdPos >= '0') && (*cmdPos <='9')) {
value = 10*value + (*cmdPos-'0');
cmdPos++;
}
while((*cmdPos == ' ') || (*cmdPos == '\t')) cmdPos++;
switch(*cmdPos) {
#if CLG_ENABLE_DEBUG
/* verbosity */
case 'V':
case 'v':
CLG_(clo).verbose = value;
break;
#endif
default:
break;
}
cmdPos = cmdNextLine;
continue;
}
/* Command with boolean/switch option */
if ((*cmdPos=='+') ||
(*cmdPos=='-')) {
int value = (cmdPos[0] == '+');
cmdPos++;
while((*cmdPos == ' ') || (*cmdPos == '\t')) cmdPos++;
switch(*cmdPos) {
case 'I':
case 'i':
CLG_(set_instrument_state)("Command", value);
break;
default:
break;
}
cmdPos = cmdNextLine;
continue;
}
/* regular command */
switch(*cmdPos) {
case 'D':
case 'd':
/* DUMP */
/* skip command */
while(*cmdPos && (*cmdPos != ' ')) cmdPos++;
if (*cmdPos)
VG_(sprintf)(buf, "Dump Command:%s", cmdPos);
else
VG_(sprintf)(buf, "Dump Command");
CLG_(dump_profile)(buf, False);
break;
case 'Z':
case 'z':
CLG_(zero_all_cost)(False);
break;
case 'K':
case 'k':
/* Kill: Delay to be able to remove command file before. */
do_kill = 1;
break;
case 'I':
case 'i':
fd = dump_info(fd);
break;
case 's':
case 'S':
fd = dump_state(fd);
break;
case 'O':
case 'o':
/* Options Info */
if ( (fd = createRes(fd)) <0) break;
VG_(sprintf)(buf, "\ndesc: Option: --skip-plt=%s\n",
CLG_(clo).skip_plt ? "yes" : "no");
VG_(write)(fd, (void*)buf, VG_(strlen)(buf));
VG_(sprintf)(buf, "desc: Option: --collect-jumps=%s\n",
CLG_(clo).collect_jumps ? "yes" : "no");
VG_(write)(fd, (void*)buf, VG_(strlen)(buf));
VG_(sprintf)(buf, "desc: Option: --separate-recs=%d\n",
CLG_(clo).separate_recursions);
VG_(write)(fd, (void*)buf, VG_(strlen)(buf));
VG_(sprintf)(buf, "desc: Option: --separate-callers=%d\n",
CLG_(clo).separate_callers);
VG_(write)(fd, (void*)buf, VG_(strlen)(buf));
break;
default:
break;
}
cmdPos = cmdNextLine;
}
/* If command executed, delete command file */
if (cmdPos) VG_(unlink)(current_command_file);
if (fd>=0) VG_(close)(fd);
if (do_kill) {
VG_(message)(Vg_UserMsg,
"Killed because of command from %s", current_command_file);
CLG_(fini)(0);
VG_(exit)(1);
}
}
void CLG_(init_command)()
{
thisPID = VG_(getpid)();
setup_control();
}
int main(void)
{
int cnt;
setup_ports();
read_config();
setup_timer();
setup_control();
PORTC = DISABLED;
setup_uart(9600UL);
flags = eeread(FLAGS_START);
ph_u = 0;
ph_v = 24;
ph_w = 12;
pwm_u = 0;
pwm_v = 0;
pwm_w = 0;
sei();
cnt = 0;
while (1) {
register byte i;
if ((flags & FLAG_POT) && cnt == 0)
read_speed();
cnt++;
cnt &= 0x3F;
if (flags & FLAG_GO) {
int8_t cnt_u, cnt_v, cnt_w;
int u_u, u_v, u_w;
/* Disable all phases */
PORTC &= ~((1 << EU) | (1 << EV) | (1 << EW));
/* Wait to turn mosfets off */
_delay_loop_2(delay_num * delay_step);
/* Switch all bridges to HIGH */
PORTC |= ((1<<UU) | (1<<UV) | (1<<UW));
/* Enable all phases */
PORTC |= ((1<<EU) | (1<<EV) | (1<<EW));
//_delay_loop_2(50);
cnt_u = cnt_v = cnt_w = -1;
u_u = u_v = u_w = 1;
for (i = 0; i < MAX_PWM; i++) {
if (i == pwm_u || (i > pwm_u && u_u)) {
CLRBIT(PORTC, EU); // DISABLE U
u_u = 0;
cnt_u = delay_num;
}
if (i == pwm_v || (i > pwm_v && u_v)) {
CLRBIT(PORTC, EV); // DISABLE V
u_v = 0;
cnt_v = delay_num;
}
if (i == pwm_w || (i > pwm_w && u_w)) {
CLRBIT(PORTC, EW); // DISABLE W
u_w = 0;
cnt_w = delay_num;
}
_delay_loop_2(delay_step);
if (cnt_u == 0) {
CLRBIT(PORTC, UU); // Set U to LOW
PORTC |= (1<<EU);
cnt_u = -1;
}
if (cnt_v == 0) {
CLRBIT(PORTC, UV); // Set V to LOW
PORTC |= (1<<EV);
cnt_v = -1;
}
if (cnt_w == 0) {
CLRBIT(PORTC, UW); // Set W to LOW
PORTC |= (1<<EW);
cnt_w = -1;
}
if (cnt_u > 0)
cnt_u--;
if (cnt_v > 0)
cnt_v--;
if (cnt_w > 0)
cnt_w--;
}
} else {
PORTC = DISABLED;
_delay_loop_1(250);
}
}
return 0;
}
void
setup_connections(void)
{
int i;
struct linger linger;
struct sockaddr_in s_in;
u_short port;
u_short sigport;
u_short dataport;
header = (odexm_header *) buf;
/*
* setup file descriptors
*/
for (;;) {
errno = 0;
i = dup(0);
if (i == -1) {
efatal("dup failed");
}
if (i > 2)
break;
}
errdesc = i;
if (dup2(i, 0) < 0)
efatal("dup2 #0 failed");
if (dup2(i, 1) < 0)
efatal("dup2 #1 failed");
if (dup2(i, 2) < 0)
efatal("dup2 #2 failed");
(void) close(i);
errdesc = 2;
/*
* setup main (command) connection
*/
i = 1;
if (setsockopt(0, SOL_SOCKET, SO_KEEPALIVE, (char *)&i, sizeof(i)) < 0)
ewarn("setsockopt (SO_KEEPALIVE)");
#ifdef DONT_SO_LINGER
if (setsockopt(0, SOL_SOCKET, SO_LINGER, (char *)&i, sizeof(i)) < 0 )
#else
linger.l_onoff = 1;
linger.l_linger = 60;
if (setsockopt(0, SOL_SOCKET, SO_LINGER, (char *)&linger, sizeof(linger))
< 0)
#endif
ewarn("setsockopt (SO_LINGER)");
i = sizeof(peername);
if (getpeername(0, (struct sockaddr *)&peername, &i) < 0)
efatal("getpeername");
i = sizeof(sockname);
if (getsockname(0, (struct sockaddr *)&sockname, &i) < 0)
efatal("getsockname");
if (debug)
diag("connection from %s (%d)",
inet_ntoa(peername.sin_addr), ntohs(peername.sin_port));
/*
* get flags
*/
if (read(0, &flags, sizeof(flags)) != sizeof(flags))
efatal("server read");
if ( flags == 1 || flags == 3 )
debug = 1;
/* if */
if ( flags > 1 )
batch = 1;
/* if */
/*
* setup control (signal) connection
*/
if (read(0, (char *)&sigport, sizeof(sigport)) != sizeof(sigport))
efatal("server read");
sigsock = socket(AF_INET, SOCK_STREAM, 0);
if (sigsock < 0)
efatal("socket");
memcpy (&s_in, &sockname, sizeof(s_in));
s_in.sin_port = 0;
if (bind(sigsock, (struct sockaddr *)&s_in, sizeof(s_in)) < 0)
efatal("bind");
i = sizeof(s_in);
if (getsockname(sigsock, (struct sockaddr *)&s_in, &i) < 0)
efatal("getsockname");
port = s_in.sin_port;
if (write(1, (char *)&port, sizeof(port)) != sizeof(port))
efatal("write");
memcpy(&s_in, &peername, sizeof(s_in));
s_in.sin_port = sigport;
if (connect(sigsock, (struct sockaddr *)&s_in, sizeof(s_in)) < 0)
efatal("connect");
/*
* start control process (actually parent)
*/
setup_control();
/*
* safe now for debugging
*/
/*
* setup data transfer connection
*/
if (debug)
diag("reading data port");
if (read(0, (char *)&dataport, sizeof(dataport)) != sizeof(dataport))
efatal("server read");
if (debug)
diag("data port %d", ntohs(dataport));
datasock = socket(AF_INET, SOCK_STREAM, 0);
if (datasock < 0)
efatal("socket");
memcpy(&s_in, &sockname, sizeof(s_in));
s_in.sin_port = 0;
if (bind(datasock, (struct sockaddr *)&s_in, sizeof(s_in)) < 0)
efatal("bind");
i = sizeof(s_in);
if (getsockname(datasock, (struct sockaddr *)&s_in, &i) < 0)
efatal("getsockname");
port = s_in.sin_port;
if (debug)
diag("sending data port %d", ntohs(port));
if (write(1, (char *)&port, sizeof(port)) != sizeof(port))
efatal("write");
memcpy(&s_in, &peername, sizeof(s_in));
s_in.sin_port = dataport;
if (connect(datasock, (struct sockaddr *)&s_in, sizeof(s_in)) < 0)
efatal("connect");
if (debug)
diag("connected to dataport");
if ( batch )
compat_sock = datasock;
else
compat_sock = 0;
/* if */
}
