timur/LibSidServo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

kalman added

Browse Source
This commit is contained in:
Edward V. Emelianov
2026-03-23 12:10:20 +03:00
parent 8698dfa016
commit 9e7c732afd
7 changed files with 270 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
ssii.c
View File

@@ -45,27 +45,35 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
}
// Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!!
static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t stat){
if(*prev == INT32_MAX){
*stat = AXIS_STOPPED;
stat = AXIS_STOPPED;
DBG("START");
}else if(*stat != AXIS_STOPPED){
if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
*stat = AXIS_STOPPED;
DBG("AXIS stopped");
}
}else if(stat == AXIS_GONNASTOP){ // check stop
if(*prev == cur){
if(++(*nstopped) > MOTOR_STOPPED_CNT){
stat = AXIS_STOPPED;
DBG("AXIS stopped");
}
}else *nstopped = 0;
}else if(*prev != cur){
DBG("AXIS moving");
*nstopped = 0;
}
*prev = cur;
return stat;
}
// check for stopped/pointing states
static void ChkStopped(const SSstat *s, mountdata_t *m){
static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
axis_status_t Xstat, Ystat;
Xstat = chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, m->Xstate);
Ystat = chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, m->Ystate);
if(Xstat != m->Xstate || Ystat != m->Ystate){
DBG("Status changed");
setStat(Xstat, Ystat);
}
}
/**
@@ -170,14 +178,17 @@ int SSsetterI(const char *cmd, int32_t ival){
}
int SSstop(int emerg){
FNAME();
int i = 0;
const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
setStat(AXIS_GONNASTOP, AXIS_GONNASTOP);
for(; i < 10; ++i){
if(!SStextcmd(cmdx, NULL)) continue;
if(SStextcmd(cmdy, NULL)) break;
}
if(i == 10) return FALSE;
DBG("Stopped");
return TRUE;
}