void rt_hw_uart_init(void)
{
// init uart
set_baudrate(115200);
IOWR_ALTERA_AVALON_UART_CONTROL(RS232_BASE, 0x80);//接收中断使能
IOWR_ALTERA_AVALON_UART_STATUS(RS232_BASE, 0x0); // clean status
alt_irq_register(RS232_IRQ, NULL, uart_isr);
// register device
uart_device.type = RT_Device_Class_Char;
/* device interface */
uart_device.init = rt_uart_init;
uart_device.open = rt_uart_open;
uart_device.close = rt_uart_close;
uart_device.read = rt_uart_read;
uart_device.write = rt_uart_write;
uart_device.control = rt_uart_control;
uart_device.user_data = RT_NULL;
uart_device.rx_indicate = RT_NULL;
uart_device.tx_complete = RT_NULL;
rt_device_register(&uart_device,
"uart", RT_DEVICE_FLAG_RDWR | RT_DEVICE_FLAG_STREAM | RT_DEVICE_FLAG_INT_RX);
}
int can_setup(int baudrate)
{
spi__init();
mcp2515_reset();
/* Wait until reset finishes. */
int count = 100;
while (1) {
if (get_mode() == REQOP_CONFIG) {
break;
}
else {
udelay(1);
count--;
}
if (count <= 0) {
printf("CAN controller not responding. Check daughter board connection!\n");
return -1;
}
}
set_baudrate(baudrate);
enable_intrrupt();
set_mode(REQOP_NORMAL);
return 0;
}
void scan_serports(void)
{
word *port_addr = (word *)0x400;
int i, io, irq;
mid_t mbox = __allocMbox( 50 );
i = 0;
/*
for(i=0; i < 4; i++, port_addr++)
{
if(*port_addr != 0) {
switch(i) {
case 0:
io = SER1_BASE;
irq = 4;
break;
case 1:
io = SER2_BASE;
irq = 3;
break;
case 2:
io = SER3_BASE;
irq = 4;
break;
case 3:
io = SER4_BASE;
irq = 3;
break;
default:
break;
}
print("SER%d: found. Addr: 0x%x IRQ: 0x%02x\n", i + 1, io, irq);
/ *
if((i == 0) || (i == 2)) {
register_irq(IRQ4);
add_ISR(&serial, IRQ4);
}
else {
register_irq(IRQ3);
add_ISR(&serial, IRQ3);
}
* /
}
}
*/
__sleep( 1000 );
__registerInt(mbox, IRQ4);
__registerInt(mbox, IRQ3);
set_dlab(1, SER1_BASE);
set_baudrate(S9600BAUD, SER1_BASE);
set_line_ctrl(0x03, SER1_BASE);
ser_fifo_ctrl(0xC7, SER1_BASE);
set_modem_ctl(0x0B, SER1_BASE);
ser_enable_int(0x07, SER1_BASE);
serialHandler();
}
/******* SerialUnit::SetBaudrate() **********************************/
BOOL serialunit_setbaudrate(OOP_Class *cl, OOP_Object *o, struct pHidd_SerialUnit_SetBaudrate *msg)
{
struct HIDDSerialUnitData * data = OOP_INST_DATA(cl, o);
BOOL valid = FALSE;
if (msg->baudrate != data->baudrate) {
valid = set_baudrate(data, msg->baudrate);
} /* if */
return valid;
}
static rt_err_t rt_uart_init (rt_device_t dev)
{
set_baudrate(115200);
IOWR_ALTERA_AVALON_UART_CONTROL(RS232_BASE, 0x80);//接收中断使能
IOWR_ALTERA_AVALON_UART_STATUS(RS232_BASE, 0x0); // clean status
rx_put_index = 0;
rx_get_index = 0;
return RT_EOK;
}
/******* SerialUnit::New() ***********************************/
static OOP_Object *serialunit_new(OOP_Class *cl, OOP_Object *obj, struct pRoot_New *msg)
{
struct HIDDSerialUnitData * data;
struct TagItem *tag, *tstate;
ULONG unitnum = 0;
EnterFunc(bug("SerialUnit::New()\n"));
tstate = msg->attrList;
while ((tag = NextTagItem(&tstate))) {
ULONG idx;
#define csd CSD(cl->UserData)
if (IS_HIDDSERIALUNIT_ATTR(tag->ti_Tag, idx)) {
#undef csd
switch (idx)
{
case aoHidd_SerialUnit_Unit:
unitnum = (ULONG)tag->ti_Data;
break;
}
}
} /* while (tags to process) */
obj = (OOP_Object *)OOP_DoSuperMethod(cl, obj, (OOP_Msg)msg);
if (obj) {
WORD dummy;
data = OOP_INST_DATA(cl, obj);
data->baseaddr = bases[unitnum];
data->datalength = 8;
data->parity = FALSE;
data->baudrate = 0; /* will be initialize in set_baudrate() */
data->unitnum = unitnum;
CSD(cl->UserData)->units[data->unitnum] = data;
D(bug("Unit %d at 0x0%x\n", data->unitnum, data->baseaddr));
/* Init UART - See 14-10 of dragonball documentation */
serial_out_w(data,USTCNT, UEN | RXEN);
dummy = RREG_W(URX1);
serial_out_w(data, USTCNT, (get_ustcnt(data) | UEN | RXEN | TXEN));
set_baudrate(data, SER_DEFAULT_BAUDRATE);
} /* if (obj) */
ReturnPtr("SerialUnit::New()", OOP_Object *, obj);
}
int main(void)
{
int baudrate = DEFAULT_BAUDRATE;
unsigned char ch = DEFAULT_CHAR;
#ifdef INTERACTIVE
get_user_info(&ch, &baudrate);
#endif
#ifdef SAPC
set_baudrate(COM1, baudrate);
#endif
/* do experiment--you fill in */
return 0;
}
static int gsmd_open_serial(char *device, int bps, int hwflow)
{
int fd;
/* use direct access to device node ([virtual] tty device) */
fd = open(device, O_RDWR);
if (fd < 0) {
fprintf(stderr, "can't open device `%s': %s\n", device, strerror(errno));
exit(1);
}
if (set_baudrate(fd, bps, hwflow) < 0) {
fprintf(stderr, "can't set baudrate\n");
exit(1);
}
return fd;
}
int can_tx_setup(int baudrate)
{
mcp2515_reset();
/* Wait until reset finishes. */
while (1) {
if (get_mode() == REQOP_CONFIG) {
break;
}
}
set_baudrate(baudrate);
enable_intrrupt();
enable_rollover();
set_rx_mode(0, RXM_ANY);
set_mode(REQOP_NORMAL);
return 0;
}
DWORD CPco_cl_com::PCO_SetTransferParameter(void* buf,int length)
{
PCO_SC2_CL_TRANSFER_PARAM par;
DWORD err=PCO_NOERROR;
par.ClockFrequency=transferpar.ClockFrequency;
par.CCline=transferpar.CCline;
par.Transmit=transferpar.Transmit;
par.DataFormat=transferpar.DataFormat;
memcpy(&par,buf,length);
if(par.baudrate!=transferpar.baudrate)
err=set_baudrate(par.baudrate);
if(err==PCO_NOERROR)
err=set_cl_config(par);
err=get_actual_cl_config();
return err;
}
void NRF24L01::set_rx_mode()
{
ce->reset();
set_addr_width(5);
set_local_addr(0,rx_addr_0);//写RX节点地址
set_chanal_ack(0,ENABLE); //使能通道0的自动应答
set_chanal(0,ENABLE);//使能通道0的接收地址
set_rf_frq(40); //设置RF通信频率
set_pload_width(0,4);//选择通道0的有效数据宽度
set_gain(RF_N_0DB);//设置TX发射参数,0db增益,低噪声增益开启
set_baudrate(_2MBPS);//2Mbps
set_power(1);//PWR_UP
set_tx_rx_mode(RX_MODE);//接收模式
set_crc(1,1);//EN_CRC,16BIT_CRC
ce->set();
}
int
UBX::configure(unsigned &baudrate)
{
_waiting_for_ack = true;
/* try different baudrates */
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
for (int baud_i = 0; baud_i < 5; baud_i++) {
baudrate = baudrates_to_try[baud_i];
set_baudrate(_fd, baudrate);
/* Send a CFG-PRT message to set the UBX protocol for in and out
* and leave the baudrate as it is, we just want an ACK-ACK from this
*/
type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
/* Set everything else of the packet to 0, otherwise the module wont accept it */
memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
_clsID_needed = UBX_CLASS_CFG;
_msgID_needed = UBX_MESSAGE_CFG_PRT;
/* Define the package contents, don't change the baudrate */
cfg_prt_packet.clsID = UBX_CLASS_CFG;
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
cfg_prt_packet.length = UBX_CFG_PRT_LENGTH;
cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID;
cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE;
cfg_prt_packet.baudRate = baudrate;
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
/* Send a CFG-PRT message again, this time change the baudrate */
cfg_prt_packet.clsID = UBX_CLASS_CFG;
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
cfg_prt_packet.length = UBX_CFG_PRT_LENGTH;
cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID;
cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE;
cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
receive(UBX_CONFIG_TIMEOUT);
if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
}
/* send a CFT-RATE message to define update rate */
type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
_clsID_needed = UBX_CLASS_CFG;
_msgID_needed = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.clsID = UBX_CLASS_CFG;
cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.length = UBX_CFG_RATE_LENGTH;
cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE;
cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
/* send a NAV5 message to set the options for the internal filter */
type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
_clsID_needed = UBX_CLASS_CFG;
_msgID_needed = UBX_MESSAGE_CFG_NAV5;
cfg_nav5_packet.clsID = UBX_CLASS_CFG;
cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5;
cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH;
cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK;
cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
_clsID_needed = UBX_CLASS_CFG;
_msgID_needed = UBX_MESSAGE_CFG_MSG;
cfg_msg_packet.clsID = UBX_CLASS_CFG;
cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG;
cfg_msg_packet.length = UBX_CFG_MSG_LENGTH;
/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
/* For satelites info 1Hz is enough */
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
_waiting_for_ack = false;
return 0;
}
return -1;
}
int
UBX::configure(unsigned &baudrate)
{
_configured = false;
/* try different baudrates */
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
int baud_i;
for (baud_i = 0; baud_i < 5; baud_i++) {
baudrate = baudrates_to_try[baud_i];
set_baudrate(_fd, baudrate);
/* Send a CFG-PRT message to set the UBX protocol for in and out
* and leave the baudrate as it is, we just want an ACK-ACK from this
*/
type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
/* Set everything else of the packet to 0, otherwise the module wont accept it */
memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
_message_class_needed = UBX_CLASS_CFG;
_message_id_needed = UBX_MESSAGE_CFG_PRT;
/* Define the package contents, don't change the baudrate */
cfg_prt_packet.clsID = UBX_CLASS_CFG;
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
cfg_prt_packet.length = UBX_CFG_PRT_LENGTH;
cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID;
cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE;
cfg_prt_packet.baudRate = baudrate;
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
/* Send a CFG-PRT message again, this time change the baudrate */
cfg_prt_packet.clsID = UBX_CLASS_CFG;
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
cfg_prt_packet.length = UBX_CFG_PRT_LENGTH;
cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID;
cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE;
cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet));
/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
wait_for_ack(UBX_CONFIG_TIMEOUT);
if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
}
/* at this point we have correct baudrate on both ends */
break;
}
if (baud_i >= 5) {
return 1;
}
/* send a CFG-RATE message to define update rate */
type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
_message_class_needed = UBX_CLASS_CFG;
_message_id_needed = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.clsID = UBX_CLASS_CFG;
cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.length = UBX_CFG_RATE_LENGTH;
cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("CFG FAIL: RATE");
return 1;
}
/* send a NAV5 message to set the options for the internal filter */
type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
_message_class_needed = UBX_CLASS_CFG;
_message_id_needed = UBX_MESSAGE_CFG_NAV5;
cfg_nav5_packet.clsID = UBX_CLASS_CFG;
cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5;
cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH;
cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK;
cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("CFG FAIL: NAV5");
return 1;
}
/* configure message rates */
/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV POSLLH");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV TIMEUTC");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV SOL");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV VELNED");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV SVINFO");
return 1;
}
configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: MON HW");
return 1;
}
_configured = true;
return 0;
}
/******* SerialUnit::New() ***********************************/
OOP_Object *PCSerUnit__Root__New(OOP_Class *cl, OOP_Object *obj, struct pRoot_New *msg)
{
struct HIDDSerialUnitData * data;
struct TagItem *tag, *tstate;
ULONG unitnum = 0;
EnterFunc(bug("SerialUnit::New()\n"));
tstate = msg->attrList;
while ((tag = NextTagItem((const struct TagItem **)&tstate)))
{
ULONG idx;
#define csd CSD(cl->UserData)
if (IS_HIDDSERIALUNIT_ATTR(tag->ti_Tag, idx))
#undef csd
{
switch (idx)
{
case aoHidd_SerialUnit_Unit:
unitnum = (ULONG)tag->ti_Data;
break;
}
}
} /* while (tags to process) */
obj = (OOP_Object *)OOP_DoSuperMethod(cl, obj, (OOP_Msg)msg);
if (obj)
{
struct IntuitionBase * IntuitionBase = (struct IntuitionBase *)OpenLibrary("intuition.library",0);
data = OOP_INST_DATA(cl, obj);
data->baseaddr = bases[unitnum];
if (NULL != IntuitionBase) {
struct Preferences prefs;
GetPrefs(&prefs,sizeof(prefs));
data->baudrate = prefs.BaudRate;
adapt_data(data, &prefs);
CloseLibrary((struct Library *)IntuitionBase);
} else {
data->datalength = 8;
data->parity = FALSE;
data->baudrate = 9600; /* will be initialize in set_baudrate() */
}
data->unitnum = unitnum;
Disable();
CSD(cl->UserData)->units[data->unitnum] = data;
Enable();
D(bug("Unit %d at 0x0%x\n", data->unitnum, data->baseaddr));
/* Wake up UART */
serial_outp(data, UART_LCR, 0xBF);
serial_outp(data, UART_EFR, UART_EFR_ECB);
serial_outp(data, UART_IER, 0);
serial_outp(data, UART_EFR, 0);
serial_outp(data, UART_LCR, 0);
/* clear the FIFOs */
serial_outp(data, UART_FCR, (UART_FCR_CLEAR_RCVR | UART_FCR_CLEAR_XMIT));
/* clear the interrupt registers */
(void)serial_inp(data, UART_RX);
(void)serial_inp(data, UART_IIR);
(void)serial_inp(data, UART_MSR);
/* initilize the UART */
serial_outp(data, UART_LCR, get_lcr(data));
serial_outp(data, UART_MCR, UART_MCR_OUT2 | UART_MCR_DTR | UART_MCR_RTS);
serial_outp(data, UART_IER, UART_IER_RDI | UART_IER_THRI | UART_IER_RLSI | UART_IER_MSI);
/* clear the interrupt registers again ... */
(void)serial_inp(data, UART_LSR);
(void)serial_inp(data, UART_RX);
(void)serial_inp(data, UART_IIR);
(void)serial_inp(data, UART_MSR);
set_baudrate(data, data->baudrate);
} /* if (obj) */
ReturnPtr("SerialUnit::New()", OOP_Object *, obj);
}
int
UBX::configure(unsigned &baudrate)
{
_configured = false;
/* try different baudrates */
const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200};
unsigned baud_i;
for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) {
baudrate = baudrates[baud_i];
set_baudrate(_fd, baudrate);
/* flush input and wait for at least 20 ms silence */
decode_init();
receive(20);
decode_init();
/* Send a CFG-PRT message to set the UBX protocol for in and out
* and leave the baudrate as it is, we just want an ACK-ACK for this */
memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt));
_buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID;
_buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE;
_buf.payload_tx_cfg_prt.baudRate = baudrate;
_buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK;
_buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK;
send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt));
if (wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false) < 0) {
/* try next baudrate */
continue;
}
/* Send a CFG-PRT message again, this time change the baudrate */
memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt));
_buf.payload_tx_cfg_prt.portID = UBX_TX_CFG_PRT_PORTID;
_buf.payload_tx_cfg_prt.mode = UBX_TX_CFG_PRT_MODE;
_buf.payload_tx_cfg_prt.baudRate = UBX_TX_CFG_PRT_BAUDRATE;
_buf.payload_tx_cfg_prt.inProtoMask = UBX_TX_CFG_PRT_INPROTOMASK;
_buf.payload_tx_cfg_prt.outProtoMask = UBX_TX_CFG_PRT_OUTPROTOMASK;
send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt));
/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
if (UBX_TX_CFG_PRT_BAUDRATE != baudrate) {
set_baudrate(_fd, UBX_TX_CFG_PRT_BAUDRATE);
baudrate = UBX_TX_CFG_PRT_BAUDRATE;
}
/* at this point we have correct baudrate on both ends */
break;
}
if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) {
return 1; // connection and/or baudrate detection failed
}
/* Send a CFG-RATE message to define update rate */
memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate));
_buf.payload_tx_cfg_rate.measRate = UBX_TX_CFG_RATE_MEASINTERVAL;
_buf.payload_tx_cfg_rate.navRate = UBX_TX_CFG_RATE_NAVRATE;
_buf.payload_tx_cfg_rate.timeRef = UBX_TX_CFG_RATE_TIMEREF;
send_message(UBX_MSG_CFG_RATE, _buf.raw, sizeof(_buf.payload_tx_cfg_rate));
if (wait_for_ack(UBX_MSG_CFG_RATE, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
/* send a NAV5 message to set the options for the internal filter */
memset(&_buf.payload_tx_cfg_nav5, 0, sizeof(_buf.payload_tx_cfg_nav5));
_buf.payload_tx_cfg_nav5.mask = UBX_TX_CFG_NAV5_MASK;
_buf.payload_tx_cfg_nav5.dynModel = UBX_TX_CFG_NAV5_DYNMODEL;
_buf.payload_tx_cfg_nav5.fixMode = UBX_TX_CFG_NAV5_FIXMODE;
send_message(UBX_MSG_CFG_NAV5, _buf.raw, sizeof(_buf.payload_tx_cfg_nav5));
if (wait_for_ack(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
#ifdef UBX_CONFIGURE_SBAS
/* send a SBAS message to set the SBAS options */
memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas));
_buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE;
send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas));
if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
#endif
/* configure message rates */
/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
/* try to set rate for NAV-PVT */
/* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */
configure_message_rate(UBX_MSG_NAV_PVT, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
_use_nav_pvt = false;
} else {
_use_nav_pvt = true;
}
UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
if (!_use_nav_pvt) {
configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_SOL, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_NAV_VELNED, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
}
configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
configure_message_rate(UBX_MSG_MON_HW, 1);
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
/* request module version information by sending an empty MON-VER message */
send_message(UBX_MSG_MON_VER, nullptr, 0);
_configured = true;
return 0;
}
DWORD CPco_cl_com::Open_Cam_Ext(DWORD num,SC2_OpenStruct *open)
{
int err;
initmode=num & ~0xFF;
num=num&0xFF;
if(num>PCO_SC2_DRIVER_ENTRY_SIZE)
{
writelog(ERROR_M,(PCO_HANDLE)1,"Open_Cam_Ext: No more entries left return NODRIVER");
return PCO_ERROR_DRIVER_NODRIVER | PCO_ERROR_DRIVER_CAMERALINK;
}
if(connected&(1<<num))
{
writelog(ERROR_M,(PCO_HANDLE)1,"Open_Cam_Ext: camera is already connected");
return PCO_ERROR_DRIVER_NODRIVER | PCO_ERROR_DRIVER_CAMERALINK;
}
hdriver=(void*)(0x100+num);
camerarev=0;
tab_timeout.command=PCO_SC2_COMMAND_TIMEOUT;
tab_timeout.image=PCO_SC2_IMAGE_TIMEOUT_L;
tab_timeout.transfer=PCO_SC2_COMMAND_TIMEOUT;
transferpar.baudrate=9600;
transferpar.DataFormat=PCO_CL_DATAFORMAT_5x12;
unsigned int num_port=0;
{
char manufacturerName[500];
unsigned int bufferSize;
unsigned int version;
char type[500];
unsigned int buffersize;
bufferSize=sizeof(manufacturerName);
buffersize=sizeof(type);
// if(clGetNumSerialPorts)
{
clGetNumSerialPorts(&num_port);
if(num>=num_port)
{
hdriver=NULL;
writelog(ERROR_M,(PCO_HANDLE)1,"Open_Cam_Ext: board %d reqested, only %d ports found return NODRIVER",num,num_port);
return PCO_ERROR_DRIVER_NODRIVER | PCO_ERROR_DRIVER_CAMERALINK;
}
}
// if(clGetManufacturerInfo)
{
clGetManufacturerInfo(manufacturerName,&bufferSize,&version);
writelog(INIT_M,(PCO_HANDLE)1,"Open_Cam_Ext: ManufacturerName %s",manufacturerName);
}
// if(clGetSerialPortIdentifier)
{
for(unsigned int i=0;i<num_port;i++)
{
clGetSerialPortIdentifier(i,type,&buffersize);
writelog(INIT_M,(PCO_HANDLE)1,"Open_Cam_Ext: PortIdentifier %d: %s",i,type);
}
}
}
err=clSerialInit(num,(void**)&serialRef);
if(err<0)
{
writelog(ERROR_M,(PCO_HANDLE)1,"Open_Cam_Ext: Cannot open serial Cameralink Device %d %d",num,err);
hdriver=NULL;
serialRef=NULL;
return PCO_ERROR_DRIVER_NODRIVER | PCO_ERROR_DRIVER_CAMERALINK;
}
boardnr=num;
//check if camera is connected, error should be timeout
//get camera descriptor to get maximal size of ccd
//scan for other baudrates than default 9600baud
SC2_Simple_Telegram com;
err=PCO_NOERROR;
err=scan_camera();
if(err!=PCO_NOERROR)
{
writelog(ERROR_M,hdriver,"Open_Cam_Ext: Control command failed with 0x%x, no camera connected",err);
clSerialClose(serialRef);
boardnr=-1;
hdriver=NULL;
serialRef=NULL;
return PCO_ERROR_DRIVER_NOTINIT | PCO_ERROR_DRIVER_CAMERALINK;
}
com.wCode=GET_CAMERA_DESCRIPTION;
com.wSize=sizeof(SC2_Simple_Telegram);
err=Control_Command(&com,sizeof(com),
&description,sizeof(SC2_Camera_Description_Response));
if(err!=PCO_NOERROR)
writelog(ERROR_M,hdriver,"Open_Cam_Ext: GET_CAMERA_DESCRIPTION failed with 0x%x",err);
get_actual_cl_config();
get_firmwarerev();
get_lut_info();
char evname[25];
sprintf(evname,"%s%d",COM_MUTEX_NAME,num);
/*
hComMutex=CreateMutex(NULL,FALSE,evname);
int e=GetLastError();
writelog(INIT_M,hdriver,"Open_Cam_Ext: Mutex 0x%x name %s created",hComMutex,evname);
hComMutex=CreateMutex(NULL,FALSE,NULL);
writelog(INIT_M,hdriver,"Open_Cam_Ext: GetLastError returned %d",e);
*/
if(transferpar.baudrate!=115200)
set_baudrate(115200);
connected|=(1<<boardnr);
return PCO_NOERROR;
}
/* UART configure
* Reference form
* "http://ulisse.elettra.trieste.it/services/doc/serial/config.html"
* */
void con_setting(int fd)
{
struct termios newtio;
char *data = NULL;
memset(&newtio,0,sizeof(newtio));
set_baudrate(&newtio,get_conf("baudrate"));
newtio.c_cflag |= CS8 | 0 | 0 | 0 | CLOCAL | CREAD;
data = get_conf("data_bits");
switch( atoi(data) ){/*Data bit*/
case 7:
newtio.c_cflag |= CS7;
break;
default :
newtio.c_cflag |= CS8;
break;
}
data = get_conf("stop_bit");
switch( atoi(data) ){/*stop_bit*/
case 2:
newtio.c_cflag |= CSTOPB;
break;
default :
newtio.c_cflag &= ~CSTOPB;
break;
}
data = get_conf("parity");
switch( atoi(data) ){/*Parity*/
case 1: /*Odd*/
newtio.c_cflag |= PARENB;
newtio.c_cflag |= PARODD;
newtio.c_cflag &= ~CSTOPB;
newtio.c_cflag &= ~CSIZE;
newtio.c_cflag |= CS7;
break;
case 2: /*even*/
newtio.c_cflag |= PARENB;
newtio.c_cflag &= ~PARODD;
newtio.c_cflag &= ~CSTOPB;
newtio.c_cflag &= ~CSIZE;
newtio.c_cflag |= CS7;
break;
case 3: /*mark*/
newtio.c_cflag &= ~PARENB;
newtio.c_cflag |= CSTOPB;
newtio.c_cflag &= ~CSIZE;
newtio.c_cflag |= CS7;
break;
case 4: /*space*/
newtio.c_cflag &= ~PARENB;
newtio.c_cflag &= ~CSTOPB;
newtio.c_cflag &= ~CSIZE;
newtio.c_cflag |= CS8;
break;
default :/*none*/
newtio.c_cflag &= ~PARENB;
newtio.c_cflag &= ~CSTOPB;
newtio.c_cflag &= ~CSIZE;
newtio.c_cflag |= CS8;
break;
}
/*
data = get_conf("flow_control");
if(!strcmp(data,"rts")){
//newtio.c_cflag |= CNEW_RTSCTS;
}else if(!strcmp(data,"xon")){
newtio.c_iflag |= (IXON | IXOFF | IXANY);
}else{
//newtio.c_cflag &= ~CNEW_RTSCTS;
}
*/
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
newtio.c_lflag = 0;
tcflush(fd, TCIFLUSH);
tcsetattr(fd, TCSANOW, &newtio);
}
/**
\fn int main(int argc, char *argv[])
\brief main routine
\param argc number of commandline arguments + 1
\param argv string array containing commandline arguments (argv[0] contains name of executable)
\return dummy return code (not used)
Main routine for import, programming, and check routines
*/
int main(int argc, char ** argv) {
char *appname; // name of application without path
char portname[STRLEN]; // name of communication port
int baudrate; // communication baudrate [Baud]
uint8_t resetSTM8; // 0=no reset; 1=HW reset via DTR (RS232/USB) or GPIO18 (Raspi); 2=SW reset by sending 0x55+0xAA
uint8_t enableBSL; // don't enable ROM bootloader after upload (caution!)
uint8_t jumpFlash; // jump to flash after upload
uint8_t pauseOnLaunch; // prompt for <return> prior to upload
int flashsize; // size of flash (kB) for w/e routines
uint8_t versBSL; // BSL version for w/e routines
char hexfile[STRLEN]; // name of file to flash
HANDLE ptrPort; // handle to communication port
char buf[BUFSIZE]; // buffer for hexfiles
char image[BUFSIZE]; // memory image buffer
uint32_t imageStart; // starting address of image
uint32_t numBytes; // number of bytes in image
char *ptr=NULL; // pointer to memory
int i, j; // generic variables
//char Tx[100], Rx[100]; // debug: buffer for tests
// initialize global variables
g_pauseOnExit = 1; // wait for <return> before terminating
g_UARTmode = 0; // 2-wire interface with UART duplex mode
verbose = false; // verbose output when requested only
// initialize default arguments
portname[0] = '\0'; // no default port name
baudrate = 230400; // default baudrate
resetSTM8 = 0; // don't automatically reset STM8
jumpFlash = 1; // jump to flash after uploade
pauseOnLaunch = 1; // prompt for return prior to upload
enableBSL = 1; // enable bootloader after upload
hexfile[0] = '\0'; // no default hexfile
// for debugging only
//sprintf(portname, "/dev/tty.usbserial-A4009I0O");
// required for strncpy()
portname[STRLEN-1] = '\0';
hexfile[STRLEN-1] = '\0';
// reset console color (needs to be called once for Win32)
setConsoleColor(PRM_COLOR_DEFAULT);
////////
// parse commandline arguments
////////
for (i=1; i<argc; i++) {
// debug: print argument
//printf("arg %d: '%s'\n", (int) i, argv[i]);
// name of communication port
if (!strcmp(argv[i], "-p"))
strncpy(portname, argv[++i], STRLEN-1);
// communication baudrate
else if (!strcmp(argv[i], "-b"))
sscanf(argv[++i],"%d",&baudrate);
// UART mode: 0=duplex, 1=1-wire reply, 2=2-wire reply (default: duplex)\n");
else if (!strcmp(argv[i], "-u")) {
sscanf(argv[++i], "%d", &j);
g_UARTmode = j;
}
// name of hexfile
else if (!strcmp(argv[i], "-f"))
strncpy(hexfile, argv[++i], STRLEN-1);
// HW reset STM8 via DTR line (RS232/USB) or GPIO18 (Raspi only)
else if (!strcmp(argv[i], "-r")) {
sscanf(argv[++i], "%d", &j);
resetSTM8 = j;
}
// don't enable ROM bootloader after upload (caution!)
else if (!strcmp(argv[i], "-x"))
enableBSL = 0;
// don't jump to address after upload
else if (!strcmp(argv[i], "-j"))
jumpFlash = 0;
// don't prompt for <return> prior to upload
else if (!strcmp(argv[i], "-Q"))
pauseOnLaunch = 0;
// don't prompt for <return> prior to exit
else if (!strcmp(argv[i], "-q"))
g_pauseOnExit = 0;
// verbose output
else if (!strcmp(argv[i], "-v"))
verbose = true;
// else print list of commandline arguments and language commands
else {
if (strrchr(argv[0],'\\'))
appname = strrchr(argv[0],'\\')+1; // windows
else if (strrchr(argv[0],'/'))
appname = strrchr(argv[0],'/')+1; // Posix
else
appname = argv[0];
printf("\n");
printf("usage: %s [-h] [-p port] [-b rate] [-u mode] [-f file] [-r ch] [-x] [-j] [-Q] [-q] [-v]\n\n", appname);
printf(" -h print this help\n");
printf(" -p port name of communication port (default: list all ports and query)\n");
printf(" -b rate communication baudrate in Baud (default: 230400)\n");
printf(" -u mode UART mode: 0=duplex, 1=1-wire reply, 2=2-wire reply (default: duplex)\n");
printf(" -f file name of s19 or intel-hex file to flash (default: none)\n");
#ifdef __ARMEL__
printf(" -r ch reset STM8: 1=DTR line (RS232), 2=send 'Re5eT!' @ 115.2kBaud, 3=GPIO18 pin (Raspi) (default: no reset)\n");
#else
printf(" -r ch reset STM8: 1=DTR line (RS232), 2=send 'Re5eT!' @ 115.2kBaud (default: no reset)\n");
#endif
printf(" -x don't enable ROM bootloader after upload (default: enable)\n");
printf(" -j don't jump to flash after upload (default: jump to flash)\n");
printf(" -Q don't prompt for <return> prior to upload (default: prompt)\n");
printf(" -q don't prompt for <return> prior to exit (default: prompt)\n");
printf(" -v verbose output\n");
printf("\n");
Exit(0, 0);
}
} // process commandline arguments
////////
// print app name & version, and change console title
////////
get_app_name(argv[0], VERSION, buf);
printf("\n%s\n", buf);
setConsoleTitle(buf);
////////
// if no port name is given, list all available ports and query
////////
if (strlen(portname) == 0) {
printf(" enter comm port name ( ");
list_ports();
printf(" ): ");
scanf("%s", portname);
getchar();
} // if no comm port name
// If specified import hexfile - do it early here to be able to report file read errors before others
if (strlen(hexfile)>0) {
const char *shortname = strrchr(hexfile, '/');
if (!shortname)
shortname = hexfile;
// convert to memory image, depending on file type
const char *dot = strrchr (hexfile, '.');
if (dot && !strcmp(dot, ".s19")) {
if (verbose)
printf(" Loading Motorola S-record file %s …\n", shortname);
load_hexfile(hexfile, buf, BUFSIZE);
convert_s19(buf, &imageStart, &numBytes, image);
}
else if (dot && (!strcmp(dot, ".hex") || !strcmp(dot, ".ihx"))) {
if (verbose)
printf(" Loading Intel hex file %s …\n", shortname);
load_hexfile(hexfile, buf, BUFSIZE);
convert_hex(buf, &imageStart, &numBytes, image);
}
else {
if (verbose)
printf(" Loading binary file %s …\n", shortname);
load_binfile(hexfile, image, &imageStart, &numBytes, BUFSIZE);
}
}
////////
// put STM8 into bootloader mode
////////
if (pauseOnLaunch) {
printf(" activate STM8 bootloader and press <return>");
fflush(stdout);
fflush(stdin);
getchar();
}
////////
// open port with given properties
////////
printf(" open port '%s' with %gkBaud ... ", portname, (float) baudrate / 1000.0);
fflush(stdout);
if (g_UARTmode == 0)
ptrPort = init_port(portname, baudrate, 1000, 8, 2, 1, 0, 0); // use even parity
else
ptrPort = init_port(portname, baudrate, 1000, 8, 0, 1, 0, 0); // use no parity
printf("ok\n");
fflush(stdout);
// debug: communication test (echo+1 test-SW on STM8)
/*
printf("open: %d\n", ptrPort);
for (i=0; i<254; i++) {
Tx[0] = i;
send_port(ptrPort, 1, Tx);
receive_port(ptrPort, 1, Rx);
printf("%d %d\n", (int) Tx[0], (int) Rx[0]);
}
printf("done\n");
Exit(1,0);
*/
////////
// communicate with STM8 bootloader
////////
// HW reset STM8 using DTR line (USB/RS232)
if (resetSTM8 == 1) {
printf(" reset via DTR ... ");
pulse_DTR(ptrPort, 10);
printf("done\n");
SLEEP(5); // allow BSL to initialize
}
// SW reset STM8 via command 'Re5eT!' at 115.2kBaud (requires respective STM8 SW)
else if (resetSTM8 == 2) {
set_baudrate(ptrPort, 115200); // expect STM8 SW to receive at 115.2kBaud
printf(" reset via UART command ... ");
sprintf(buf, "Re5eT!"); // reset command (same as in STM8 SW!)
for (i=0; i<6; i++) {
send_port(ptrPort, 1, buf+i); // send reset command bytewise to account for slow handling
SLEEP(10);
}
printf("done\n");
set_baudrate(ptrPort, baudrate); // restore specified baudrate
}
// HW reset STM8 using GPIO18 pin (only Raspberry Pi!)
#ifdef __ARMEL__
else if (resetSTM8 == 3) {
printf(" reset via GPIO18 ... ");
pulse_GPIO(18, 10);
printf("done\n");
SLEEP(5); // allow BSL to initialize
}
#endif // __ARMEL__
// synchronize baudrate
bsl_sync(ptrPort);
// get bootloader info for selecting flash w/e routines
bsl_getInfo(ptrPort, &flashsize, &versBSL);
// select device dependent flash routines for upload
if ((flashsize==32) && (versBSL==0x10)) {
ptr = (char*) STM8_Routines_E_W_ROUTINEs_32K_ver_1_0_s19;
ptr[STM8_Routines_E_W_ROUTINEs_32K_ver_1_0_s19_len]=0;
}
else if ((flashsize==32) && (versBSL==0x12)) {
ptr = (char*) STM8_Routines_E_W_ROUTINEs_32K_ver_1_2_s19;
ptr[STM8_Routines_E_W_ROUTINEs_32K_ver_1_2_s19_len]=0;
}
else if ((flashsize==32) && (versBSL==0x13)) {
ptr = (char*) STM8_Routines_E_W_ROUTINEs_32K_ver_1_3_s19;
ptr[STM8_Routines_E_W_ROUTINEs_32K_ver_1_3_s19_len]=0;
}
else if ((flashsize==32) && (versBSL==0x14)) {
ptr = (char*) STM8_Routines_E_W_ROUTINEs_32K_ver_1_4_s19;
ptr[STM8_Routines_E_W_ROUTINEs_32K_ver_1_4_s19_len]=0;
}
else if ((flashsize==128) && (versBSL==0x20)) {
ptr = (char*) STM8_Routines_E_W_ROUTINEs_128K_ver_2_0_s19;
ptr[STM8_Routines_E_W_ROUTINEs_128K_ver_2_0_s19_len]=0;
}
else if ((flashsize==128) && (versBSL==0x21)) {
ptr = (char*) STM8_Routines_E_W_ROUTINEs_128K_ver_2_1_s19;
ptr[STM8_Routines_E_W_ROUTINEs_128K_ver_2_1_s19_len]=0;
}
else if ((flashsize==128) && (versBSL==0x22)) {
ptr = (char*) STM8_Routines_E_W_ROUTINEs_128K_ver_2_2_s19;
ptr[STM8_Routines_E_W_ROUTINEs_128K_ver_2_2_s19_len]=0;
}
else if ((flashsize==128) && (versBSL==0x24)) {
ptr = (char*) STM8_Routines_E_W_ROUTINEs_128K_ver_2_4_s19;
ptr[STM8_Routines_E_W_ROUTINEs_128K_ver_2_4_s19_len]=0;
}
else if ((flashsize==256) && (versBSL==0x10)) {
ptr = (char*) STM8_Routines_E_W_ROUTINEs_256K_ver_1_0_s19;
ptr[STM8_Routines_E_W_ROUTINEs_256K_ver_1_0_s19_len]=0;
}
else {
setConsoleColor(PRM_COLOR_RED);
fprintf(stderr, "\n\nerror: unsupported device, exit!\n\n");
Exit(1, g_pauseOnExit);
}
{
char ramImage[8192];
uint32_t ramImageStart;
uint32_t numRamBytes;
convert_s19(ptr, &ramImageStart, &numRamBytes, ramImage);
if (verbose)
printf("Uploading RAM routines\n");
bsl_memWrite(ptrPort, ramImageStart, numRamBytes, ramImage, 0);
}
// if specified upload hexfile
if (strlen(hexfile)>0)
bsl_memWrite(ptrPort, imageStart, numBytes, image, 1);
// memory read
//imageStart = 0x8000; numBytes = 128*1024; // complete 128kB flash
//imageStart = 0x00A0; numBytes = 352; // RAM
//bsl_memRead(ptrPort, imageStart, numBytes, image);
// enable ROM bootloader after upload (option bytes always on same address)
if (enableBSL==1) {
printf(" activate bootloader ... ");
bsl_memWrite(ptrPort, 0x487E, 2, (char*)"\x55\xAA", 0);
printf("done\n");
}
// jump to flash start address after upload (reset vector always on same address)
if (jumpFlash)
bsl_jumpTo(ptrPort, 0x8000);
////////
// clean up and exit
////////
close_port(&ptrPort);
if (verbose)
printf("done with program\n");
Exit(0, g_pauseOnExit);
// avoid compiler warnings
return(0);
} // main
/* main */
int main( int argc, char *argv[] )
{
pthread_t command_thread;
pthread_t control_thread;
pthread_t update_thread;
int command_thread_en;
int control_thread_en;
int update_thread_en;
Ver_t version;
Param_t driver_param;
int i, ret;
ParametersPtr param;
char paramfile[512];
int quit;
hook_pre_global();
ret = arg_analyze( argc, argv );
if( option( OPTION_DAEMON ) )
{
#if HAVE_FORK
pid_t pid;
pid = fork( );
if( pid < 0 )
{
return -1;
}
else if( pid != 0 )
{
return 0;
}
setsid( );
if( chdir( "/" ) < 0 )
{
yprintf( OUTPUT_LV_ERROR, "Failed to chdir.\n" );
return EXIT_FAILURE;
}
close( STDIN_FILENO );
close( STDOUT_FILENO );
close( STDERR_FILENO );
#else
yprintf( OUTPUT_LV_ERROR, "Daemon mode is not supported in your system.\n" );
return EXIT_FAILURE;
#endif
}
if( option( OPTION_SHOW_HELP ) )
{
arg_help( argc, argv );
return EXIT_SUCCESS;
}
if( option( OPTION_SHOW_LONGHELP ) )
{
arg_longhelp( argc, argv );
return EXIT_SUCCESS;
}
if( option( OPTION_SHOW_PARAMHELP ) )
{
param_help( );
return EXIT_SUCCESS;
}
if( option( OPTION_VERSION ) )
{
fprintf( stderr, "YamabicoProject-Spur\n" );
fprintf( stderr, " Ver. %s\n", PACKAGE_VERSION );
return EXIT_SUCCESS;
}
if( !ret ) /* オプション解析に失敗したとき */
return EXIT_FAILURE;
yprintf( OUTPUT_LV_PROCESS, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
yprintf( OUTPUT_LV_PROCESS, "YamabicoProject-Spur\n" );
yprintf( OUTPUT_LV_PROCESS, " Ver. %s\n", PACKAGE_VERSION );
yprintf( OUTPUT_LV_PROCESS, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
/* Ctrl-C割り込みハンドラーの登録 */
escape_road( );
g_emergency = 0;
/* パラメータを読み込み、セットする */
param = get_param_ptr( );
#ifdef HAVE_SSM
/* SSM初期化 */
if( !option( OPTION_WITHOUT_SSM ) )
init_ypspurSSM( param->ssm_id );
#endif
/* 座標系の初期化、コマンド処理系の初期化 */
init_coordinate_systems( );
init_odometry( );
init_spur_command( );
fflush( stderr );
command_thread_en = 0;
command_thread_en = 0;
do
{
FILE *temp_paramfile = NULL;
quit = 0;
yprintf( OUTPUT_LV_PROCESS, "Device Information\n" );
yprintf( OUTPUT_LV_PROCESS, " Port : %s \n", param->device_name );
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
if( !serial_connect( param->device_name ) )
{
// quit=0;でbreakしたら異常終了と判断される
break;
}
if( !( option( OPTION_DO_NOT_USE_YP ) ) )
{
int current, age;
sscanf( YP_PROTOCOL_NAME, "YPP:%d:%d", ¤t, &age );
yprintf( OUTPUT_LV_PROCESS, " Checking device information...\r" );
for ( i = 0; i < 3; i++ )
{
int device_current, device_age;
// プロトコルがYPであることを確認
if( get_version( &version ) == -1 )
{
continue;
}
if( strstr( version.protocol, "YPP:" ) != version.protocol )
{
continue;
}
sscanf( version.protocol, "YPP:%d:%d", &device_current, &device_age );
if( device_current - device_age > current || device_current < current )
{
continue;
}
break;
}
yprintf( OUTPUT_LV_PARAM, " Vender : %s\033[K\n", version.vender );
yprintf( OUTPUT_LV_PARAM, " Product : %s\n", version.product );
yprintf( OUTPUT_LV_PARAM, " Firmware: %s\n", version.firmware );
yprintf( OUTPUT_LV_PARAM, " Protcol : %s\n", version.protocol );
yprintf( OUTPUT_LV_PARAM, " Serialno: %s\n", version.serialno );
yprintf( OUTPUT_LV_PARAM, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
if( i == 3 )
{
yprintf( OUTPUT_LV_ERROR, "Error: Device doesn't have available YP protocol version.\n" );
if( option( OPTION_RECONNECT ) && g_emergency == 0 )
{
yp_usleep( 500000 );
continue;
}
break; // quit=0でbreakしたら異常終了と判断
}
}
fflush( stderr );
if( get_parameter( &driver_param ) == -1 )
{
continue;
}
yprintf( OUTPUT_LV_PARAM, "Driver depending parameters\n" );
yprintf( OUTPUT_LV_PARAM, " Name : %s\n", driver_param.robot_name );
yprintf( OUTPUT_LV_PARAM, " PWM resolution: %s\n", driver_param.pwm_resolution );
yprintf( OUTPUT_LV_PARAM, " Motor number : %s\n", driver_param.motor_num );
yprintf( OUTPUT_LV_PARAM, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
if( strlen( driver_param.pwm_resolution ) <= 0 ||
strlen( driver_param.motor_num ) <= 0 )
{
yprintf( OUTPUT_LV_ERROR, "Error: Failed to load driver parameters.\n" );
if( option( OPTION_RECONNECT ) && g_emergency == 0 )
{
yp_usleep( 500000 );
continue;
}
break;
}
}
if( !( option( OPTION_PARAM_FILE ) ) )
{
// パラメータファイルが指定されておらず、ドライバにパラメータが内蔵されている場合
if( strcmp( driver_param.robot_name, "embedded" ) == 0 )
{
char param[2048];
yprintf( OUTPUT_LV_MODULE, "Reading device embedded parameter.\n" );
temp_paramfile = tmpfile( );
if( !temp_paramfile )
{
yprintf( OUTPUT_LV_ERROR, "Error: Failed to create temporary file.\n" );
return 0;
}
if( !get_embedded_param( param ) )
{
yprintf( OUTPUT_LV_ERROR, "Error: Failed to read embedded parameters.\n" );
if( option( OPTION_RECONNECT ) && g_emergency == 0 )
{
yp_usleep( 500000 );
continue;
}
break;
}
fprintf( temp_paramfile, "%s", param );
fseek( temp_paramfile, 0L, SEEK_SET );
}
// パラメータファイルが指定されておらず、ドライバにロボット名が登録されている場合
else if( strlen( driver_param.robot_name ) > 0 && strcmp( driver_param.robot_name, "unknown" ) != 0 )
{
strcpy( param->parameter_filename, driver_param.robot_name );
strcat( param->parameter_filename, ".param" );
}
}
if( temp_paramfile )
{
yprintf( OUTPUT_LV_PARAM, "Embedded parameter file\n" );
if( !set_paramptr( temp_paramfile ) )
{
yprintf( OUTPUT_LV_ERROR, "Error: Cannot use embedded parameter.\n" );
return 0;
}
}
else
{
yprintf( OUTPUT_LV_PARAM, "Parameter file: %s\n", param->parameter_filename );
if( !set_param( param->parameter_filename, paramfile ) )
{
yprintf( OUTPUT_LV_ERROR, "Error: Cannot find parameter file.\n" );
return 0;
}
}
{
int i;
for ( i = 0; i < YP_PARAM_MOTOR_NUM; i++ )
{
*pp( YP_PARAM_PWM_MAX, i ) = atoi( driver_param.pwm_resolution );
}
}
yprintf( OUTPUT_LV_PARAM, "++++++++++++++++++++++++++++++++++++++++++++++++++\n\n" );
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
// ボーレートの設定
if( param->speed )
{
yprintf( OUTPUT_LV_MODULE, "Setting baudrate to %d baud.\n", param->speed );
}
else {
// 指定されてない場合デフォルトの値
param->speed = DEFAULT_BAUDRATE;
}
ret = set_baudrate( param->speed );
if( ret == 0 )
{
// 設定失敗
yprintf( OUTPUT_LV_WARNING, "Error: Failed to change baudrate.\n" );
serial_close( );
quit = 0;
break; // quit=0でbreakしたら異常終了と判断
}
if (ret == 4)
{
// ボーレートの設定未対応
yprintf( OUTPUT_LV_WARNING, "Warn: Baudrate setting is not supported on this device.\n" );
}
else
{
// 設定成功
// 正常ならば何もしない
}
if( param->admask )
{
yprintf( OUTPUT_LV_MODULE, "Setting admask to %x.\n", param->admask );
set_admask( param->admask );
}
if( option( OPTION_ENABLE_GET_DIGITAL_IO ) )
{
yprintf( OUTPUT_LV_MODULE, "Enabling digital io input.\n" );
set_diomask( 1 );
}
if( !( option( OPTION_PARAM_CONTROL ) ) )
{
apply_robot_params( );
}
/* サーボをかける */
if( state( YP_STATE_MOTOR ) && state( YP_STATE_VELOCITY ) )
{
motor_servo( );
}
}
yprintf( OUTPUT_LV_MODULE, "YP-Spur coordinator started.\n" );
/* スレッド初期化 */
init_command_thread( &command_thread );
pthread_detach( command_thread );
command_thread_en = 1;
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
init_control_thread( &control_thread );
pthread_detach( control_thread );
control_thread_en = 1;
}
if( option( OPTION_UPDATE_PARAM ) )
{
init_param_update_thread( &update_thread, paramfile );
pthread_detach( update_thread );
update_thread_en = 1;
}
// オドメトリ受信ループ
#if HAVE_SIGLONGJMP
if( sigsetjmp( ctrlc_capture, 1 ) != 0 )
{
quit = 1;
}
else
#elif HAVE_LONGJMP
if( setjmp( ctrlc_capture ) != 0 )
{
quit = 1;
}
else
#endif
{
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
odometry_receive_loop( );
}
else
{
while( 1 )
yp_usleep( 1000000 );
}
yprintf( OUTPUT_LV_MODULE, "Connection to %s was closed.\n", param->device_name );
}
/* 終了処理 */
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
serial_close( );
}
if( update_thread_en )
{
pthread_cancel( update_thread );
pthread_join( update_thread, NULL );
update_thread_en = 0;
}
if( control_thread_en )
{
pthread_cancel( control_thread );
pthread_join( control_thread, NULL );
control_thread_en = 0;
}
if( command_thread_en )
{
pthread_cancel( command_thread );
pthread_join( command_thread, NULL );
command_thread_en = 0;
}
if( option( OPTION_RECONNECT ) && quit == 0 )
{
init_spur_command( );
yp_usleep( 500000 );
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
while( !serial_tryconnect( param->device_name ) )
{
yp_usleep( 200000 );
}
}
yprintf( OUTPUT_LV_MODULE, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
yp_usleep( 500000 );
continue;
}
break;
}
while( 1 );
#ifdef HAVE_SSM
/* SSM終了処理 */
if( !option( OPTION_WITHOUT_SSM ) )
end_ypspurSSM( );
#endif
yp_usleep( 200000 );
fflush( stderr );
return ( quit ? EXIT_SUCCESS : EXIT_FAILURE );
}
int test_uart_bridge(int argc, char *argv[])
{
char *device_uart = "/dev/ttyS0";
unsigned baudrate_uart = 38400;
char *device_usb = "/dev/ttyACM0";
if (argc > 2) {
if (!strcmp(argv[1], "-a")) {
device_uart = argv[2];
} else {
printf("tests uart_bridge -a /dev/ttyS0 -b 38400 -c /dev/ttyACM0\n");
return 0;
}
}
if (argc > 4) {
if (!strcmp(argv[3], "-b")) {
if (!strcmp(argv[4], "9600")) {
baudrate_uart = 9600;
} else if (!strcmp(argv[4], "19200")) {
baudrate_uart = 19200;
} else if (!strcmp(argv[4], "38400")) {
baudrate_uart = 38400;
} else if (!strcmp(argv[4], "57600")) {
baudrate_uart = 57600;
} else if (!strcmp(argv[4], "115200")) {
baudrate_uart = 115200;
} else {
printf("tests uart_bridge -a /dev/ttyS0 -b 38400 -c /dev/ttyACM0\n");
return 0;
}
} else {
printf("tests uart_bridge -a /dev/ttyS0 -b 38400 -c /dev/ttyACM0\n");
return 0;
}
}
if (argc > 6) {
if (!strcmp(argv[5], "-c")) {
device_usb = argv[6];
} else {
printf("tests uart_bridge -a /dev/ttyS0 -b 38400 -c /dev/ttyACM0\n");
return 0;
}
}
printf("Opening %s at %u\n", device_uart, baudrate_uart);
int uart = open(device_uart, O_RDWR | O_NONBLOCK | O_NOCTTY);
set_baudrate(uart, baudrate_uart);
printf("Opening %s \n", device_usb);
int uartusb = open(device_usb, O_RDWR | O_NONBLOCK | O_NOCTTY);
if (uart < 0) {
printf("ERROR opening %s, aborting..\n", device_uart);
return uart;
}
if (uartusb < 0) {
printf("ERROR opening %s, aborting..\n", device_usb);
exit(uartusb);
}
int r;
while (1) {
uint8_t sample_stdout[1];
r = read(uart, sample_stdout, 1);
if (r > 0)
write(uartusb, sample_stdout, 1);
r = read(uartusb, sample_stdout, 1);
if (r > 0)
write(uart, sample_stdout, 1);
}
close(uart);
close(uartusb);
return 0;
}
