eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

shortcmd checked

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-02-16 21:59:08 +03:00
parent 53b18299dc
commit 5250aa185d
13 changed files with 281 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
LibSidServo/serial.c
View File

@@ -32,7 +32,6 @@
#include "dbg.h"
#include "serial.h"
#include "ssii.h"
// serial devices FD
static int encfd = -1, mntfd = -1;
@@ -338,16 +337,14 @@ int openMount(const char *path, int speed){
void closeSerial(){
if(mntfd > -1){
DBG("Kill mount thread");
if(0 == pthread_cancel(mntthread))
pthread_join(mntthread, NULL);
pthread_cancel(mntthread);
DBG("close fd");
close(mntfd);
mntfd = -1;
}
if(encfd > -1){
DBG("Kill encoder thread");
if(0 == pthread_cancel(encthread))
pthread_join(encthread, NULL);
pthread_cancel(encthread);
DBG("close fd");
close(encfd);
encfd = -1;
@@ -363,21 +360,19 @@ mcc_errcodes_t getMD(mountdata_t *d){
return MCC_E_OK;
}
/**
* @brief MountWriteRead - write and read @ once (or only read/write)
* @param out (o) - data to write or NULL if not need
* @param in (i) - data to read or NULL if not need
* @return FALSE if failed
*/
int MountWriteRead(const data_t *out, data_t *in){
// write-read without locking mutex (to be used inside other functions)
static int wr(const data_t *out, data_t *in, int needeol){
if((!out && !in) || mntfd < 0) return FALSE;
int ret = FALSE;
pthread_mutex_lock(&mntmutex);
if(out){
if(out->len != (size_t)write(mntfd, out->buf, out->len)) goto ext;
if(out->len != (size_t)write(mntfd, out->buf, out->len)){
DBG("written bytes not equal to need");
return FALSE;
}
//DBG("Send to mount %zd bytes: %s", out->len, out->buf);
int g = write(mntfd, "\r", 1); // add EOL
(void) g;
if(needeol){
int g = write(mntfd, "\r", 1); // add EOL
(void) g;
}
}
if(in){
in->len = 0;
@@ -387,8 +382,79 @@ int MountWriteRead(const data_t *out, data_t *in){
in->buf[in->len++] = (uint8_t) b;
}
}
ret = TRUE;
ext:
return TRUE;
}
/**
* @brief MountWriteRead - write and read @ once (or only read/write)
* @param out (o) - data to write or NULL if not need
* @param in (i) - data to read or NULL if not need
* @return FALSE if failed
*/
int MountWriteRead(const data_t *out, data_t *in){
pthread_mutex_lock(&mntmutex);
int ret = wr(out, in, 1);
pthread_mutex_unlock(&mntmutex);
return ret;
}
#ifdef EBUG
static void logscmd(SSscmd *c){
printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed);
printf("xychange=0x%02X, Xbits=0x%02X, Ybits=0x%02X\n", c->xychange, c->XBits, c->YBits);
if(c->checksum != SScalcChecksum((uint8_t*)c, sizeof(SSscmd)-2)) printf("Checksum failed\n");
else printf("Checksum OK\n");
}
static void loglcmd(SSlcmd *c){
printf("Xmot=%d, Ymot=%d, Xspeed=%d, Yspeed=%d\n", c->Xmot, c->Ymot, c->Xspeed, c->Yspeed);
printf("Xadder=%d, Yadder=%d, Xatime=%d, Yatime=%d\n", c->Xadder, c->Yadder, c->Xatime, c->Yatime);
if(c->checksum != SScalcChecksum((uint8_t*)c, sizeof(SSlcmd)-2)) printf("Checksum failed\n");
else printf("Checksum OK\n");
}
#endif
// send short/long binary command; return FALSE if failed
static int bincmd(uint8_t *cmd, int len){
static data_t *dscmd = NULL, *dlcmd = NULL;
if(!dscmd) dscmd = cmd2dat(CMD_SHORTCMD);
if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD);
int ret = FALSE;
pthread_mutex_lock(&mntmutex);
// dummy buffer to clear trash in input
char ans[300];
data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
if(len == sizeof(SSscmd)){
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Short command");
logscmd((SSscmd*)cmd);
if(!wr(dscmd, &a, 1)) goto rtn;
}else if(len == sizeof(SSlcmd)){
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Long command");
loglcmd((SSlcmd*)cmd);
if(!wr(dlcmd, &a, 1)) goto rtn;
}else{
goto rtn;
}
DBG("Write %d bytes and wait for ans", len);
data_t d;
d.buf = cmd;
d.len = d.maxlen = len;
ret = wr(&d, &d, 0);
#ifdef EBUG
if(len == sizeof(SSscmd)) logscmd((SSscmd*)cmd);
else loglcmd((SSlcmd*)cmd);
#endif
DBG("%s", ret ? "SUCCESS" : "FAIL");
rtn:
pthread_mutex_unlock(&mntmutex);
return ret;
}
// return TRUE if OK
int cmdS(SSscmd *cmd){
return bincmd((uint8_t *)cmd, sizeof(SSscmd));
}
int cmdL(SSlcmd *cmd){
return bincmd((uint8_t *)cmd, sizeof(SSlcmd));
}