void delaySec(int secs){
int start_time = 3600*getHour()+60*getMin()+getSec();
int time2 = start_time;
while(time2 <= start_time+secs){
time2 = 3600*getHour()+60*getMin()+getSec();
}
}
static void blockForGpsStabilization() {
float consecutiveSeconds = 0;
int lastTime = getSec();
while (consecutiveSeconds < 5) {
int thisTime = getSec();
int timeDelta = thisTime - lastTime;
consecutiveSeconds += timeDelta;
if (lastTime > thisTime || timeDelta > 1 ) {
consecutiveSeconds = 0;
}
}
}
std::string TTime::toString(){
char buffer [8];
sprintf (buffer, "%.2i:%.2i:%.2i", getHour(), getMin(), getSec());
std::string ret = buffer;
return ret;
}
void print_time(time_t tim) {
std::cout << getWeekday(tim) << std::endl;
std::cout << getSec(tim) << std::endl;
std::cout << getMin(tim) << std::endl;
std::cout << getHour(tim) << std::endl;
std::cout << getYear(tim) << std::endl;
std::cout << getDay(tim) << std::endl;
std::cout << getMonth(tim) << std::endl;
}
double ossimLocalTm::getJulian()const
{
int J = getMonth();
int K = getDay();
int I = getYear();
return (K-32075+1461*(I+4800+(J-14)/12)/4+367*(J-2-(J-14)/12*12)
/12-3*((I+4900+(J-14)/12)/100)/4+
(getHour()/24.0)+
(getMin()/1440.0)+
((getSec()+theFractionalSecond)/86400.0));
}
ossimRefPtr<ossimXmlNode> ossimLocalTm::saveXml()const
{
ossimRefPtr<ossimXmlNode> result = new ossimXmlNode;
result->setTag("ossimDate");
result->addAttribute("version", "1");
result->addChildNode("month", ossimString::toString(getMonth()));
result->addChildNode("day", ossimString::toString(getDay()));
result->addChildNode("year", ossimString::toString(getYear()));
result->addChildNode("hour", ossimString::toString(getHour()));
result->addChildNode("minutes", ossimString::toString(getMin()));
result->addChildNode("seconds", ossimString::toString(getSec()));
result->addChildNode("fractionalSecond", ossimString::toString(getFractionalSecond()));
return result.get();
}
void TTime::print() {
printf("%.2i:%.2i:%.2i", getHour(), getMin(), getSec());
}
int main(int argc, char *argv[])
{
int server_sock, client_sock;
char header[2] = {'$', '#'};
struct sockaddr_in server_addr, client_addr;
pid_t pid;
struct sigaction act;
socklen_t addr_size;
if(argc!=2) {
printf("Usage : %s <port>\n", argv[0]);
exit(1);
}
// signal setting
act.sa_handler = ctrl_childproc;
sigemptyset(&act.sa_mask);
act.sa_flags = 0;
if(sigaction(SIGCHLD, &act, 0) != 0)
error_handling("LOG ( Sigaction() error. )\n");
// socket create
if((server_sock = socket(PF_INET, SOCK_STREAM, 0)) < 0)
error_handling("LOG ( Can't open stream socket. )\n");
// socket addr setting
bzero((char *)&server_addr, sizeof(server_addr));
server_addr.sin_family=AF_INET;
server_addr.sin_addr.s_addr=htonl(INADDR_ANY);
server_addr.sin_port=htons(atoi(argv[1]));
// socket bind
if(bind(server_sock, (struct sockaddr*) &server_addr, sizeof(server_addr)) == -1)
error_handling("LOG ( Bind() error )\n");
// stay for client
if(listen(server_sock, MAX_CLIENT) == -1)
error_handling("LOG ( Listen() error )\n");
while(1)
{
// client connetion
addr_size = sizeof(client_addr);
// try to client accept
if((client_sock = accept(server_sock, (struct sockaddr*)&client_addr, &addr_size)) == -1)
continue;
else
puts("LOG ( New client connected... )");
if((pid = fork()) > 0)
childcnt++;
if(pid == -1)
{
close(client_sock);
continue;
}
// child process
if(pid == 0)
{
close(server_sock);
int tempSec = getSec();
int OESec = tempSec % 2;
char buf[BUF_SIZE];
memset(buf, 0, sizeof(buf));
sprintf(buf, "%c%d_0123456789\n", header[childcnt], childcnt);
while(1)
{
char strTemp[256] = { 0, };
int y = getYear();
int mon = getMonth();
int d = getDay();
int h = getHour();
int min = getMin();
int s = getSec();
if (s % 2 == OESec && tempSec != s) {
//////////////////////////////
//add checksum byte...
char setData[1024] = {0,};
int index = 1;
unsigned char checksum = 0;
while (index < strlen(buf) - 1) {
if (c_state == IDLE) {
c_state =
(buf[0] == '$' || buf[0] == '#') ?
PAYLOAD : IDLE;
} else if (c_state == PAYLOAD) {
printf(
"add buf[%d] = %c, checksum = (%%c)%c, (%%x)%x\n",
index, buf[index], checksum, checksum);
checksum ^= (buf[index++] & 0xFF);
}
}
printf("checksum = %x\n", checksum);
////////////////////////////////////////////////////////////////
sprintf(setData, "%s%c", buf, checksum);
sprintf(strTemp,
"%04d-%02d-%02d %02dhour %02dmin %02dsec\n", y, mon,
d, h, min, s);
strcat(strTemp, setData);
tempSec = s;
if(write(client_sock, setData, strlen(setData)) < 0)
break;
printf("send to client : %s\n", strTemp);
memset(strTemp, 0, sizeof(strTemp));
}
}
close(client_sock);
puts("LOG ( Client disconnected... )");
return 0;
}
else
close(client_sock);
}
close(server_sock);
return 0;
}
int main(int argc, char *argv[])
{
int fdElf;
int lenElf;
unsigned char * binElf;
unsigned int entryElf;
unsigned int flagsElf;
unsigned int ramAddr;
unsigned int romram;
int secElf;
int count;
int temp;
unsigned int utemp;
int startSec, endSec;
unsigned int start, src, size, end, type, flag;
unsigned int maxFlash, lowFlash, highFlash;
usb_dev_handle *udev;
struct usb_device *dev;
int found;
struct usb_bus *bus;
char cserial[256];
tIntermediateBuffer* actBuf = NULL;
tIntermediateBuffer* startBuf = NULL;
if ((argc < 2) || (argc > 3))
{
printf("lpc21iap version v%d.%d, ", LPC21IAP_VER_MAJ, LPC21IAP_VER_MIN);
printf("usage: %s file.elf [usb_serial_number]\n", argv[0]);
exit(1);
}
fdElf = open(argv[1], O_RDONLY | O_BINARY);
if(fdElf < 0)
{
perror("open file");
exit(1);
}
lenElf = lseek(fdElf, 0, SEEK_END);
if(lenElf <= 0)
{
perror("file length");
exit(1);
}
lseek(fdElf, 0, SEEK_SET);
binElf = malloc(lenElf);
if (binElf == NULL)
{
perror("malloc failed");
exit(1);
}
startBuf = malloc(sizeof(tIntermediateBuffer));
if (startBuf == NULL)
{
perror("malloc failed");
exit(1);
}
startBuf->start = 0xFFFFFFFF;
startBuf->next = NULL;
memset(startBuf->data, 0xFF, MAX_SECT);
count = 0;
while (count < lenElf)
{
temp = read(fdElf, binElf+count, lenElf-count);
if (temp <= 0)
{
perror("could not read");
exit(1);
}
count += temp;
}
close(fdElf);
/* check ELF header */
if (!ELFCheckHeader(binElf))
{
perror("no ELF header" );
exit(1);
}
flagsElf = ELFHdrFlags(binElf);
if (EF_ARM_HASENTRY == (flagsElf & EF_ARM_HASENTRY))
{
entryElf = ELFEntryAddr(binElf);
}
secElf = ELFNoPSections(binElf);
found = 0;
usb_init();
usb_find_busses();
for(count = 0; count < 100 && !found; count++)
{
printf(".");
fflush(stdout);
if (usb_find_devices())
{
for (bus = usb_get_busses(); bus && (!found); bus = bus->next)
{
for (dev = bus->devices; dev && (!found); dev = dev->next)
{
if ((dev->descriptor.idVendor == VENDOR_ID) &&
(dev->descriptor.idProduct == DEVICE_ID))
{
udev = usb_open(dev);
if (udev)
{
printf("\nFound USB device\n");
if (argc==3)
{
if (dev->descriptor.iSerialNumber)
{
if (usb_get_string_simple(udev, dev->descriptor.iSerialNumber, cserial, sizeof(cserial)) > 0)
{
if (strncmp(argv[2], cserial, strlen(argv[2])) == 0)
{
found = 1;
}
else
{
printf("Serial Number does not match: %s\n", cserial);
}
}
else
{
printf("could not get serial number\n");
}
}
else
{
printf("no serial number\n");
}
if (!found) usb_close(udev);
}
else found = 1;
}
}
}
}
}
Sleep(200);
}
if(!found)
{
printf("\nno USB device\n");
exit(1);
}
if (!unlock(udev)) exit(1);
if (!readBootCode(udev, &utemp)) exit(1);
printf("BootROM code: %d.%d\n", (utemp >> 8) & 0xFF, utemp & 0xFF);
if (!readPartID(udev, &utemp)) exit(1);
for (count = 0; LPCtypes[count].id != 0; count++)
{
if (utemp == LPCtypes[count].id) break;
}
printf("Part ID: 0x%08X (%s)\n", utemp, LPCtypes[count].name);
if (!readBootloaderVersion(udev, &utemp)) exit(1);
printf("BootLoader version: %d.%d\n", (utemp >> 8) & 0xFF, utemp & 0xFF);
if (!readRAMAddress(udev, &ramAddr)) exit(1);
if (!readBootloaderLocation(udev, &romram)) exit(1);
/* find used sectors (kill the unused in-betweens, speed optimized) */
/* calc maximum usable flash address */
maxFlash = 0;
for (count=0;
count < sizeof(SectorTable_214x)/sizeof(SectorTable_214x[0]);
count++)
{
maxFlash += SectorTable_214x[count];
}
/* find min and max used flash address */
lowFlash = maxFlash;
highFlash = 0;
for (count=0; count < secElf; count++)
{
start = ELFAddrPSection(binElf, count);
src = ELFOffsPSection(binElf, count);
size = ELFSizePSection(binElf, count);
type = ELFTypePSection(binElf, count);
flag = ELFFlagPSection(binElf, count);
if ((size > 0) &&
(src != 0) &&
(type & PT_LOAD) &&
(flag & (PF_X | PF_W | PF_R)) &&
(start+size <= maxFlash))
{
if (start+size > highFlash) highFlash = start+size;
if (start < lowFlash) lowFlash = start;
}
}
if ((romram == RUN_ROM) && (lowFlash < BOOTLOAD_SECT*MAX_SECT))
{
printf("running from ROM, can not flash low sectors, load RAM version first\n");
exit(2);
}
/* anything to flash erase? */
if (lowFlash != maxFlash)
{
startSec = getSec(lowFlash);
endSec = getSec(highFlash);
/* prepare sectors for erase */
if (!prepareSectors(udev, startSec, endSec)) exit(1);
/* erase sectors */
if (!eraseSectors(udev, startSec, endSec)) exit(1);
}
for (count=0; count < secElf; count++)
{
start = ELFAddrPSection(binElf, count);
src = ELFOffsPSection(binElf, count);
size = ELFSizePSection(binElf, count);
type = ELFTypePSection(binElf, count);
flag = ELFFlagPSection(binElf, count);
/* adjust size to 32 bit alignment */
size = (size + 3) & 0xFFFFFFFC;
if ((size > 0) &&
(src != 0) &&
(type & PT_LOAD) &&
(flag & (PF_X | PF_W | PF_R)))
{
if (start+size <= maxFlash)
{
unsigned int cnt;
unsigned int startPage, endPage, countPage;
unsigned int splitGap, splitCnt;
#if 0
/* Flash */
printf( "Flash sect = %02d, start = 0x%08X, src = 0x%08X, size = 0x%08X, type = 0x%08X, flag = 0x%08X\n",
count,
start,
src,
size,
type,
flag);
#endif
/* end address in flash */
end = start + size;
cnt = 0;
/* page address of the first sector */
startPage = start & ~(MAX_SECT-1);
/* page address of the end sector */
endPage = (end+MAX_SECT) & ~(MAX_SECT-1);
/* always 4k "blocks" are written */
for (countPage = startPage; countPage < endPage; countPage += MAX_SECT)
{
/* do checksum for first block (LE machines) */
if (countPage == 0)
{
unsigned int * dat = (unsigned int *) (binElf+src);
unsigned int crc = 0;
int count;
dat[5] = 0;
for(count = 0; count < 8; count++) {
crc += dat[count];
}
dat[5] = (unsigned long) -crc;
printf("changing vector table");
}
printf("#");
fflush(stdout);
splitCnt = MAX_SECT;
/* The LPC214x does not allow pages (4k) to be
programmed twice without erasing in between, not even
with 0xFF's being filled into the gaps. So we have to
store all the partly filled pages until the end and
flash them in one flush. */
/* first page, not beginning at page start */
if ((countPage == startPage) && (start != startPage))
{
/* gap of non programmed bytes at the beginning of page */
splitGap = start % MAX_SECT;
/* number of bytes in the first block */
if ((splitGap + size) < MAX_SECT) {
/* data ends within first page */
splitCnt = size;
}
else {
/* data until end of page */
splitCnt = MAX_SECT - splitGap;
}
}
/* last page, not ending at page end */
else if ((countPage+MAX_SECT == endPage) && (end != endPage))
{
/* no gap at beginning */
splitGap = 0;
/* number of bytes in the last sector */
splitCnt = end % MAX_SECT;
}
if (splitCnt != MAX_SECT)
{
/* keep partly filled pages for later programming */
actBuf = startBuf;
while (1)
{
if (actBuf->start == countPage)
{
break;
}
if (actBuf->next == NULL)
{
actBuf->next = malloc(sizeof(tIntermediateBuffer));
if (actBuf->next == NULL)
{
perror("malloc failed");
exit(1);
}
actBuf = actBuf->next;
actBuf->start = countPage;
actBuf->next = NULL;
memset(actBuf->data, 0xFF, MAX_SECT);
break;
}
actBuf = actBuf->next;
}
/* copy data to buffer */
// read(startBuf->data + splitGap, splitCnt):
memcpy(actBuf->data + splitGap, binElf+src+cnt, splitCnt);
}
else
{
/* flash full filled 4k block */
/* copy data to buffer */
// read(startBuf->data, MAX_SECT):
memcpy(startBuf->data, binElf+src+cnt, MAX_SECT);
if (!writeRAM(udev, ramAddr, MAX_SECT)) exit(1);
if (!USBReqData(udev, startBuf->data, MAX_SECT)) exit(1);
if (!prepareSectors(udev, getSec(countPage), getSec(countPage))) exit(1);
if (!copyRAMFlash(udev, countPage, ramAddr, MAX_SECT)) exit(1);
if (!compareMem(udev, countPage, ramAddr, MAX_SECT)) exit(1);
}
cnt += splitCnt;
}
}
else
{
#if 0
/* RAM */
printf( "RAM sect = %02d, start = 0x%08X, src = 0x%08X, size = 0x%08X, type = 0x%08X, flag = 0x%08X\n",
count,
start,
src,
size,
type,
flag);
#endif
/* copy it in one piece */
if (!writeRAM(udev, start, size)) exit(1);
if (!USBReqData(udev, (unsigned char*) (binElf+src), size)) exit(1);
}
}
}
actBuf = startBuf;
/* go through all the split sectors */
while (actBuf != NULL)
{
if (actBuf->start != 0xFFFFFFFF)
{
if (!writeRAM(udev, ramAddr, MAX_SECT)) exit(1);
if (!USBReqData(udev, actBuf->data, MAX_SECT)) exit(1);
if (!prepareSectors(udev, getSec(actBuf->start), getSec(actBuf->start))) exit(1);
if (!copyRAMFlash(udev, actBuf->start & ~(MAX_SECT-1), ramAddr, MAX_SECT)) exit(1);
if (!compareMem(udev, actBuf->start & ~(MAX_SECT-1), ramAddr, MAX_SECT)) exit(1);
}
startBuf = actBuf;
actBuf = actBuf->next;
free(startBuf);
}
printf("\n");
free(binElf);
if (EF_ARM_HASENTRY == (flagsElf & EF_ARM_HASENTRY))
{
printf( "Starting software at 0x%08X\n", entryElf );
if (!goSw(udev, entryElf)) exit(1);
}
return(0);
}
int timeInSecs(){
return 3600*getHour() + 60*getMin() + getSec();
}
bool servosEnabled = true; uint32_t photogram = 1; int rollServoSpeed = ROLL_SERVO_SPEED; int pitchServoSpeed = PITCH_SERVO_SPEED; int rollServoDelay = ROLL_SERVO_DELAY; int pitchServoDelay = PITCH_SERVO_DELAY; short desiredx = 0; short desiredy = 0; int32_t lastPhotogramSpentTime; int64_t lastPositionTime; int64_t microsSinceStart = 0; int64_t photogramMicros = 0; int64_t lastPhotogramMicros = 0; int64_t lastPositionedMicros = -1; int64_t startSec = getSec(); float droneRoll = 0; float dronePitch = 0; float droneYaw = 0; float roll = 0; // Camera roll float pitch = 0; // Camera pitch float yawIncrement = 0; Container beaconContainerA; Container beaconContainerB; Container nominalContainer; Container * beaconContainer; Container * oldBeaconContainer; Output output(8080); Vector3<float> glitchOffset; IPC ipc; sem_t semCam;
long double Timer::getMin() {
return getSec() / 60.0;
}