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

some more

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-01-30 22:14:10 +03:00
parent 5441a87fff
commit 31c885ba02
8 changed files with 224 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
LibSidServo/examples/dumpmoving.c
View File

@ -74,7 +74,8 @@ int main(int argc, char **argv){
signal(SIGINT, signals); // ctrl+C - quit
signal(SIGQUIT, signals); // ctrl+\ - quit
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
;;;
double t0 = sl_dtime();
while(sl_dtime() - t0 < 10.);
Mount.quit();
return 0;
}

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-27T22:02:50. -->
<!-- Written by QtCreator 15.0.0, 2025-01-30T22:13:51. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

1
LibSidServo/libsidservo.files
View File

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

58
LibSidServo/main.c
View File

@ -24,33 +24,6 @@
conf_t Conf = {0};
/**
* @brief init - open serial devices and do other job
* @param c - initial configuration
* @return error code
*/
static mcc_errcodes_t init(conf_t *c){
if(!c) return MCC_E_BADFORMAT;
Conf = *c;
if(!Conf.EncoderPath || Conf.EncoderSpeed < 1200){
DBG("Define encoder device path and speed");
return MCC_E_BADFORMAT;
}
if(!Conf.MountPath || Conf.MountSpeed < 1200){
DBG("Define mount device path and speed");
return MCC_E_BADFORMAT;
}
if(!openEncoder(Conf.EncoderPath, Conf.EncoderSpeed)){
DBG("Can't open %s with speed %d", Conf.EncoderPath, Conf.EncoderSpeed);
return MCC_E_ENCODERDEV;
}
if(!openMount(Conf.MountPath, Conf.MountSpeed)){
DBG("Can't open %s with speed %d", Conf.MountPath, Conf.MountSpeed);
return MCC_E_MOUNTDEV;
}
return MCC_E_OK;
}
/**
* @brief quit - close all opened and return to default state
*/
@ -60,9 +33,38 @@ static void quit(){
DBG("Exit");
}
/**
* @brief init - open serial devices and do other job
* @param c - initial configuration
* @return error code
*/
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){
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);
ret = MCC_E_MOUNTDEV;
}
if(Conf.SepEncoder){
if(!Conf.EncoderPath || Conf.EncoderSpeed < 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);
ret = MCC_E_ENCODERDEV;
}
}
if(ret != MCC_E_OK) quit();
return ret;
}
// init mount class
mount_t Mount = {
.init = init,
.quit = quit,
.getEnc = getEnc
.getMountData = getMD
};

101
LibSidServo/serial.c
View File

@ -19,32 +19,31 @@
#include <asm-generic/termbits.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>
#include <pthread.h>
#include <stdatomic.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <time.h>
#include <unistd.h>
#include "dbg.h"
#include "serial.h"
#include "ssii.h"
// serial devices FD
static int encfd = -1, mntfd = -1;
// time of last EncData started
static _Atomic double tgot = 0.;
// last Enc values
static _Atomic int32_t encX = 0, encY = 0;
// main mount data
static mountdata_t mountdata = {0};
// mutex for RW operations with mount device
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER;
// encoders thread
static pthread_t encthread;
// mutexes for RW operations with mount device and data
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
datamutex = PTHREAD_MUTEX_INITIALIZER;
// encoders thread and mount thread
static pthread_t encthread, mntthread;
// encoders raw data
typedef struct __attribute__((packed)){
@ -54,6 +53,10 @@ typedef struct __attribute__((packed)){
uint8_t CRC[4];
} enc_t;
#define Xpos2rad(pos) (2. * (double)pos / ENC_TURN_XTICKS * M_PI)
#define Ypos2rad(pos) (2. * (double)pos / ENC_TURN_YTICKS * M_PI)
#if 0
/**
* @brief dtime - UNIX time with microsecond
* @return value
@ -66,12 +69,21 @@ static double dtime(){
return t;
}
// init start time
static void gttime(){
struct timeval tv;
gettimeofday(&tv, NULL);
tv_sec_got = tv.tv_sec;
tv_usec_got = tv.tv_usec;
}
#endif
/**
* @brief parse_encbuf - check encoder buffer and fill fresh data
* @brief parse_encbuf - check encoder buffer (for separate encoder) and fill fresh data
* @param databuf - input buffer with 13 bytes of data
* @param nexttime - time when databuf[0] got
*/
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double nexttime){
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
enc_t *edata = (enc_t*) databuf;
if(edata->magick != ENC_MAGICK){
DBG("No magick");
@ -98,10 +110,12 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double nexttime){
DBG("CRC[2] = 0x%02x, need 0x%02x", edata->CRC[2], y);
return;
}
encX = edata->encX;
encY = edata->encY;
tgot = nexttime;
DBG("time = %g, X=%d, Y=%d", tgot, encX, encY);
pthread_mutex_lock(&datamutex);
mountdata.position.X = Xpos2rad(edata->encX);
mountdata.position.Y = Ypos2rad(edata->encY);
mountdata.position.msrtime = *tv;
pthread_mutex_unlock(&datamutex);
DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.position.X*180./M_PI, mountdata.position.Y*180./M_PI);
}
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
@ -130,11 +144,11 @@ static int getbyte(){
return (int)byte;
}
// main encoder thread: read next data and make parsing
// main encoder thread (for separate encoder): read next data and make parsing
static void *encoderthread(void _U_ *u){
uint8_t databuf[ENC_DATALEN];
int wridx = 0, errctr = 0;
double starttime = 0.;
struct timeval tv;
while(encfd > -1 && errctr < MAX_ERR_CTR){
int b = getbyte();
if(b == -2) ++errctr;
@ -145,12 +159,12 @@ static void *encoderthread(void _U_ *u){
if((uint8_t)b == ENC_MAGICK){
DBG("Got magic -> start filling packet");
databuf[wridx++] = (uint8_t) b;
starttime = dtime();
gettimeofday(&tv, NULL);
}
continue;
}else databuf[wridx++] = (uint8_t) b;
if(wridx == ENC_DATALEN){
parse_encbuf(databuf, starttime);
parse_encbuf(databuf, &tv);
wridx = 0;
}
}
@ -161,6 +175,26 @@ static void *encoderthread(void _U_ *u){
return NULL;
}
// main mount thread
static void *mountthread(void _U_ *u){
int errctr = 0;
while(mntfd > -1 && errctr < MAX_ERR_CTR){
;
pthread_mutex_lock(&mntmutex);
;
if(!Conf.SepEncoder){ // fill encoder data from here, as there's no separate enc thread
;
}
;
pthread_mutex_unlock(&mntmutex);
}
if(mntfd > -1){
close(mntfd);
mntfd = -1;
}
return NULL;
}
// open device and return its FD or -1
static int ttyopen(const char *path, int speed){
int fd = -1;
@ -186,11 +220,16 @@ static int ttyopen(const char *path, int speed){
// return FALSE if failed
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);
if(encfd < 0) return FALSE;
if(pthread_create(&encthread, NULL, encoderthread, NULL)) return FALSE;
DBG("Encoder opened");
if(pthread_create(&encthread, NULL, encoderthread, NULL)){
close(encfd);
encfd = -1;
return FALSE;
}
DBG("Encoder opened, thread started");
return TRUE;
}
@ -199,7 +238,12 @@ int openMount(const char *path, int speed){
if(mntfd > -1) close(mntfd);
mntfd = ttyopen(path, speed);
if(mntfd < 0) return FALSE;
DBG("Mount opened");
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
close(mntfd);
mntfd = -1;
return FALSE;
}
DBG("Mount opened, thread started");
return TRUE;
}
@ -222,11 +266,10 @@ void closeSerial(){
}
// get fresh encoder information
mcc_errcodes_t getEnc(coords_t *c){
if(!c) return MCC_E_BADFORMAT;
if(encfd < 0) return MCC_E_ENCODERDEV;
c->msrtime = tgot;
c->X = (double)encX / ENC_TURN_XTICKS * 360.;
c->Y = (double)encY / ENC_TURN_YTICKS * 360.;
mcc_errcodes_t getMD(mountdata_t *d){
if(!d) return MCC_E_BADFORMAT;
pthread_mutex_lock(&datamutex);
*d = mountdata;
pthread_mutex_unlock(&datamutex);
return MCC_E_OK;
}

2
LibSidServo/serial.h
View File

@ -33,4 +33,4 @@
int openEncoder(const char *path, int speed);
int openMount(const char *path, int speed);
void closeSerial();
mcc_errcodes_t getEnc(coords_t *c);
mcc_errcodes_t getMD(mountdata_t *d);

12
LibSidServo/sidservo.h
View File

@ -18,6 +18,8 @@
#pragma once
#include <sys/time.h>
// error codes
typedef enum{
MCC_E_OK = 0, // all OK
@ -32,18 +34,24 @@ typedef struct{
int MountSpeed; // serial speed
char* EncoderPath; // path to encoder device
int EncoderSpeed; // serial speed
int SepEncoder; // ==1 if encoder works as separate serial device
} conf_t;
// coordinates in degrees: X, Y and time when they were reached
typedef struct{
double X; double Y; double msrtime;
double X; double Y; struct timeval msrtime;
} coords_t;
typedef struct{
coords_t position;
coords_t speed;
} mountdata_t;
// mount class
typedef struct{
mcc_errcodes_t (*init)(conf_t *c);
void (*quit)();
mcc_errcodes_t (*getEnc)(coords_t *c);
mcc_errcodes_t (*getMountData)(mountdata_t *d);
} mount_t;
extern mount_t Mount;

107
LibSidServo/ssii.h Normal file
View File

@ -0,0 +1,107 @@
/*
* This file is part of the SSII project.
* Copyright 2022 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/>.
*/
#pragma once
#include <stdint.h>
// ASCII commands
#define U8P(x) ((uint8_t*)x)
// get binary data of all statistics
#define CMD_GETSTAT U8P("XXS")
// send short command
#define CMD_SHORTCMD U8P("XXR")
// send long command
#define CMD_LONGCMD U8P("YXR")
// get/set X/Y in motsteps
#define CMD_MOTX U8P("X")
#define CMD_MOTY U8P("Y")
// -//- in encoders' ticks
#define CMD_ENCX U8P("XZ")
#define CMD_ENCY U8P("YZ")
// normal stop X/Y
#define CMD_STOPX U8P("XN")
#define CMD_STOPY U8P("YN")
// emergency stop
#define CMD_EMSTOPX U8P("XG")
#define CMD_EMSTOPY U8P("YG")
// getters of motor's encoders per rev
#define CMD_GETXMEPR U8P("XXU")
#define CMD_GETYMEPR U8P("XXV")
// -//- axis encoders
#define CMD_GETXAEPR U8P("XXT")
#define CMD_GETYAEPR U8P("XXZ")
// exit ASCII checksum mode
#define CMD_EXITACM U8P("YXY0\r\xb8")
// timeout (seconds) of reading answer (from last symbol read)
#define READTIMEOUT (0.05)
// steps per revolution; TODO: move to Conf
#define HA_MOT_STEPSPERREV (4394294)
#define DEC_MOT_STEPSPERREV (3325291)
// encoder's tolerance (ticks); TODO: move to Conf
#define HAENCTOL (25)
#define DECENCTOL (25)
// all need data in one
typedef struct{ // 41 bytes
uint8_t ctrlAddr; // 0 a8 + controller address
int32_t DECmot; // 1 Dec/HA motor position
int32_t HAmot; // 5
int32_t DECenc; // 9 Dec/HA encoder position
int32_t HAenc; // 13
uint8_t keypad; // 17 keypad status
uint8_t XBits; // 18
uint8_t YBits; // 19
uint8_t ExtraBits; // 20
uint16_t ain0; // 21 analog inputs
uint16_t ain1; // 23
uint32_t millis; // 25 milliseconds clock
int8_t tF; // 29 temperature (degF)
uint8_t voltage; // 30 input voltage *10 (RA worm phase?)
uint32_t DecLast; // 31 Alt/Dec motor location at last Alt/Dec scope encoder location change
uint32_t HALast; // 35 Az/RA motor location at last Az/RA scope encoder location change
uint16_t checksum; // 39 checksum, H inverted
}__attribute__((packed)) SSstat;
typedef struct{
int32_t DECmot; // 0 DEC motor position
int32_t DECspeed; // 4 DEC speed
int32_t HAmot; // 8
int32_t HAspeed; // 12
uint8_t xychange; // 16 change Xbits/Ybits value
uint8_t XBits; // 17
uint8_t YBits; // 18
uint16_t checksum; // 19
} __attribute__((packed)) SSscmd; // short command
typedef struct{
int32_t DECmot; // 0 DEC motor position
int32_t DECspeed; // 4 DEC speed
int32_t HAmot; // 8
int32_t HAspeed; // 12
int32_t DECadder; // 16 - DEC adder
int32_t HAadder; // 20
int32_t DECatime; // 24 DEC adder time
int32_t HAatime; // 28
uint16_t checksum; // 32
} __attribute__((packed)) SSlcmd; // long command