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

remove dependence of libusefull_macros

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-01-28 20:44:12 +03:00
parent 19f61697d6
commit 5441a87fff
11 changed files with 453 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
LibSidServo/conf.c
View File

@ -1,53 +0,0 @@
/*
* 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 <stdio.h>
#include <usefull_macros.h>
#include "sidservo.h"
conf_t Conf = {0};
static sl_option_t cmdlnopts[] = {
{"mountpath", NEED_ARG, NULL, 0, arg_string, APTR(&Conf.MountPath), "path to mount device"},
{"mountspeed", NEED_ARG, NULL, 0, arg_int, APTR(&Conf.MountSpeed), "serial speed of mount device"},
{"encoderpath", NEED_ARG, NULL, 0, arg_string, APTR(&Conf.EncoderPath), "path to encoder device"},
{"encoderspeed",NEED_ARG, NULL, 0, arg_int, APTR(&Conf.EncoderSpeed), "serial speed of encoder device"},
{"verbose", NEED_ARG, NULL, 0, arg_int, APTR(&Conf.verbose), "verbose level"},
};
int readconf(const char *path){
int o = sl_conf_readopts(path, cmdlnopts);
if(o > 0){
DBG("Got %d parameters in %s", o, path);
char *buf = sl_print_opts(cmdlnopts, TRUE);
DBG("%s\n", buf);
FREE(buf);
}
return o;
}
void dumpconf(){
char *buf = sl_print_opts(cmdlnopts, TRUE);
printf("%s\n", buf);
FREE(buf);
}
void help(){
sl_showhelp(-1, cmdlnopts);
}

23
LibSidServo/conf.h
View File

@ -1,23 +0,0 @@
/*
* 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/>.
*/
#pragma once
int readconf(const char *path);
void dumpconf();
void help();

64
LibSidServo/dbg.h Normal file
View File

@ -0,0 +1,64 @@
/*
* 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/>.
*/
#pragma once
#include <stdlib.h>
#include "sidservo.h"
extern conf_t Conf;
// unused arguments of functions
#define _U_ __attribute__((__unused__))
// break absent in `case`
#define FALLTHRU __attribute__ ((fallthrough))
// and synonym for FALLTHRU
#define NOBREAKHERE __attribute__ ((fallthrough))
// weak functions
#define WEAK __attribute__ ((weak))
#ifndef DBL_EPSILON
#define DBL_EPSILON (2.2204460492503131e-16)
#endif
#ifndef FALSE
#define FALSE (0)
#endif
#ifndef TRUE
#define TRUE (1)
#endif
#ifdef EBUG
#include <stdio.h>
#define COLOR_RED "\033[1;31;40m"
#define COLOR_GREEN "\033[1;32;40m"
#define COLOR_OLD "\033[0;0;0m"
#define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
#define DBG(...) do{ fprintf(stderr, COLOR_RED "\n%s " COLOR_OLD, __func__); \
fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n");} while(0)
#else // EBUG
#define FNAME() do{}while(0)
#define DBG(...) do{}while(0)
#endif // EBUG

20
LibSidServo/examples/dumpmoving.c
View File

@ -25,10 +25,8 @@
typedef struct{ typedef struct{
int help; int help;
int confhelp;
int verbose; int verbose;
char *logfile; char *logfile;
char *conffile;
char *coordsoutput; char *coordsoutput;
} parameters; } parameters;
@ -36,10 +34,8 @@ static parameters G = {0};
static sl_option_t cmdlnopts[] = { static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"confhelp", NO_ARGS, NULL, 0, arg_int, APTR(&G.confhelp), "show configuration file help"},
{"verbose", NO_ARGS, NULL, 'v', arg_none, APTR(&G.verbose), "verbose level (each -v adds 1)"}, {"verbose", NO_ARGS, NULL, 'v', arg_none, APTR(&G.verbose), "verbose level (each -v adds 1)"},
{"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), "log file name"}, {"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), "log file name"},
{"conffile", NEED_ARG, NULL, 'c', arg_string, APTR(&G.conffile), "configuration file name"},
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"}, {"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
end_option end_option
}; };
@ -54,25 +50,25 @@ void signals(int sig){
exit(sig); exit(sig);
} }
static conf_t Config = {
.MountPath = "/dev/ttyS1",
.MountSpeed = 19200,
.EncoderPath = "/dev/ttyUSB0",
.EncoderSpeed = 153000
};
int main(int argc, char **argv){ int main(int argc, char **argv){
sl_init(); sl_init();
sl_parseargs(&argc, &argv, cmdlnopts); sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help) sl_showhelp(-1, cmdlnopts); if(G.help) sl_showhelp(-1, cmdlnopts);
if(G.confhelp) Mount.helpandquit();
if(!G.conffile) ERRX("Point configuration file name");
sl_loglevel_e lvl = G.verbose + LOGLEVEL_ERR; sl_loglevel_e lvl = G.verbose + LOGLEVEL_ERR;
if(lvl >= LOGLEVEL_AMOUNT) lvl = LOGLEVEL_AMOUNT - 1; if(lvl >= LOGLEVEL_AMOUNT) lvl = LOGLEVEL_AMOUNT - 1;
if(G.logfile) OPENLOG(G.logfile, lvl, 1); if(G.logfile) OPENLOG(G.logfile, lvl, 1);
time_t curtime = time(NULL); time_t curtime = time(NULL);
LOGMSG("Started @ %s", ctime(&curtime)); LOGMSG("Started @ %s", ctime(&curtime));
if(Mount.readconf(G.conffile) < 0) ERRX("Can't read configuration file %s", G.conffile);
green("Got config:\n");
Mount.dumpconf();
if(!Mount.init()) ERRX("Can't init");
DBG("Devices ready"); DBG("Devices ready");
LOGMSG("Mount device %s @ %d", Conf.MountPath, Conf.MountSpeed); LOGMSG("Mount device %s @ %d", Config.MountPath, Config.MountSpeed);
LOGMSG("Encoder device %s @ %d", Conf.EncoderPath, Conf.EncoderSpeed); LOGMSG("Encoder device %s @ %d", Config.EncoderPath, Config.EncoderSpeed);
signal(SIGTERM, signals); // kill (-15) - quit signal(SIGTERM, signals); // kill (-15) - quit
signal(SIGHUP, SIG_IGN); // hup - ignore signal(SIGHUP, SIG_IGN); // hup - ignore
signal(SIGINT, signals); // ctrl+C - quit signal(SIGINT, signals); // ctrl+C - quit

184
LibSidServo/libsidservo.creator.user.cf63021 Normal file
View File

@ -0,0 +1,184 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 15.0.0, 2025-01-27T14:56:12. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
</data>
<data>
<variable>ProjectExplorer.Project.ActiveTarget</variable>
<value type="qlonglong">0</value>
</data>
<data>
<variable>ProjectExplorer.Project.EditorSettings</variable>
<valuemap type="QVariantMap">
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
<value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
<value type="QString" key="language">Cpp</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
</valuemap>
</valuemap>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
<value type="QString" key="language">QmlJS</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
</valuemap>
</valuemap>
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
<value type="int" key="EditorConfiguration.IndentSize">4</value>
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
<value type="int" key="EditorConfiguration.TabSize">8</value>
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.PluginSettings</variable>
<valuemap type="QVariantMap">
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
<value type="bool" key="AutoTest.Framework.Boost">true</value>
<value type="bool" key="AutoTest.Framework.CTest">false</value>
<value type="bool" key="AutoTest.Framework.Catch">true</value>
<value type="bool" key="AutoTest.Framework.GTest">true</value>
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
</valuemap>
<value type="bool" key="AutoTest.ApplyFilter">false</value>
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
<value type="int" key="AutoTest.RunAfterBuild">0</value>
<value type="bool" key="AutoTest.UseGlobal">true</value>
<valuemap type="QVariantMap" key="ClangTools">
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
<value type="int" key="ClangTools.ParallelJobs">4</value>
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
</valuemap>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.Target.0</variable>
<valuemap type="QVariantMap">
<value type="QString" key="DeviceType">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
<value type="QString">all</value>
</valuelist>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
<value type="QString">clean</value>
</valuelist>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.TargetCount</variable>
<value type="qlonglong">1</value>
</data>
<data>
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
<value type="int">22</value>
</data>
<data>
<variable>Version</variable>
<value type="int">22</value>
</data>
</qtcreator>

3
LibSidServo/libsidservo.files
View File

@ -1,6 +1,5 @@
CMakeLists.txt CMakeLists.txt
conf.c dbg.h
conf.h
examples/dumpmoving.c examples/dumpmoving.c
main.c main.c
sidservo.h sidservo.h

2
LibSidServo/libsidservo.includes
View File

@ -0,0 +1,2 @@
.
..

47
LibSidServo/main.c
View File

@ -18,44 +18,51 @@
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <usefull_macros.h>
#include "conf.h" #include "dbg.h"
#include "serial.h" #include "serial.h"
#include "sidservo.h"
static int init(){ conf_t Conf = {0};
if(!Conf.EncoderPath || !Conf.EncoderSpeed){
WARNX("Define encoder device path and speed"); /**
return FALSE; * @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){ if(!Conf.MountPath || Conf.MountSpeed < 1200){
WARNX("Define mount device path and speed"); DBG("Define mount device path and speed");
return FALSE; return MCC_E_BADFORMAT;
} }
if(!openEncoder(Conf.EncoderPath, Conf.EncoderSpeed)){ if(!openEncoder(Conf.EncoderPath, Conf.EncoderSpeed)){
WARNX("Can't open %s with speed %d", Conf.EncoderPath, Conf.EncoderSpeed); DBG("Can't open %s with speed %d", Conf.EncoderPath, Conf.EncoderSpeed);
return FALSE; return MCC_E_ENCODERDEV;
} }
if(!openMount(Conf.MountPath, Conf.MountSpeed)){ if(!openMount(Conf.MountPath, Conf.MountSpeed)){
WARNX("Can't open %s with speed %d", Conf.MountPath, Conf.MountSpeed); DBG("Can't open %s with speed %d", Conf.MountPath, Conf.MountSpeed);
return FALSE; return MCC_E_MOUNTDEV;
} }
// init RNG return MCC_E_OK;
srand48(sl_random_seed());
return TRUE;
} }
/**
* @brief quit - close all opened and return to default state
*/
static void quit(){ static void quit(){
DBG("Close serial devices"); DBG("Close serial devices");
closeSerial(); closeSerial();
DBG("Exit"); DBG("Exit");
} }
// init mount class
mount_t Mount = { mount_t Mount = {
.readconf = readconf,
.dumpconf = dumpconf,
.helpandquit = help,
.init = init, .init = init,
.quit = quit, .quit = quit,
.getEnc = getEnc
}; };

170
LibSidServo/serial.c
View File

@ -16,35 +16,62 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>. * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <asm-generic/termbits.h>
#include <errno.h>
#include <fcntl.h>
#include <pthread.h> #include <pthread.h>
#include <stdatomic.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h>
#include <string.h> #include <string.h>
#include <usefull_macros.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 "serial.h"
#include "sidservo.h"
// serial devices // serial devices FD
static sl_tty_t *EncDev = NULL, *MntDev = NULL; static int encfd = -1, mntfd = -1;
// time of last EncData started // time of last EncData started
static double tgot = 0.; static _Atomic double tgot = 0.;
// last Enc values // last Enc values
static uint32_t encX = 0, encY = 0; static _Atomic int32_t encX = 0, encY = 0;
// mutex for RW operations with mount device
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER;
// encoders thread
static pthread_t encthread; static pthread_t encthread;
// encoders raw data
typedef struct __attribute__((packed)){ typedef struct __attribute__((packed)){
uint8_t magick; uint8_t magick;
uint32_t encX; int32_t encX;
uint32_t encY; int32_t encY;
uint8_t CRC[4]; uint8_t CRC[4];
} enc_t; } enc_t;
/**
* @brief dtime - UNIX time with microsecond
* @return value
*/
static double dtime(){
double t;
struct timeval tv;
gettimeofday(&tv, NULL);
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
return t;
}
/**
static void parce_encbuf(uint8_t databuf[ENC_DATALEN], double nexttime){ * @brief parse_encbuf - check encoder buffer 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){
enc_t *edata = (enc_t*) databuf; enc_t *edata = (enc_t*) databuf;
if(edata->magick != ENC_MAGICK){ if(edata->magick != ENC_MAGICK){
DBG("No magick"); DBG("No magick");
@ -74,67 +101,132 @@ static void parce_encbuf(uint8_t databuf[ENC_DATALEN], double nexttime){
encX = edata->encX; encX = edata->encX;
encY = edata->encY; encY = edata->encY;
tgot = nexttime; tgot = nexttime;
DBG("time = %g, X=%d, Y=%d", tgot, encX, encY);
} }
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
static int getbyte(){
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};
do{
FD_ZERO(&rfds);
FD_SET(encfd, &rfds);
tv = tvdeflt;
int retval = select(encfd + 1, &rfds, NULL, NULL, &tv);
if(!retval) break;
if(retval < 0){
if(errno == EINTR) continue;
return -1;
}
if(FD_ISSET(encfd, &rfds)){
ssize_t l = read(encfd, &byte, 1);
if(l != 1) return -2; // disconnected ??
break;
}
}while(1);
return (int)byte;
}
// main encoder thread: read next data and make parsing
static void *encoderthread(void _U_ *u){ static void *encoderthread(void _U_ *u){
uint8_t databuf[ENC_DATALEN]; uint8_t databuf[ENC_DATALEN];
int wridx = 0; int wridx = 0, errctr = 0;
double nexttime = 0.; double starttime = 0.;
void add2buf(){ while(encfd > -1 && errctr < MAX_ERR_CTR){
size_t len = ENC_DATALEN - wridx; int b = getbyte();
if(EncDev->buflen < len) len = EncDev->buflen; if(b == -2) ++errctr;
memcpy(databuf+wridx, EncDev->buf, len); if(b < 0) continue;
wridx += len; errctr = 0;
} DBG("Got byte from Encoder: 0x%02X", b);
while(EncDev){
if(sl_tty_read(EncDev)){
DBG("Got %zd bytes from Encoder", EncDev->buflen);
if(EncDev->buflen <= ENC_DATALEN){
if(wridx == 0){ if(wridx == 0){
if((uint8_t)EncDev->buf[0] == ENC_MAGICK){ if((uint8_t)b == ENC_MAGICK){
add2buf(); DBG("Got magic -> start filling packet");
nexttime = sl_dtime(); databuf[wridx++] = (uint8_t) b;
starttime = dtime();
} }
}else add2buf(); continue;
}else databuf[wridx++] = (uint8_t) b;
if(wridx == ENC_DATALEN){ if(wridx == ENC_DATALEN){
parce_encbuf(databuf, nexttime); parse_encbuf(databuf, starttime);
wridx = 0; wridx = 0;
} }
} }
} if(encfd > -1){
close(encfd);
encfd = -1;
} }
return NULL; return NULL;
} }
// open device and return its FD or -1
static int ttyopen(const char *path, int speed){
int fd = -1;
struct termios2 tty;
DBG("Try to open %s @ %d", path, speed);
if((fd = open(path, O_RDWR|O_NOCTTY)) < 0) return -1;
if(ioctl(fd, TCGETS2, &tty)){ close(fd); return -1; }
tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
tty.c_iflag = 0; // don't do any changes in input stream
tty.c_oflag = 0; // don't do any changes in output stream
tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl
tty.c_ispeed = speed;
tty.c_ospeed = speed;
tty.c_cc[VMIN] = 0; // non-canonical mode
tty.c_cc[VTIME] = 5;
if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; }
DBG("Check speed");
if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
// try to set exclusive
ioctl(fd, TIOCEXCL);
return fd;
}
// return FALSE if failed
int openEncoder(const char *path, int speed){ int openEncoder(const char *path, int speed){
EncDev = sl_tty_new((char*)path, speed, 256); if(encfd > -1) close(encfd);
if(EncDev) EncDev = sl_tty_open(EncDev, 1); encfd = ttyopen(path, speed);
if(!EncDev) return FALSE; if(encfd < 0) return FALSE;
sl_tty_tmout(5.); // 5us timeout
if(pthread_create(&encthread, NULL, encoderthread, NULL)) return FALSE; if(pthread_create(&encthread, NULL, encoderthread, NULL)) return FALSE;
DBG("Encoder opened"); DBG("Encoder opened");
return TRUE; return TRUE;
} }
// return FALSE if failed
int openMount(const char *path, int speed){ int openMount(const char *path, int speed){
MntDev = sl_tty_new((char*)path, speed, 256); if(mntfd > -1) close(mntfd);
if(MntDev) MntDev = sl_tty_open(MntDev, 1); mntfd = ttyopen(path, speed);
if(!MntDev) return FALSE; if(mntfd < 0) return FALSE;
DBG("Mount opened"); DBG("Mount opened");
return TRUE; return TRUE;
} }
// close all opened serial devices and quit threads
void closeSerial(){ void closeSerial(){
if(MntDev){ if(mntfd > -1){
DBG("Close mount"); DBG("Close mount");
pthread_mutex_lock(&mntmutex); pthread_mutex_lock(&mntmutex);
sl_tty_close(&MntDev); close(mntfd);
mntfd = -1;
pthread_mutex_unlock(&mntmutex); pthread_mutex_unlock(&mntmutex);
} }
if(EncDev){ if(encfd > -1){
DBG("Close encoder"); DBG("Close encoder");
pthread_cancel(encthread); pthread_cancel(encthread);
pthread_join(encthread, NULL); pthread_join(encthread, NULL);
sl_tty_close(&EncDev); close(encfd);
encfd = -1;
} }
} }
// 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.;
return MCC_E_OK;
}

8
LibSidServo/serial.h
View File

@ -18,11 +18,19 @@
#pragma once #pragma once
#include "sidservo.h"
// magick starting sequence // magick starting sequence
#define ENC_MAGICK (204) #define ENC_MAGICK (204)
// encoder data sequence length // encoder data sequence length
#define ENC_DATALEN (13) #define ENC_DATALEN (13)
// max error counter (when read() returns -1)
#define MAX_ERR_CTR (100)
// encoder ticks per turn
#define ENC_TURN_XTICKS (111111.)
#define ENC_TURN_YTICKS (111111.)
int openEncoder(const char *path, int speed); int openEncoder(const char *path, int speed);
int openMount(const char *path, int speed); int openMount(const char *path, int speed);
void closeSerial(); void closeSerial();
mcc_errcodes_t getEnc(coords_t *c);

35
LibSidServo/sidservo.h
View File

@ -18,21 +18,32 @@
#pragma once #pragma once
typedef struct{ // error codes
char* MountPath; typedef enum{
int MountSpeed; MCC_E_OK = 0, // all OK
char* EncoderPath; MCC_E_FATAL, // some fatal error
int EncoderSpeed; MCC_E_BADFORMAT, // wrong arguments of function
int verbose; MCC_E_ENCODERDEV, // encoder device error or can't open
} conf_t; MCC_E_MOUNTDEV, // mount device error or can't open
} mcc_errcodes_t;
typedef struct{ typedef struct{
int (*readconf)(const char *path); // read config file char* MountPath; // path to mount device
void (*dumpconf)(); int MountSpeed; // serial speed
void (*helpandquit)(); char* EncoderPath; // path to encoder device
int (*init)(); int EncoderSpeed; // serial speed
} conf_t;
// coordinates in degrees: X, Y and time when they were reached
typedef struct{
double X; double Y; double msrtime;
} coords_t;
// mount class
typedef struct{
mcc_errcodes_t (*init)(conf_t *c);
void (*quit)(); void (*quit)();
mcc_errcodes_t (*getEnc)(coords_t *c);
} mount_t; } mount_t;
extern mount_t Mount; extern mount_t Mount;
extern conf_t Conf;