void sys_app_process()
{
#ifdef CONFIG_UART
/* UART 、SPI 收发处理,驱动层*/
uart_driver_process();
watchdog();
//timer8N();
#endif
/* plc mac层收发处理，驱动层 */
plc_mac_proc();
watchdog();
//timer8N();
#ifdef CONFIG_HW2000
/* 2.4G mac层收发处理，驱动层 */
hw2000_driver_process();
watchdog();
#endif
/* 链路层处理，包括了plc和2.4G或其他的协议 */
linklay_process();
watchdog();
//timer8N();
/* 上层处理，和具体的产品相关 */
app_process();
watchdog();
//timer8N();
}
void init_watchdog(void)
{
pid_t pid;
fflush(stdout);
pid = fork();
if (pid == 0) {
const char watchdogname[17]="trinity-watchdog";
__unused__ int ret = nice(-20);
watchdog_pid = getpid();
if (wait_for_shmready() == FALSE)
return;
output(0, "Watchdog is alive. (pid:%d)\n", watchdog_pid);
prctl(PR_SET_NAME, (unsigned long) &watchdogname);
(void)signal(SIGSEGV, SIG_DFL);
watchdog();
output(0, "[%d] Watchdog exiting because %s.\n",
watchdog_pid, decode_exit());
_exit(EXIT_SUCCESS);
} else {
watchdog_pid = pid;
output(0, "Started watchdog process, PID is %d\n", watchdog_pid);
}
}
int err (userT *user, char *str)
{
int fd = user->fd;
if ( (fd < 0) || (str == NULL) )
{
return (-1);
}
user->errors++;
if (abfab (fd, ERR, sizeof (char) * (strlen (ERR))) < 0)
{
// Error
return (-1);
}
abfab (fd, " ", sizeof (char));
abfab (fd, " ", sizeof (char));
abfab (fd, str, sizeof (char) * (strlen (str)));
abfab (fd, CRLF, 2*sizeof (char));
if (LOG_LEVEL > 39)
{
#ifdef DBUG
printf ("%s: -ERR %s\n", user->username, str);
#endif
syslog (LOG_NOTICE, "%s: -ERR %s\n", user->username, str);
}
watchdog (user);
return (0);
}
int watchdog_main(int argc, char *argv[])
{
/*
* Run it under background
*/
switch (fork()) {
case -1:
perror("fork failed");
exit(1);
break;
case 0:
/*
* child process
*/
watchdog();
exit(0);
break;
default:
/*
* parent process should just die
*/
_exit(0);
}
return 0;
}
bool ESP8266wifi::send(char channel, const char * message, bool sendNow){
watchdog();
byte avail = sizeof(msgOut) - strlen(msgOut) - 1;
strncat(msgOut, message, avail);
if (!sendNow)
return true;
byte length = strlen(msgOut);
if(flags.endSendWithNewline)
length += 2;
writeCommand(CIPSEND);
_serialOut -> print(channel);
writeCommand(COMMA);
_serialOut -> println(length);
byte prompt = readCommand(1000, PROMPT, LINK_IS_NOT);
if (prompt != 2) {
if(flags.endSendWithNewline)
_serialOut -> println(msgOut);
else
_serialOut -> print(msgOut);
byte sendStatus = readCommand(5000, SEND_OK, BUSY);
if (sendStatus == 1) {
msgOut[0] = '\0';
if(channel == SERVER)
flags.connectedToServer = true;
return true;
}
}
//else
if(channel == SERVER)
flags.connectedToServer = false;
msgOut[0] = '\0';
return false;
}
bool Camera_SSAGClass::Capture(int duration, usImage& img, int options, const wxRect& subframe)
{
// Only does full frames
unsigned short *dptr;
int xsize = FullSize.GetWidth();
int ysize = FullSize.GetHeight();
bool firstimg = true;
//qglogfile->AddLine(wxString::Format("Capturing dur %d",duration)); //qglogfile->Write();
_SSAG_ProgramCamera(0, 0, 1280, 1024, (GuideCameraGain * 63 / 100));
if (img.Init(FullSize))
{
DisconnectWithAlert(CAPT_FAIL_MEMORY);
return true;
}
CameraWatchdog watchdog(duration, GetTimeoutMs());
_SSAG_ThreadedExposure(duration, NULL);
//qglogfile->AddLine("Exposure programmed"); //qglogfile->Write();
if (duration > 100)
{
if (WorkerThread::MilliSleep(duration - 100, WorkerThread::INT_ANY) &&
(WorkerThread::TerminateRequested() || StopExposure()))
{
return true;
}
}
while (_SSAG_isExposing())
{
wxMilliSleep(50);
if (WorkerThread::InterruptRequested() &&
(WorkerThread::TerminateRequested() || StopExposure()))
{
return true;
}
if (watchdog.Expired())
{
DisconnectWithAlert(CAPT_FAIL_TIMEOUT);
return true;
}
}
//qglogfile->AddLine("Exposure done"); //qglogfile->Write();
dptr = img.ImageData;
_SSAG_GETBUFFER(dptr, img.NPixels * 2);
if (options & CAPTURE_SUBTRACT_DARK) SubtractDark(img);
//qglogfile->AddLine("Image loaded"); //qglogfile->Write();
return false;
}
static void *centerMain(void *arg)
{
int cnt;
processlist = NULL; // processlist = empty
cnt = 99999; // cnt = expired
do_reload(0); // reload speadsheat
while(1)
{
if(toCenter.poll() == rlFifo::DATA_AVAILABLE)
{
toCenter.read(interpreter.line,rl_PRINTF_LENGTH-1);
if (interpreter.isCommand("sigterm(")) do_sigterm();
else if(interpreter.isCommand("sigkill(")) do_sigkill();
else if(interpreter.isCommand("reload(")) do_reload(1);
else if(interpreter.isCommand("remove(")) do_remove();
else if(interpreter.isCommand("save(")) do_save();
if(processes_have_been_modified) do_save();
}
else if(cnt++ >= WATCHDOG_CYCLE_TIME)
{
cnt = 0;
watchdog();
if(processes_have_been_modified) do_save();
}
else
{
rlsleep(1000); // 1 second
}
if(arg == NULL) return NULL; // just to fool the compiler
}
}
void ATCommandHandler::run() {
std::cout << std::endl << "AtCommand handler started and working" << std::endl;
navDataDemoMode();
fTrim();
takeOff();
watchdog();
move();
land();
}
void MavStatus::sendStatus()
{
static unsigned int seq = 0;
status_msg_.header.seq = seq;
status_msg_.header.frame_id = ns_;
status_msg_.fcu_alive = watchdog(last_update_fcu_);
pub_status_.publish(status_msg_);
}
bool Camera_ASCOMLateClass::ST4PulseGuideScope(int direction, int duration)
{
if (!m_hasGuideOutput)
return true;
if (!pMount || !pMount->IsConnected()) return false;
GITObjRef cam(m_gitEntry);
// Start the motion (which may stop on its own)
VARIANTARG rgvarg[2];
rgvarg[1].vt = VT_I2;
rgvarg[1].iVal = direction;
rgvarg[0].vt = VT_I4;
rgvarg[0].lVal = (long) duration;
DISPPARAMS dispParms;
dispParms.cArgs = 2;
dispParms.rgvarg = rgvarg;
dispParms.cNamedArgs = 0;
dispParms.rgdispidNamedArgs =NULL;
MountWatchdog watchdog(duration, 5000);
EXCEPINFO excep;
Variant vRes;
HRESULT hr;
if (FAILED(hr = cam.IDisp()->Invoke(dispid_pulseguide, IID_NULL, LOCALE_USER_DEFAULT, DISPATCH_METHOD,
&dispParms,&vRes,&excep,NULL)))
{
LogExcep(hr, "invoke pulseguide", excep);
return true;
}
if (watchdog.Time() < duration) // likely returned right away and not after move - enter poll loop
{
while (ASCOM_IsMoving(cam.IDisp()))
{
wxMilliSleep(50);
if (WorkerThread::TerminateRequested())
return true;
if (watchdog.Expired())
{
Debug.AddLine("Mount watchdog timed-out waiting for ASCOM_IsMoving to clear");
return true;
}
}
}
return false;
}
/**************************************************************************
* 函数名称：delay_ms
* 功能描述：毫秒级延时函数
* 输入参数：ms 延时毫秒数，输入范围0-6553
* 返回参数：无
* 函数作者：
* 完成日期：
* 修订历史：
* 修订日期：
**************************************************************************/
void delay_ms(uint16_t ms)
{
uint16_t i;
T8N = 0x00;
T8NIF = 0;
T8NC = 0x99; //预分频1:4
for (i = 0; i < 10*ms; i++) {
while (T8NIF == 0); //0.1024ms delay
T8NIF = 0;
watchdog();
}
}
void WatchdogTest::testWatchdog() {
m_timeouts = 0;
ola::Watchdog watchdog(4, ola::NewCallback(this, &WatchdogTest::Timeout));
// Not enabled yet
for (unsigned int i = 0; i < 10; i++) {
watchdog.Clock();
}
OLA_ASSERT_EQ(0u, m_timeouts);
watchdog.Enable();
// Test with regular kicks.
for (unsigned int i = 0; i < 10; i++) {
watchdog.Clock();
if (i % 2) {
watchdog.Kick();
}
}
OLA_ASSERT_EQ(0u, m_timeouts);
// test with non kicks
for (unsigned int i = 0; i < 10; i++) {
watchdog.Clock();
}
OLA_ASSERT_EQ(1u, m_timeouts);
// Disable and re-enable.
watchdog.Disable();
watchdog.Enable();
for (unsigned int i = 0; i < 3; i++) {
watchdog.Clock();
}
OLA_ASSERT_EQ(1u, m_timeouts);
// Disable triggers a reset of the count
watchdog.Disable();
watchdog.Enable();
watchdog.Clock();
watchdog.Clock();
OLA_ASSERT_EQ(1u, m_timeouts);
watchdog.Clock();
watchdog.Clock();
OLA_ASSERT_EQ(2u, m_timeouts);
}
int LongPollThread::Run()
{
SetThreadName("LongPoll");
WatchDog watchdog("LongPoll");
// While not terminated,
do
{
watchdog.Feed(10000);
PollSubject.Call();
WakeEvent.Wait(WakeupInterval);
WakeEvent.ResetEvent();
} while (!Terminated);
return 0;
}
void init_watchdog(void)
{
pid_t pid;
fflush(stdout);
pid = fork();
if (pid == 0) {
watchdog_pid = getpid();
watchdog();
output(0, "[%d] Watchdog exiting\n", watchdog_pid);
_exit(EXIT_SUCCESS);
} else {
watchdog_pid = pid;
output(0, "Started watchdog process, PID is %d\n", watchdog_pid);
}
}
WifiMessage ESP8266wifi::listenForIncomingMessage(int timeout){
watchdog();
char buf[16] = {'\0'};
msgIn[0] = '\0';
static WifiMessage msg;
msg.hasData = false;
msg.channel = '-';
msg.message = msgIn;
//TODO listen for unlink etc...
byte msgOrRestart = readCommand(timeout, IPD, READY);
//Detected a esp8266 restart
if (msgOrRestart == 2){
restart();
return msg;
}
//Message received..
else if (msgOrRestart == 1) {
char channel = readChar();
if (channel == SERVER)
flags.connectedToServer = true;
readChar(); // removing comma
readBuffer(&buf[0], sizeof(buf) - 1, ':'); // read char count
readChar(); // removing ':' delim
byte length = atoi(buf);
readBuffer(&msgIn[0], min(length, sizeof(msgIn) - 1));
msg.hasData = true;
msg.channel = channel;
msg.message = msgIn;
readCommand(10, OK); // cleanup after rx
}
return msg;
}
static void timer(param, psize)
/*ARGSUSED*/
{
static int watch_count;
for (;;) {
sleep(60);
time_update();
/* We must keep the time, which is stored with the directory entries
* consistent among the members. Otherwise programs like 'make' might
* be confused.
*/
broadcast_time();
if ((watch_count++ % WATCHDOG_INTERVAL) == 0) {
if (dirsvr_functioning()) {
MON_EVENT("watchdog");
sp_begin();
watchdog(); /* which calls sp_end() */
}
}
}
}
void process_record(char *inseed)
{
int i;
time_t curr;
FILE *outputfile;
ITEMLIST *taglist=NULL;
outputfile = stdout;
if (pidfile)
watchdog(pidfile);
if (select_file) {
int interesting;
char n[10],s[10],l[10],c[10];
SH_Get_Idents((SEED_DATA *) inseed,n,s,l,c);
TrimString(n);
Upcase(n);
TrimString(s);
Upcase(s);
TrimString(l);
Upcase(l);
TrimString(c);
Upcase(c);
interesting = SelectDateInteresting(&dumproot,
SH_Start_Time((SEED_DATA *) inseed),
SH_End_Time((SEED_DATA *) inseed),
n,s,l,c,
&taglist);
if (!interesting) return;
}
/* Are we capturing? */
if (capture) {
/* See if flush interval is over - if so, we start new file */
curr = time(NULL);
if (curr>now+flushint) {
/* Close old file and rename it - if any */
if (outf) {
fclose(outf);
/* mv temp name to permanent name */
unlink(outf_dest);
i = link(outf_name, outf_dest);
if (i!=0)
fprintf(stderr,"Couldn't link new file %s - %s\n",
outf_dest, strerror(errno));
else
unlink(outf_name);
outf = NULL;
}
/* Open new file */
sprintf(outf_dest, capture, startint++);
strcpy(outf_name, outf_dest);
strcat(outf_name, "_live");
outf = fopen(outf_name,"w");
if (outf==NULL) {
printf("Couldn't write file %s\n",outf_name);
exit(20);
}
if (verbose)
fprintf(stderr,"[Open capture file %s]\n",
outf_dest);
now = curr; /* When did we open the file? */
}
outputfile = outf;
}
if (!extendedoutput) {
i = write(fileno(outputfile), inseed, seed_size);
if (i!=seed_size) {
fprintf(stderr,"Cannot write to output\n");
exit(30);
}
fflush(outputfile);
return;
}
}
bool Camera_INDIClass::Capture(int duration, usImage& img, int options, const wxRect& subframeArg)
{
if (Connected) {
bool takeSubframe = UseSubframes;
wxRect subframe(subframeArg);
// we can set the exposure time directly in the camera
if (expose_prop) {
if (Binning != m_curBinning)
{
FullSize = wxSize(m_maxSize.x / Binning, m_maxSize.y / Binning);
binning_x->value = Binning;
binning_y->value = Binning;
sendNewNumber(binning_prop);
m_curBinning = Binning;
}
if (subframe.width <= 0 || subframe.height <= 0)
{
takeSubframe = false;
}
// Program the size
if (!takeSubframe)
{
subframe = wxRect(0, 0, FullSize.GetWidth(), FullSize.GetHeight());
}
if (subframe != m_roi)
{
frame_x->value = subframe.x*Binning;
frame_y->value = subframe.y*Binning;
frame_width->value = subframe.width*Binning;
frame_height->value = subframe.height*Binning;
sendNewNumber(frame_prop);
m_roi = subframe;
}
//printf("Exposing for %d(ms)\n", duration);
// set the exposure time, this immediately start the exposure
expose_prop->np->value = (double)duration/1000;
sendNewNumber(expose_prop);
modal = true; // will be reset when the image blob is received
unsigned long loopwait = duration > 100 ? 10 : 1;
CameraWatchdog watchdog(duration, GetTimeoutMs());
while (modal) {
wxMilliSleep(loopwait);
if (WorkerThread::TerminateRequested())
return true;
if (watchdog.Expired())
{
DisconnectWithAlert(CAPT_FAIL_TIMEOUT);
return true;
}
}
}
// for video camera without exposure time setting
else if (video_prop) {
takeSubframe = false;
//printf("Enabling video capture\n");
ISwitch *v_on = IUFindSwitch(video_prop,"ON");
ISwitch *v_off = IUFindSwitch(video_prop,"OFF");
v_on->s = ISS_ON;
v_off->s = ISS_OFF;
// start capture, every video frame is received as a blob
sendNewSwitch(video_prop);
// wait the required time
wxMilliSleep(duration); // TODO : add the frames received during exposure
//printf("Stop video capture\n");
v_on->s = ISS_OFF;
v_off->s = ISS_ON;
sendNewSwitch(video_prop);
}
else {
return true;
}
//printf("Exposure end\n");
if (strcmp(cam_bp->format, ".fits") == 0) {
//printf("Processing fits file\n");
// for CCD camera
if ( ! ReadFITS(img,takeSubframe,subframe) ) {
if (options & CAPTURE_SUBTRACT_DARK) {
//printf("Subtracting dark\n");
SubtractDark(img);
}
if (options & CAPTURE_RECON) {
if (PixSizeX != PixSizeY) SquarePixels(img, PixSizeX, PixSizeY);
}
return false;
} else {
return true;
}
} else if (strcmp(cam_bp->format, ".stream") == 0) {
//printf("Processing stream file\n");
// for video camera
return ReadStream(img);
} else {
pFrame->Alert(_("Unknown image format: ") + wxString::FromAscii(cam_bp->format));
return true;
}
}
else {
// in case the camera is not connected
return true;
}
// we must never go here
return true;
}
int main(int argc,char **argv)
{
int c;
extern char *optarg;
extern int optind;
int errflg=0;
int i;
char test1[500],test2[500];
while ((c = getopt(argc, argv, "xvLEb:s:p:c:f:i:P:")) != EOF)
switch (c) {
case 'x':
extendedoutput++;
break;
case 'v':
verbose++;
break;
case 'L':
liss2++;
break;
case 'E':
ignoreeof++;
break;
case 'b':
i = atoi(optarg);
switch(i) {
case 256:
case 512:
case 4096:
seed_size = i;
break;
default:
fprintf(stderr,"Block size must be 256, 512, or 4096\n");
errflg++;
break;
}
break;
case 'p':
tel_port = atoi(optarg);
break;
case 's':
select_file = strdup(optarg);
break;
case 'P':
pidfile = strdup(optarg);
break;
case 'c':
capture = strdup(optarg);
break;
case 'f':
flushint = atoi(optarg);
if (flushint<=5) {
fprintf(stderr,"Flush interval must be >= 5 seconds\n");
errflg++;
}
break;
case 'i':
startint = atoi(optarg);
if (startint<0) {
fprintf(stderr,"Starting sequence must be >= 0\n");
errflg++;
}
break;
case '?':
errflg++;
}
if (capture) {
sprintf(test1,capture,1);
sprintf(test2,capture,9999);
if (strcmp(test1,test2)==0) {
fprintf(stderr,"You must specify capture masks with the %%d operator - like out%%d.seed\n");
errflg++;
}
}
if (liss2) {
printf("LISS2 server connections not supported yet\n");
errflg++;
}
if (extendedoutput) {
printf("Extended (LISS2) mode not yet supported\n");
errflg++;
}
if (errflg)
usage(argc,argv);
if (pidfile)
watchdog(pidfile);
SelectInit(&dumproot);
if (select_file) {
SelectLoadup(&dumproot, select_file);
if (verbose)
SelectList(&dumproot);
}
for (; optind < argc; optind++) {
ifil[num_files++] = strdup(argv[optind]);
if (num_files+1>MAX_FILES)
bombout(EXIT_ABORT,"Too many files - only supports %d",MAX_FILES);
}
if (num_files<=0) usage(argc,argv);
if (num_files>1) usage(argc,argv);
now = 0;
while(1) {
i = process_telemetry(ifil[0], tel_port);
if (!ignoreeof) break;
sleep(20);
}
exit(0);
}
void Control::watchdog() {
watchdog(m_seqNum.fetch_add(1));
}
int smunge (userT *user)
{
// Interactively deals with users pop commands,
// servicing them from the users pop servers,
// smunging the results as needed.
int argc;
char argv0[MAX_ARG_LEN];
char argv1[MAX_ARG_LEN];
char argv2[MAX_ARG_LEN];
char argv3[MAX_ARG_LEN];
int i;
char response[MAX_LINE];
if (!user || (user->fd < 1))
{
return -1;
}
while (user->fd > 0)
{
watchdog (user);
if (user->fd < 0)
{
return (-1);
}
argc=get_command (&user->fd, argv0, argv1, argv2, argv3);
if (argc < 0)
{
// there was a problem.
return (0);
} else
if (argc)
{
if (!strcmp (argv0, "QUIT"))
{
quit (user);
ok (user->fd, "Have a nice day");
return (0);
} else
if (!strcmp (argv0, "DBUG") && (LOG_LEVEL==666))
{
debug (user);
} else
if (!strcmp (argv0, "STAT"))
{
long messages = 0;
long size = 0;
user->time = time(NULL);
for (i=0; i < user->mailboxes; i++)
{
if (user->mailbox[i].fd > 0)
{
if (status (user, i, &messages, &size) == -1)
{
return (0);
}
}
}
snprintf (response, MAX_LINE-1, "%ld %ld", messages, size);
ok (user->fd, response);
} else
if (!strcmp (argv0, "LIST"))
{
user->time = time(NULL);
// if given with no arguments
if (argc == 1)
{
if (list (user) == -1)
{
return (0);
}
} else
{
if (list_s (user, atol(argv1)) == -1)
{
return (0);
}
}
} else
if (!strcmp (argv0, "UIDL"))
{
user->time = time(NULL);
// if given with no arguments
if (argc == 1)
{
if (uidl (user) == -1)
{
return (0);
}
} else
{
if (uidl_s (user, atol(argv1)) == -1)
{
return (0);
}
}
} else
if ((argc == 2) && (!strcmp (argv0, "RETR")))
{
user->time = time(NULL);
if (retr (user, atol(argv1), -1) == -1)
{
return (0);
}
} else
if ((argc == 2) && (!strcmp (argv0, "DELE")))
{
user->time = time(NULL);
if (dele (user, atol(argv1)) == -1)
{
return (0);
}
} else
if ((argc == 3) && (!strcmp (argv0, "TOP")))
{
user->time = time(NULL);
if (atol(argv2) < 0)
{
err (user, "Need to supply a non-negative number of lines");
} else
if (retr (user, atol(argv1), atol(argv2)) == -1)
{
return (0);
}
} else
if (!strcmp (argv0, "RSET"))
{
user->time = time(NULL);
if (rset (user) == -1)
{
return (0);
}
} else
if (!strcmp (argv0, "NOOP"))
{
user->time = time(NULL);
ok (user->fd, "Keeping out of trouble");
} else
{
user->time = time(NULL);
snprintf (response, MAX_LINE-1, "Invalid commmand - %s", argv0);
err (user, response);
}
} else
{
user->time = time(NULL);
err (user, "Talk to me");
}
}
return (0);
}
bool Camera_Altair::Capture(int duration, usImage& img, int options, const wxRect& subframe)
{
if (img.Init(FullSize))
{
DisconnectWithAlert(CAPT_FAIL_MEMORY);
return true;
}
wxRect frame;
wxPoint subframePos; // position of subframe within frame
//bool useSubframe = UseSubframes;
//if (subframe.width <= 0 || subframe.height <= 0)
// useSubframe = false;
//if (useSubframe)
//{
// // ensure transfer size is a multiple of 1024
// // moving the sub-frame or resizing it is somewhat costly (stopCapture / startCapture)
// frame.SetLeft(round_down(subframe.GetLeft(), 32));
// frame.SetRight(round_up(subframe.GetRight() + 1, 32) - 1);
// frame.SetTop(round_down(subframe.GetTop(), 32));
// frame.SetBottom(round_up(subframe.GetBottom() + 1, 32) - 1);
// subframePos = subframe.GetLeftTop() - frame.GetLeftTop();
//}
//else
//{
frame = wxRect(FullSize);
// }
long exposureUS = duration * 1000;
unsigned int cur_exp;
if (Altair_get_ExpoTime(m_handle, &cur_exp) == 0 &&
cur_exp != exposureUS)
{
Debug.AddLine("Altair: set CONTROL_EXPOSURE %d", exposureUS);
Altair_put_ExpoTime(m_handle, exposureUS);
}
long new_gain = cam_gain(m_minGain, m_maxGain, GuideCameraGain);
unsigned short cur_gain;
if (Altair_get_ExpoAGain(m_handle, &cur_gain) == 0 &&
new_gain != cur_gain)
{
Debug.AddLine("Altair: set CONTROL_GAIN %d%% %d", GuideCameraGain, new_gain);
Altair_put_ExpoAGain(m_handle, new_gain);
}
/* bool size_change = frame.GetSize() != m_frame.GetSize();
bool pos_change = frame.GetLeftTop() != m_frame.GetLeftTop();
if (size_change || pos_change)
{
m_frame = frame;
Debug.AddLine("Altair: frame (%d,%d)+(%d,%d)", m_frame.x, m_frame.y, m_frame.width, m_frame.height);
}
if (size_change)
{
StopCapture();
ASI_ERROR_CODE status = ASISetROIFormat(m_handle, frame.GetWidth(), frame.GetHeight(), 1, ASI_IMG_Y8);
if (status != ASI_SUCCESS)
Debug.AddLine("Altair: setImageFormat(%d,%d) => %d", frame.GetWidth(), frame.GetHeight(), status);
}
if (pos_change)
{
ASI_ERROR_CODE status = ASISetStartPos(m_handle, frame.GetLeft(), frame.GetTop());
if (status != ASI_SUCCESS)
Debug.AddLine("Altair: setStartPos(%d,%d) => %d", frame.GetLeft(), frame.GetTop(), status);
}*/
// the camera and/or driver will buffer frames and return the oldest frame,
// which could be quite stale. read out all buffered frames so the frame we
// get is current
//flush_buffered_image(m_handle, img);
if (!m_capturing)
{
Debug.AddLine("Altair: startcapture");
m_frameReady = false;
Altair_StartPullModeWithCallback(m_handle, CameraCallback, this);
m_capturing = true;
}
int frameSize = frame.GetWidth() * frame.GetHeight();
int poll = wxMin(duration, 100);
CameraWatchdog watchdog(duration, duration + GetTimeoutMs() + 10000); // total timeout is 2 * duration + 15s (typically)
if (WorkerThread::MilliSleep(duration, WorkerThread::INT_ANY) &&
(WorkerThread::TerminateRequested() || StopCapture()))
{
return true;
}
while (true)
{
if (m_frameReady)
{
m_frameReady = false;
unsigned int width, height;
if (SUCCEEDED(Altair_PullImage(m_handle, m_buffer, 8, &width, &height)))
break;
}
WorkerThread::MilliSleep(poll, WorkerThread::INT_ANY);
if (WorkerThread::InterruptRequested())
{
StopCapture();
return true;
}
if (watchdog.Expired())
{
Debug.AddLine("Altair: getimagedata failed");
StopCapture();
DisconnectWithAlert(CAPT_FAIL_TIMEOUT);
return true;
}
}
//if (useSubframe)
//{
// img.Subframe = subframe;
// // Clear out the image
// img.Clear();
// for (int y = 0; y < subframe.height; y++)
// {
// const unsigned char *src = m_buffer + (y + subframePos.y) * frame.width + subframePos.x;
// unsigned short *dst = img.ImageData + (y + subframe.y) * FullSize.GetWidth() + subframe.x;
// for (int x = 0; x < subframe.width; x++)
// *dst++ = *src++;
// }
//}
//else
{
for (int i = 0; i < img.NPixels; i++)
img.ImageData[i] = m_buffer[i];
}
if (options & CAPTURE_SUBTRACT_DARK) SubtractDark(img);
return false;
}
int main(int argc, char* argv[])
{
char *path;
char cmd[256];
int udev = 0;
uev_t timer; /* Bootstrap timer, on timeout call finalize() */
uev_ctx_t loop;
/*
* finit/init/telinit client tool uses /dev/initctl pipe
* for compatibility but initctl client tool uses socket
*/
if (getpid() != 1)
return client(argc, argv);
/*
* Initalize event context.
*/
uev_init(&loop);
ctx = &loop;
/*
* Set PATH and SHELL early to something sane
*/
setenv("PATH", _PATH_STDPATH, 1);
setenv("SHELL", _PATH_BSHELL, 1);
/*
* Mount base file system, kernel is assumed to run devtmpfs for /dev
*/
chdir("/");
umask(0);
mount("none", "/proc", "proc", 0, NULL);
mount("none", "/sys", "sysfs", 0, NULL);
if (fisdir("/proc/bus/usb"))
mount("none", "/proc/bus/usb", "usbfs", 0, NULL);
/*
* Parse kernel command line (debug, rescue, splash, etc.)
* Also calls log_init() to set correct log level
*/
conf_parse_cmdline();
/* Set up canvas */
if (!rescue && !log_is_debug())
screen_init();
/*
* In case of emergency.
*/
emergency_shell();
/*
* Initial setup of signals, ignore all until we're up.
*/
sig_init();
/*
* Load plugins early, finit.conf may contain references to
* features implemented by plugins.
*/
plugin_init(&loop);
/*
* Hello world.
*/
banner();
/*
* Check file filesystems in /etc/fstab
*/
for (int pass = 1; pass < 10 && !rescue; pass++) {
if (fsck(pass))
break;
}
/*
* Initialize .conf system and load static /etc/finit.conf
* Also initializes global_rlimit[] for udevd, below.
*/
conf_init();
/*
* Some non-embedded systems without an initramfs may not have /dev mounted yet
* If they do, check if system has udevadm and perform cleanup from initramfs
*/
if (!fismnt("/dev"))
mount("udev", "/dev", "devtmpfs", MS_RELATIME, "size=10%,nr_inodes=61156,mode=755");
else if (whichp("udevadm"))
run_interactive("udevadm info --cleanup-db", "Cleaning up udev db");
/* Some systems use /dev/pts */
makedir("/dev/pts", 0755);
mount("devpts", "/dev/pts", "devpts", 0, "gid=5,mode=620");
/*
* Some systems rely on us to both create /dev/shm and, to mount
* a tmpfs there. Any system with dbus needs shared memory, so
* mount it, unless its already mounted, but not if listed in
* the /etc/fstab file already.
*/
makedir("/dev/shm", 0755);
if (!fismnt("/dev/shm") && !ismnt("/etc/fstab", "/dev/shm"))
mount("shm", "/dev/shm", "tmpfs", 0, NULL);
/*
* New tmpfs based /run for volatile runtime data
* For details, see http://lwn.net/Articles/436012/
*/
if (fisdir("/run") && !fismnt("/run"))
mount("tmpfs", "/run", "tmpfs", MS_NODEV, "mode=0755,size=10%");
umask(022);
/* Bootstrap conditions, needed for hooks */
cond_init();
/*
* Populate /dev and prepare for runtime events from kernel.
* Prefer udev if mdev is also available on the system.
*/
path = which("udevd");
if (!path)
path = which("/lib/systemd/systemd-udevd");
if (path) {
/* Desktop and server distros usually have a variant of udev */
udev = 1;
/* Register udevd as a monitored service */
snprintf(cmd, sizeof(cmd), "[S12345789] pid:udevd %s -- Device event managing daemon", path);
if (service_register(SVC_TYPE_SERVICE, cmd, global_rlimit, NULL)) {
_pe("Failed registering %s", path);
udev = 0;
} else {
snprintf(cmd, sizeof(cmd), ":1 [S] <svc%s> "
"udevadm trigger -c add -t devices "
"-- Requesting device events", path);
service_register(SVC_TYPE_RUN, cmd, global_rlimit, NULL);
snprintf(cmd, sizeof(cmd), ":2 [S] <svc%s> "
"udevadm trigger -c add -t subsystems "
"-- Requesting subsystem events", path);
service_register(SVC_TYPE_RUN, cmd, global_rlimit, NULL);
}
free(path);
} else {
path = which("mdev");
if (path) {
/* Embedded Linux systems usually have BusyBox mdev */
if (log_is_debug())
touch("/dev/mdev.log");
snprintf(cmd, sizeof(cmd), "%s -s", path);
free(path);
run_interactive(cmd, "Populating device tree");
}
}
/*
* Start built-in watchdog as soon as possible, if enabled
*/
wdogpid = watchdog(argv[0]);
/*
* Mount filesystems
*/
if (!rescue) {
#ifdef REMOUNT_ROOTFS
run("mount -n -o remount,rw /");
#endif
#ifdef SYSROOT
mount(SYSROOT, "/", NULL, MS_MOVE, NULL);
#endif
}
if (!rescue) {
_d("Root FS up, calling hooks ...");
plugin_run_hooks(HOOK_ROOTFS_UP);
umask(0);
if (run_interactive("mount -na", "Mounting filesystems"))
plugin_run_hooks(HOOK_MOUNT_ERROR);
_d("Calling extra mount hook, after mount -a ...");
plugin_run_hooks(HOOK_MOUNT_POST);
run("swapon -ea");
umask(0022);
}
/* Base FS up, enable standard SysV init signals */
sig_setup(&loop);
if (!rescue) {
_d("Base FS up, calling hooks ...");
plugin_run_hooks(HOOK_BASEFS_UP);
}
/*
* Set up inotify watcher for /etc/finit.d and read all .conf
* files to figure out how to bootstrap the system.
*/
conf_monitor(&loop);
/*
* Initalize state machine and start all bootstrap tasks
* NOTE: no network available!
*/
sm_init(&sm);
sm_step(&sm);
/* Debian has this little script to copy generated rules while the system was read-only */
if (udev && fexist("/lib/udev/udev-finish"))
run_interactive("/lib/udev/udev-finish", "Finalizing udev");
/* Start new initctl API responder */
api_init(&loop);
umask(022);
/*
* Wait for all SVC_TYPE_RUNTASK to have completed their work in
* [S], or timeout, before calling finalize()
*/
_d("Starting bootstrap finalize timer ...");
uev_timer_init(&loop, &timer, service_bootstrap_cb, finalize, 1000, 1000);
/*
* Enter main loop to monior /dev/initctl and services
*/
_d("Entering main loop ...");
return uev_run(&loop, 0);
}
bool Camera_DSIClass::Capture(int duration, usImage& img, int options, const wxRect& subframe)
{
MeadeCam->SetGain((unsigned int) (GuideCameraGain * 63 / 100));
MeadeCam->SetExposureTime(duration);
if (img.Init(MeadeCam->GetWidth(), MeadeCam->GetHeight()))
{
DisconnectWithAlert(CAPT_FAIL_MEMORY);
return true;
}
bool retval = MeadeCam->GetImage(img.ImageData, true);
if (!retval)
return true;
// The AbortImage method does not appear to work with the DSI camera. If abort is called and the thread is terminated, the
// pending image is still downloaded and PHD2 will crash
#if AbortActuallyWorks
CameraWatchdog watchdog(duration, GetTimeoutMs());
// wait for image to finish and d/l
while (!MeadeCam->ImageReady)
{
wxMilliSleep(20);
if (WorkerThread::InterruptRequested())
{
MeadeCam->AbortImage();
return true;
}
if (watchdog.Expired())
{
MeadeCam->AbortImage();
DisconnectWithAlert(CAPT_FAIL_TIMEOUT);
return true;
}
}
#else // handle the pending image download, regardless
// We also need to prevent the thread from being killed when phd2 is closed
WorkerThreadKillGuard _guard;
if (duration > 100) {
wxMilliSleep(duration - 100); // wait until near end of exposure, nicely
}
bool still_going = true;
while (still_going) { // wait for image to finish and d/l
wxMilliSleep(20);
still_going = !(MeadeCam->ImageReady);
}
#endif // end of waiting for the image
if (options & CAPTURE_SUBTRACT_DARK) SubtractDark(img);
if (options & CAPTURE_RECON)
{
if (MeadeCam->IsColor)
QuickLRecon(img);
if (MeadeCam->IsDsiII)
SquarePixels(img, 8.6, 8.3);
else if (!MeadeCam->IsDsiIII) // Original DSI
SquarePixels(img, 9.6, 7.5);
}
return false;
}
/*
* Purpose:
* Input:
* Return:
*/
static void *retry_write_pty_thread(
void *arg)
{
Channel *channel = (Channel *)arg;
GSM0710_Frame *frame;
int write_result = 0, written = 0;
unsigned char msc_cmd[4];
unsigned int waiting = 0;
unsigned long msec = 1000;
LOGMUX(LOG_DEBUG, "Created:Chnl_Num=%d, Frame list ptr = 0x%08X", channel->id,
(unsigned int)channel->rx_fl);
// Get first frame and retrieve the number of bytes that is already written
pthread_mutex_lock(&channel->rx_fc_lock);
channel->rx_thread = 1;
frame = _fl_popFrame(channel->rx_fl);
written = channel->rx_fl_written;
channel->rx_fl_written = 0;
pthread_mutex_unlock(&channel->rx_fc_lock);
// First frame doesn't exist, ASSERT
if (frame == NULL) Gsm0710Muxd_Assert(GSM0710MUXD_RXTHREAD_ERR);
/* This function is thread execution function: It should runs as a while loop until some error happens!*/
do {
while (1) {
LOGMUX(LOG_DEBUG, "written=%d, frame_ptr=0x%08X, frame_len=%d", written,
(unsigned int)frame, frame->length);
if ((write_result = write(channel->fd, frame->data + written,
frame->length - written)) >= 0) {
LOGMUX(LOG_INFO, "Write %dBytes to PTY Channel:%d", write_result, channel->id);
//LOGMUX(LOG_DEBUG,"Write PTY data val=0x%02X", *(node->frame_data + already_written_len));
//syslogdump("<P ", node->frame_data + already_written_len, written_len);
written += write_result;
waiting = 0;
if (written < frame->length) {
usleep(msec * (++waiting));
} else {
written = 0;
break;
}
} else {
switch (errno) {
case EINTR:
LOGMUX(LOG_NOTICE, "Interrupt signal EINTR caught");
break;
case EAGAIN:
LOGMUX(LOG_NOTICE, "Interrupt signal EAGAIN caught");
usleep(msec * ((waiting >= 10) ? waiting : waiting++));
break;
default:
if (channel->reopen) {
LOGMUX(LOG_ERR, "channel%d needs to be reopened\n", channel->id);
watchdog(&serial);
} else {
LOGMUX(LOG_ERR, "Unknown interrupt signal errno=%d caught from write()\n"
, errno);
Gsm0710Muxd_Assert(GSM0710MUXD_PTY_WRITE_ERR);
}
break;
}
}
}
pthread_mutex_lock(&channel->rx_fc_lock);
frame = _fl_popFrame(channel->rx_fl);
if (frame == NULL) channel->rx_fc_off = 0;
pthread_mutex_unlock(&channel->rx_fc_lock);
} while (frame != NULL);
/* Send MSC with FC On to modem side: Enable RX again! */
memcpy(msc_cmd, msc_channel_cmd, 4);
msc_cmd[2] = (msc_cmd[2] | (channel->id << 2));
/* The default value of FC bit (i.e.,msc_channel_cmd[3]) : Disable the FC Off flag */
#ifndef __MUX_UT__
write_frame(0, msc_cmd, 4, GSM0710_TYPE_UIH | GSM0710_PF);
#else
if (channel->id < (MAX_NON_GEMINI_NON_DATA_CHNL_NUM + 1)) {
write_frame(0, msc_cmd, 4, GSM0710_TYPE_UIH | GSM0710_PF);
} else {
/* Channel_Num >= (MAX_NON_GEMINI_NON_DATA_CHNL_NUM+1) are used to UT Test: Its peer is a virtual serail device which will not parse the recv data from AP side */
/* For UT Chnl: signal the rx_fc_on_req to ut_thread_serial */
#if 0
pthread_cond_signal(&channel->rx_fc_on_req_signal);
#endif
}
#endif
LOGMUX(LOG_DEBUG, "Terminate Retry Write PTY Thread!");
pthread_mutex_lock(&channel->rx_fc_lock);
channel->rx_thread = 0;
pthread_mutex_unlock(&channel->rx_fc_lock);
pthread_cond_signal(&channel->rx_fc_on_signal);
return NULL;
}
void SDK_mainloop(void)
{
sdkCycleStartTime = T1TC;
WO_SDK.ctrl_mode = 0x00; //0x00: absolute angle and throttle control
sdkLoops++;
parseRxFifo();
// check for new LL command packet
if (packetCmdLL->updated)
{
cmdLLNew = 1;
packetCmdLL->updated = 0;
}
// check if LL commands arrive at max every CMD_MAX_PERIOD ms
if ((sdkLoops % CMD_MAX_PERIOD) == 0)
{
if (cmdLLNew == 1)
{
cmdLLNew = 0;
cmdLLValid++;
}
else
{
cmdLLValid--;
}
if (cmdLLValid < -2)
cmdLLValid = -2; // min 3 packets required
else if (cmdLLValid > 3)
cmdLLValid = 3; // fall back after 3 missed packets
}
// check for motor start/stop packet
if (packetMotors->updated)
{
motor_state = motors.motors;
motor_state_count = 0;
packetMotors->updated = 0;
}
// check for new HL command packet
if (packetCmdHL->updated)
{
packetCmdHL->updated = 0;
// SSDK operates in NED, need to convert from ENU
extPositionCmd.heading = -extPositionCmd.heading + 360000;
extPositionCmd.y = -extPositionCmd.y;
extPositionCmd.z = -extPositionCmd.z;
extPositionCmd.vY = -extPositionCmd.vY;
extPositionCmd.vZ = -extPositionCmd.vZ;
extPositionCmd.vYaw = -extPositionCmd.vYaw;
}
// handle parameter packet
if (packetSSDKParams->updated == 1)
{
packetSSDKParams->updated = 0;
statusData.have_SSDK_parameters = 1;
ssdk_status.have_parameters = 1;
}
// decide which position/state input we take for position control
// SSDK operates in NED --> convert from ENU
switch(config.mode_state_estimation){
case HLI_MODE_STATE_ESTIMATION_HL_SSDK:
extPositionValid = 1;
extPosition.bitfield = 0;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
case HLI_MODE_STATE_ESTIMATION_EXT:
extPositionValid = 1;
extPosition.bitfield = EXT_POSITION_BYPASS_FILTER;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
default:
extPositionValid = 0;
}
// execute ssdk - only executed if ssdk parameters are available
// reads position reference from extPosition
// reads position/velocity command from extPositionCmd
// finally writes to WO_CTRL_Input. therefore, make sure to overwrite it after this call if you don't want to have its output
rt_OneStep();
// --- write commands to LL ------------------------------------------------
short motorsRunning = LL_1khz_attitude_data.status2 & 0x1;
if (motor_state == -1 || motor_state == 2)
{ // motors are either stopped or running --> normal operation
// commands are always written to LL by the Matlab controller, decide if we need to overwrite them
if (extPositionValid > 0 && statusData.have_SSDK_parameters == 1 && config.mode_position_control == HLI_MODE_POSCTRL_HL)
{
WO_CTRL_Input.ctrl = config.position_control_axis_enable;
WO_SDK.ctrl_enabled = 1;
}
else if (cmdLLValid > 0 && (config.mode_position_control == HLI_MODE_POSCTRL_LL || config.mode_position_control == HLI_MODE_POSCTRL_OFF))
{
writeCommand(cmdLL.x, cmdLL.y, cmdLL.yaw, cmdLL.z, config.position_control_axis_enable, 1);
}
else
{
writeCommand(0, 0, 0, 0, 0, 0);
}
}
// start / stop motors, allow commands max for 1.5 s
else if (motor_state == 1)
{
if (motor_state_count < 1500)
{
if (!motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely started
else if (motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = 2;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count ++;
}
else if (motor_state == 0)
{
if (motor_state_count < 1500)
{
if (motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely shut down
else if (!motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count++;
}
else
{
// undefined state, disable everything
writeCommand(0, 0, 0, 0, 0, 0);
}
// TODO: thrust limit in case something really goes wrong, may be removed
if (WO_CTRL_Input.thrust > 4095)
WO_CTRL_Input.thrust = 4095;
// ------------------------------------------------------------------------
// --- send data to UART 0 ------------------------------------------------
if (checkTxPeriod(subscription.imu))
{
sendImuData();
}
if (checkTxPeriod(subscription.rcdata))
{
sendRcData();
}
if (checkTxPeriod(subscription.gps))
{
sendGpsData();
}
if ((sdkLoops + 20) % 500 == 0)
{
sendStatus();
writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_STATUS, (unsigned char*)&ssdk_status, sizeof(ssdk_status));
}
if (checkTxPeriod(subscription.ssdk_debug))
{
ssdk_debug.timestamp = timestamp;
writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_DEBUG, (unsigned char*)&ssdk_debug, sizeof(ssdk_debug));
}
//
UART_send_ringbuffer();
synchronizeTime();
if (packetBaudrate->updated)
{
packetBaudrate->updated = 0;
while (!UART0_txEmpty())
;
}
// ------------------------------------------------------------------------
unsigned int dt;
if (T1TC < sdkCycleStartTime)
dt = (processorClockFrequency() - sdkCycleStartTime) + T1TC;
else
dt = T1TC - sdkCycleStartTime;
cpuLoad = ControllerCyclesPerSecond * ((dt * 1e2) / processorClockFrequency()); // cpu load in %
watchdog();
}
void SDK_mainloop(void)
{
sdkCycleStartTime = T1TC;
WO_SDK.ctrl_mode = 0x02; // attitude and throttle control
WO_SDK.disable_motor_onoff_by_stick = 0;
sdkLoops++;
parseRxFifo();
// check for new LL command packet
if (packetCmdLL->updated)
{
cmdLLNew = 1;
packetCmdLL->updated = 0;
}
// check if LL commands arrive at max every CMD_MAX_PERIOD ms
if ((sdkLoops % CMD_MAX_PERIOD) == 0)
{
if (cmdLLNew == 1)
{
cmdLLNew = 0;
cmdLLValid++;
}
else
{
cmdLLValid--;
}
if (cmdLLValid < -2)
cmdLLValid = -2; // min 3 packets required
else if (cmdLLValid > 3)
cmdLLValid = 3; // fall back after 3 missed packets
}
// check for motor start/stop packet
if (packetMotors->updated)
{
motor_state = motors.motors;
motor_state_count = 0;
packetMotors->updated = 0;
}
// check for camera control commands
if (packetCamera->updated)
{
int cam_pitch = camera.desired_cam_pitch * 1000;
int cam_roll = camera.desired_cam_roll * 1000;
PTU_set_desired_pitch(cam_pitch);
PTU_set_desired_roll(cam_roll);
packetCamera->updated = 0;
}
// check for new HL command packet
if (packetCmdHL->updated)
{
packetCmdHL->updated = 0;
// SSDK operates in NED, need to convert from ENU
extPositionCmd.heading = -extPositionCmd.heading + 360000;
extPositionCmd.y = -extPositionCmd.y;
extPositionCmd.z = -extPositionCmd.z;
extPositionCmd.vY = -extPositionCmd.vY;
extPositionCmd.vZ = -extPositionCmd.vZ;
extPositionCmd.vYaw = -extPositionCmd.vYaw;
if (extPositionCmd.bitfield & EXT_POSITION_CMD_BODYFIXED)
{
float s_yaw, c_yaw;
c_yaw = approxCos((float)extPosition.heading / 1000 / 180 * M_PI);
s_yaw = approxCos(M_halfPI - (float)extPosition.heading / 1000 / 180 * M_PI);
if (extPositionCmd.bitfield & EXT_POSITION_CMD_VELOCITY)
{
float vx = extPositionCmd.vX;
float vy = extPositionCmd.vY;
extPositionCmd.vX = c_yaw * vx - s_yaw * vy;
extPositionCmd.vY = s_yaw * vx + c_yaw * vy;
}
else
{
float x = extPositionCmd.x;
float y = extPositionCmd.y;
extPositionCmd.x = c_yaw * x - s_yaw * y + extPosition.x;
extPositionCmd.y = s_yaw * x + c_yaw * y + extPosition.y;
extPositionCmd.z += extPosition.z;
extPositionCmd.heading += extPosition.heading;
if(extPositionCmd.heading > 360000)
extPositionCmd.heading -= 360000;
// should not happen ...
else if(extPositionCmd.heading < 0)
extPositionCmd.heading += 360000;
}
}
}
// handle parameter packet
if (packetSSDKParams->updated == 1)
{
packetSSDKParams->updated = 0;
statusData.have_SSDK_parameters = 1;
ssdk_status.have_parameters = 1;
}
// decide which position/state input we take for position control
// SSDK operates in NED --> convert from ENU
switch(hli_config.mode_state_estimation){
case HLI_MODE_STATE_ESTIMATION_HL_SSDK:
extPositionValid = 1;
extPosition.bitfield = 0;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
case HLI_MODE_STATE_ESTIMATION_EXT:
extPositionValid = 1;
extPosition.bitfield = EXT_POSITION_BYPASS_FILTER;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
case HLI_MODE_STATE_ESTIMATION_HL_EKF:
DEKF_step(&dekf, timestamp);
if(DEKF_getInitializeEvent(&dekf) == 1)
ssdk_reset_state = 1;
extPositionValid = 1;
break;
default:
extPositionValid = 0;
}
// dekf initialize state machine
// sets the acc/height/gps switch to 0 for 10 loops so that refmodel gets reset to the new state
if (ssdk_reset_state >= 1 && ssdk_reset_state < 10)
{
RO_RC_Data.channel[0] = 2048;
RO_RC_Data.channel[1] = 2048;
RO_RC_Data.channel[2] = 2048;
RO_RC_Data.channel[3] = 2048;
RO_RC_Data.channel[5] = 0;
ssdk_reset_state++;
}
else
{
ssdk_reset_state = 0;
}
// execute ssdk - only executed if ssdk parameters are available
// reads position reference from extPosition
// reads position/velocity command from extPositionCmd
// finally writes to WO_CTRL_Input. therefore, make sure to overwrite it after this call if you don't want to have its output
rt_OneStep();
// --- write commands to LL ------------------------------------------------
short motorsRunning = LL_1khz_attitude_data.status2 & 0x1;
if (motor_state == -1 || motor_state == 2)
{ // motors are either stopped or running --> normal operation
// commands are always written to LL by the Matlab controller, decide if we need to overwrite them
if (extPositionValid > 0 && statusData.have_SSDK_parameters == 1 && hli_config.mode_position_control == HLI_MODE_POSCTRL_HL)
{
WO_CTRL_Input.ctrl = hli_config.position_control_axis_enable;
WO_SDK.ctrl_enabled = 1;
// limit yaw rate:
if(WO_CTRL_Input.yaw > 1000)
WO_CTRL_Input.yaw = 1000;
else if(WO_CTRL_Input.yaw < -1000)
WO_CTRL_Input.yaw = -1000;
}
else if (cmdLLValid > 0 && (hli_config.mode_position_control == HLI_MODE_POSCTRL_LL || hli_config.mode_position_control == HLI_MODE_POSCTRL_OFF))
{
writeCommand(cmdLL.x, -cmdLL.y, -cmdLL.yaw, cmdLL.z, hli_config.position_control_axis_enable, 1);
}
else
{
writeCommand(0, 0, 0, 0, 0, 0);
}
}
// start / stop motors, allow commands max for 1.5 s
else if (motor_state == 1)
{
if (motor_state_count < 1500)
{
if (!motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely started
else if (motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = 2;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count++;
}
else if (motor_state == 0)
{
if (motor_state_count < 1500)
{
if (motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely shut down
else if (!motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count++;
}
else
{
// undefined state, disable everything
writeCommand(0, 0, 0, 0, 0, 0);
}
// TODO: thrust limit in case something really goes wrong, may be removed
if (WO_CTRL_Input.thrust > 4095)
WO_CTRL_Input.thrust = 4095;
// ------------------------------------------------------------------------
// --- send data to UART 0 ------------------------------------------------
if (checkTxPeriod(subscription.imu))
{
sendImuData();
}
if (checkTxPeriod(subscription.rcdata))
{
sendRcData();
}
if (checkTxPeriod(subscription.gps))
{
sendGpsData();
}
if ((sdkLoops + 20) % 500 == 0)
{
sendStatus();
// writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_STATUS, (unsigned char*)&ssdk_status, sizeof(ssdk_status));
}
if (checkTxPeriod(subscription.ssdk_debug))
{
ssdk_debug.timestamp = timestamp;
writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_DEBUG, (unsigned char*)&ssdk_debug, sizeof(ssdk_debug));
}
if (checkTxPeriod(subscription.ekf_state))
{
// sendEkfState();
DEKF_sendState(&dekf, timestamp);
}
if (checkTxPeriod(subscription.mag))
{
sendMagData();
}
//
UART_send_ringbuffer();
synchronizeTime();
if (packetBaudrate->updated)
{
packetBaudrate->updated = 0;
while (!UART0_txEmpty())
;
}
// ------------------------------------------------------------------------
unsigned int dt;
if (T1TC < sdkCycleStartTime)
dt = (processorClockFrequency() - sdkCycleStartTime) + T1TC;
else
dt = T1TC - sdkCycleStartTime;
cpuLoad = ControllerCyclesPerSecond * ((dt * 1e2) / processorClockFrequency()); // cpu load in %
watchdog();
}
bool Camera_ASCOMLateClass::Capture(int duration, usImage& img, int options, const wxRect& subframeArg)
{
bool retval = false;
bool takeSubframe = UseSubframes;
wxRect subframe(subframeArg);
if (subframe.width <= 0 || subframe.height <= 0)
{
takeSubframe = false;
}
bool binning_changed = false;
if (Binning != m_curBin)
{
FullSize = wxSize(m_maxSize.x / Binning, m_maxSize.y / Binning);
binning_changed = true;
}
// Program the size
if (!takeSubframe)
{
subframe = wxRect(0, 0, FullSize.GetWidth(), FullSize.GetHeight());
}
if (img.Init(FullSize))
{
pFrame->Alert(_("Cannot allocate memory to download image from camera"));
return true;
}
GITObjRef cam(m_gitEntry);
EXCEPINFO excep;
if (binning_changed)
{
if (ASCOM_SetBin(cam.IDisp(), Binning, &excep))
{
pFrame->Alert(_("The ASCOM camera failed to set binning. See the debug log for more information."));
return true;
}
m_curBin = Binning;
}
if (subframe != m_roi)
{
ASCOM_SetROI(cam.IDisp(), subframe, &excep);
m_roi = subframe;
}
bool takeDark = HasShutter && ShutterClosed;
// Start the exposure
if (ASCOM_StartExposure(cam.IDisp(), (double)duration / 1000.0, takeDark, &excep))
{
Debug.AddLine(ExcepMsg("ASCOM_StartExposure failed", excep));
pFrame->Alert(ExcepMsg(_("ASCOM error -- Cannot start exposure with given parameters"), excep));
return true;
}
CameraWatchdog watchdog(duration, GetTimeoutMs());
if (duration > 100)
{
// wait until near end of exposure
if (WorkerThread::MilliSleep(duration - 100, WorkerThread::INT_ANY) &&
(WorkerThread::TerminateRequested() || AbortExposure()))
{
return true;
}
}
while (true) // wait for image to finish and d/l
{
wxMilliSleep(20);
bool ready;
EXCEPINFO excep;
if (ASCOM_ImageReady(cam.IDisp(), &ready, &excep))
{
Debug.AddLine(ExcepMsg("ASCOM_ImageReady failed", excep));
pFrame->Alert(ExcepMsg(_("Exception thrown polling camera"), excep));
return true;
}
if (ready)
break;
if (WorkerThread::InterruptRequested() &&
(WorkerThread::TerminateRequested() || AbortExposure()))
{
return true;
}
if (watchdog.Expired())
{
DisconnectWithAlert(CAPT_FAIL_TIMEOUT);
return true;
}
}
// Get the image
if (ASCOM_Image(cam.IDisp(), img, takeSubframe, subframe, &excep))
{
Debug.AddLine(ExcepMsg(_T("ASCOM_Image failed"), excep));
pFrame->Alert(ExcepMsg(_("Error reading image"), excep));
return true;
}
if (options & CAPTURE_SUBTRACT_DARK)
SubtractDark(img);
if (Color && Binning == 1 && (options & CAPTURE_RECON))
QuickLRecon(img);
return false;
}