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

nxt

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-02-04 21:33:02 +03:00
parent a2e2896f29
commit aefdf3912c
9 changed files with 215 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
LibSidServo/examples/dumpmoving.c
View File

@ -51,10 +51,11 @@ void signals(int sig){
}
static conf_t Config = {
.MountPath = "/dev/ttyS1",
.MountSpeed = 19200,
.EncoderPath = "/dev/ttyUSB0",
.EncoderSpeed = 153000
.MountDevPath = "/dev/ttyS1",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB0",
//.EncoderDevSpeed = 153000,
.SepEncoder = 0
};
int main(int argc, char **argv){
@ -67,8 +68,8 @@ int main(int argc, char **argv){
time_t curtime = time(NULL);
LOGMSG("Started @ %s", ctime(&curtime));
DBG("Devices ready");
LOGMSG("Mount device %s @ %d", Config.MountPath, Config.MountSpeed);
LOGMSG("Encoder device %s @ %d", Config.EncoderPath, Config.EncoderSpeed);
LOGMSG("Mount device %s @ %d", Config.MountDevPath, Config.MountDevSpeed);
LOGMSG("Encoder device %s @ %d", Config.EncoderDevPath, Config.EncoderDevSpeed);
signal(SIGTERM, signals); // kill (-15) - quit
signal(SIGHUP, SIG_IGN); // hup - ignore
signal(SIGINT, signals); // ctrl+C - quit

2
LibSidServo/libsidservo.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 15.0.0, 2025-01-30T22:13:51. -->
<!-- Written by QtCreator 15.0.0, 2025-02-04T21:29:56. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

1
LibSidServo/libsidservo.files
View File

@ -6,4 +6,5 @@ sidservo.h
serial.c
examples/CMakeLists.txt
serial.h
ssii.c
ssii.h

16
LibSidServo/main.c
View File

@ -42,22 +42,26 @@ static mcc_errcodes_t init(conf_t *c){
if(!c) return MCC_E_BADFORMAT;
Conf = *c;
mcc_errcodes_t ret = MCC_E_OK;
if(!Conf.MountPath || Conf.MountSpeed < 1200){
if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
DBG("Define mount device path and speed");
ret = MCC_E_BADFORMAT;
}else if(!openMount(Conf.MountPath, Conf.MountSpeed)){
DBG("Can't open %s with speed %d", Conf.MountPath, Conf.MountSpeed);
}else if(!openMount(Conf.MountDevPath, Conf.MountDevSpeed)){
DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed);
ret = MCC_E_MOUNTDEV;
}
if(Conf.SepEncoder){
if(!Conf.EncoderPath || Conf.EncoderSpeed < 1200){
if(!Conf.EncoderDevPath || Conf.EncoderDevSpeed < 1200){
DBG("Define encoder device path and speed");
ret = MCC_E_BADFORMAT;
}else if(!openEncoder(Conf.EncoderPath, Conf.EncoderSpeed)){
DBG("Can't open %s with speed %d", Conf.EncoderPath, Conf.EncoderSpeed);
}else if(!openEncoder(Conf.EncoderDevPath, Conf.EncoderDevSpeed)){
DBG("Can't open %s with speed %d", Conf.EncoderDevPath, Conf.EncoderDevSpeed);
ret = MCC_E_ENCODERDEV;
}
}
if(Conf.MountReqInterval < 1 || Conf.MountReqInterval > 1000000){
DBG("Bad value of MountReqInterval");
retr = MCC_E_FATAL;
}
if(ret != MCC_E_OK) quit();
return ret;
}

99
LibSidServo/serial.c
View File

@ -44,7 +44,8 @@ static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
datamutex = PTHREAD_MUTEX_INITIALIZER;
// encoders thread and mount thread
static pthread_t encthread, mntthread;
// max timeout for 1.5 bytes of encoder and 2 bytes of mount
static struct timeval encRtmout = {0}, mntRtmout = {0};
// encoders raw data
typedef struct __attribute__((packed)){
uint8_t magick;
@ -119,16 +120,15 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
}
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
static int getbyte(){
static int getencbyte(){
if(encfd < 0) return -1;
uint8_t byte;
fd_set rfds;
// default timeot = 100us, 1.5 bytes
struct timeval tv, tvdeflt = {.tv_sec = 0, .tv_usec = 100};
struct timeval tv;
do{
FD_ZERO(&rfds);
FD_SET(encfd, &rfds);
tv = tvdeflt;
tv = encRtmout;
int retval = select(encfd + 1, &rfds, NULL, NULL, &tv);
if(!retval) break;
if(retval < 0){
@ -139,7 +139,31 @@ static int getbyte(){
ssize_t l = read(encfd, &byte, 1);
if(l != 1) return -2; // disconnected ??
break;
} else return -1;
}while(1);
return (int)byte;
}
// read 1 byte from mount; return -1 if nothing to read, -2 if disconnected
static int getmntbyte(){
if(mntfd < 0) return -1;
uint8_t byte;
fd_set rfds;
struct timeval tv;
do{
FD_ZERO(&rfds);
FD_SET(mntfd, &rfds);
tv = mntRtmout;
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
if(!retval) break;
if(retval < 0){
if(errno == EINTR) continue;
return -1;
}
if(FD_ISSET(mntfd, &rfds)){
ssize_t l = read(mntfd, &byte, 1);
if(l != 1) return -2; // disconnected ??
break;
} else return -1;
}while(1);
return (int)byte;
}
@ -150,7 +174,7 @@ static void *encoderthread(void _U_ *u){
int wridx = 0, errctr = 0;
struct timeval tv;
while(encfd > -1 && errctr < MAX_ERR_CTR){
int b = getbyte();
int b = getencbyte();
if(b == -2) ++errctr;
if(b < 0) continue;
errctr = 0;
@ -178,15 +202,32 @@ static void *encoderthread(void _U_ *u){
// main mount thread
static void *mountthread(void _U_ *u){
int errctr = 0;
SSstat status;
// data to get
data_t d = {.buf = (uint8_t*)&status, .maxlen = sizeof(SSstat)};
// cmd to send
const data_t cmd = {.buf = CMD_GETSTAT, .len = sizeof(CMD_GETSTAT)-1, .maxlen = sizeof(CMD_GETSTAT)-1};
while(mntfd > -1 && errctr < MAX_ERR_CTR){
;
pthread_mutex_lock(&mntmutex);
;
// read data to status
if(MountWriteRead(&cmd, &d) || d.len != sizeof(SSstat)){
DBG("Can't read SSstat");
++errctr; continue;
}
if(SScalcChecksum((uint8_t*)status, sizeof(SSstat)-2) != status.checksum){
DBG("BAD checksum of SSstat");
++errctr; continue;
}
errctr = 0;
pthread_mutex_lock(&datamutex);
// now change data
SSconvstat(&status, &mountdata);
if(!Conf.SepEncoder){ // fill encoder data from here, as there's no separate enc thread
;
}
;
pthread_mutex_unlock(&mntmutex);
pthread_mutex_unlock(&datamutex);
// allow writing & getters
usleep(Conf.MountReqInterval);
}
if(mntfd > -1){
close(mntfd);
@ -196,7 +237,7 @@ static void *mountthread(void _U_ *u){
}
// open device and return its FD or -1
static int ttyopen(const char *path, int speed){
static int ttyopen(const char *path, speed_t speed){
int fd = -1;
struct termios2 tty;
DBG("Try to open %s @ %d", path, speed);
@ -222,8 +263,10 @@ static int ttyopen(const char *path, int speed){
int openEncoder(const char *path, int speed){
if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
if(encfd > -1) close(encfd);
encfd = ttyopen(path, speed);
encfd = ttyopen(path, (speed_t) speed);
if(encfd < 0) return FALSE;
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 15000000 / speed; // 1.5 bytes
if(pthread_create(&encthread, NULL, encoderthread, NULL)){
close(encfd);
encfd = -1;
@ -236,8 +279,10 @@ int openEncoder(const char *path, int speed){
// return FALSE if failed
int openMount(const char *path, int speed){
if(mntfd > -1) close(mntfd);
mntfd = ttyopen(path, speed);
mntfd = ttyopen(path, (speed_t) speed);
if(mntfd < 0) return FALSE;
mntRtmout.tv_sec = 0;
mntRtmout.tv_usec = 20000000 / speed; // 2 bytes
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
close(mntfd);
mntfd = -1;
@ -273,3 +318,31 @@ mcc_errcodes_t getMD(mountdata_t *d){
pthread_mutex_unlock(&datamutex);
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){
if(!out && !in) return FALSE;
int ret = FALSE;
pthread_mutex_lock(&mntmutex);
if(out){
if(out->len != write(mntfd, out->buf, out->len)) goto ext;
write(mntfd, "\r", 1); // add EOL
}
if(in){
in->len = 0;
for(size_t i = 0; i < in->maxlen; ++i){
int b = getmntbyte();
if(b < 0) break; // nothing to read -> go out
in->buf[in->len++] = (uint8_t) b;
}
}
ret = TRUE;
ext:
pthread_mutex_unlock(&mntmutex);
return ret;
}

1
LibSidServo/serial.h
View File

@ -34,3 +34,4 @@ int openEncoder(const char *path, int speed);
int openMount(const char *path, int speed);
void closeSerial();
mcc_errcodes_t getMD(mountdata_t *d);
int MountWriteRead(const data_t *out, data_t *in);

40
LibSidServo/sidservo.h
View File

@ -18,6 +18,8 @@
#pragma once
#include <stddef.h>
#include <stdint.h>
#include <sys/time.h>
// error codes
@ -30,11 +32,13 @@ typedef enum{
} mcc_errcodes_t;
typedef struct{
char* MountPath; // path to mount device
int MountSpeed; // serial speed
char* EncoderPath; // path to encoder device
int EncoderSpeed; // serial speed
int SepEncoder; // ==1 if encoder works as separate serial device
char* MountDevPath; // path to mount device
int MountDevSpeed; // serial speed
char* EncoderDevPath; // path to encoder device
int EncoderDevSpeed; // serial speed
int SepEncoder; // ==1 if encoder works as separate serial device
suseconds_t MountReqInterval;// interval between subsequent mount requests (microseconds)
;
} conf_t;
// coordinates in degrees: X, Y and time when they were reached
@ -42,9 +46,31 @@ typedef struct{
double X; double Y; struct timeval msrtime;
} coords_t;
// data to read/write
typedef struct{
coords_t position;
coords_t speed;
uint8_t *buf; // data buffer
size_t len; // its length
size_t maxlen; // maximal buffer size
} data_t;
typedef struct{
uint8_t XBits;
uint8_t YBits;
uint8_t ExtraBits;
uint16_t ain0;
uint16_t ain1;
} extradata_t;
typedef struct{
coords_t motposition;
coords_t encposition;
coords_t lastmotposition;
coords_t motspeed;
uint8_t keypad;
extradata_t extradata;
uint32_t millis;
double temperature;
double voltage;
} mountdata_t;
// mount class

70
LibSidServo/ssii.c Normal file
View File

@ -0,0 +1,70 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "serial.h"
#include "ssii.h"
uint16_t SScalcChecksum(uint8_t *buf, int len){
uint16_t checksum = 0;
for(int i = 0; i < len; i++)
checksum += *buf++;
checksum ^= 0xFF00; // invert high byte
//DBG("Checksum of %d bytes: 0x%04x", len, checksum);
return checksum;
}
// send short/long binary command
static int bincmd(uint8_t *cmd, int len){
data_t d;
if(len == sizeof(SSscmd)){
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Short command");
d.buf = CMD_SHORTCMD;
d.len = sizeof(CMD_SHORTCMD) - 1;
if(!MountWriteRead(&d, NULL)) return -1;
}else if(len == sizeof(SSlcmd)){
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Long command");
d.buf = CMD_LONGCMD;
d.len = sizeof(CMD_LONGCMD) - 1;
if(!MountWriteRead(&d, NULL)) return -1;
}else{
return -1;
}
DBG("Write %d bytes and wait for ans", len);
d.buf = cmd;
d.len = d.maxlen = len;
return MountWriteRead(&d, &d);
}
// return TRUE if OK
int SScmdS(SSscmd *cmd){
return bincmd(cmd, sizeof(SSscmd));
}
int SScmdL(SSlcmd *cmd){
return bincmd(cmd, sizeof(SSlcmd));
}
/**
* @brief SSconvstat - convert stat from SSII format to human
* @param status (i) - just read data
* @param mountdata (o) - output
*/
void SSconvstat(const SSstat *status, mountdata_t *mountdata){
;
}

6
LibSidServo/ssii.h
View File

@ -20,6 +20,8 @@
#include <stdint.h>
#include "sidservo.h"
// ASCII commands
#define U8P(x) ((uint8_t*)x)
// get binary data of all statistics
@ -105,3 +107,7 @@ typedef struct{
uint16_t checksum; // 32
} __attribute__((packed)) SSlcmd; // long command
int SScmdS(SSscmd *cmd);
int SScmdL(SSlcmd *cmd);
uint16_t SScalcChecksum(uint8_t *buf, int len);
void SSconvstat(const SSstat *status, mountdata_t *mountdata);