From d92d5cf7306c12e76d07ab4d0b377d090ff5694e Mon Sep 17 00:00:00 2001 From: Ian Jackson Date: Fri, 31 Dec 2010 13:08:44 +0000 Subject: [PATCH] movpos: wip multiple movfeatkinds: compiles, not tested --- hostside/movpos.c | 278 ++++++++++++++++++++++++-------------------- hostside/realtime.c | 2 +- hostside/safety.h | 4 +- 3 files changed, 152 insertions(+), 132 deletions(-) diff --git a/hostside/movpos.c b/hostside/movpos.c index 7fe7b89..c95b80e 100644 --- a/hostside/movpos.c +++ b/hostside/movpos.c @@ -12,14 +12,34 @@ typedef struct { } Motion; typedef struct Method Method; +typedef struct Change Change; -struct MovPosChange { +typedef struct MovPosChange { Segment *move; /* everything beyond here is private for indep */ MovPosComb actual, target; int n_changes; Change *changes[]; -}; +} Indep; + +static void method_update_feature(Method*, MovPosChange*, const Motion *mo); + /* Called from methods' execution logic when an individual feature + * has been moved. This is used by the method-independent code to + * compute the correct delta set of movements from the current + * actual position, when thinking about new plans. It is also sent + * to clients and ultimately used by the UI. + */ + +static void method_change_done(Method *m, Change *chg); + /* Called from methods' execution logic when a whole change + * has been done. The method-independent code will take care of + * updating move->movposcomb. etc. + * + * REENTRANCY: must NOT be called from within a call to the method's + * execute() (and of course cannot legally be called from within + * prepare, consider, check or dispose). + */ + /* Kind-independent code is responsible for determining * the method, doing a bit of cleanup, and adjusting the flow @@ -48,24 +68,21 @@ struct MovPosChange { * No Changes remain Prepared while the Method is Executing. */ -typedef struct Change Change; - struct Change { /* valid in: filled in by and when: */ Method *meth; /* PIDG indep after prepare() */ MovPosChange *indep; /* PID indep after prepare() */ unsigned installed:1; /* private for indep */ /* kind-specific data follows */ /* varies kind-specific code, varies */ -} Change; +}; struct Method { const char *pname; - unsigned needcheck, needexec; /* used by indep to track T/Y/E */ ErrorCode (*prepare)(Method *m, /* TYE->T */ - Segment *move, + const Segment *move, int n_motions, const Motion *motions, - int ms, int confirming, - Change *chg_r, /* 0->P; err: 0->0; may be 0 */ + int ms, int confirmation, + Change **chg_r, /* 0->P; err: 0->0; may be 0 */ int *cost_r /* may be 0 */); void (*dispose)(Method *m, /* TY->TY */ Change*); /* P->U */ @@ -80,18 +97,20 @@ struct Method { ErrorCode (*check)(Method *m); /* TYE->Y; err: TYE->TYE */ void (*execute)(Method *m); /* EY->E */ void (*all_abandon)(Method *m); /* TYE->S */ + + unsigned needcheck, needexec; /* used by indep to track T/Y/E */ }; /*========== general utility functions ==========*/ -const char *movpos_pname(Segment *move, MovPosComb poscomb) { +const char *movpos_pname(const Segment *move, MovPosComb poscomb) { return !SOMEP(poscomb) ? "?" : move->i->poscombs[poscomb].pname; } -static void ouposn_moving(Change *chg) { - Segment *move= chg->move; +static void ouposn_moving(const MovPosChange *indep) { + Segment *move= indep->move; ouprintf("movpos %s position %s moving\n", - move->i->pname, movpos_pname(move, chg->actual)); + move->i->pname, movpos_pname(move, indep->actual)); } MovPosComb movposcomb_update_feature(MovPosComb startpoint, @@ -103,7 +122,7 @@ MovPosComb movposcomb_update_feature(MovPosComb startpoint, return above*above_weight + featpos*mfi->weight + below; } -MovPosComb movpos_poscomb_actual(Segment *seg) { +MovPosComb movpos_poscomb_actual(const Segment *seg) { return seg->moving ? seg->motion->actual : seg->movposcomb; } @@ -246,8 +265,8 @@ static ErrorCode fsq_check_plan(FsqMethod *m) { future++; DPRINTF2("-"); continue; } - DPRINTF2("%s/%s[%d@t+%d]", whichr->h.move->i->pname, - movpos_pname(whichr->h.move, whichr->h.intent), + DPRINTF2("%s/%s[%d@t+%d]", whichr->h.indep->move->i->pname, + movpos_pname(whichr->h.indep->move, whichr->h.indep->target), whichr->n_motions, whichwhen); if (future > whichwhen) { DPRINTF2("!...bad\n"); @@ -273,14 +292,18 @@ static ErrorCode fsq_queue_insert_item(FsqMethod *m, FsqQueue *q, FsqReq *r) { q->l[insat]= q->l[insat-1]; q->l[insat]= r; q->n++; + + return 0; } -static void fsq_item_debug(FsqMethod *m, FsqReq *r, - const char *opwhat, Segment *move) { +static void fsq_item_debug(FsqMethod *m, FsqReq *r, const char *opwhat) { int DP; - DPRINTF1("%s %s %s", m->m.pname, opwhat, move.pname); + Segment *move= r->h.indep->move; + DPRINTF1(movpos,fsq, "%s %s %s", m->m.pname, opwhat, move->i->pname); if (r->motions[0].i) { - for (int i=0, Motion *mo=r->motions; in_motions; i++, mo++) + int i; + Motion *mo; + for (i=0, mo=r->motions; in_motions; i++, mo++) DPRINTF2("/%s%d", mo->i->pname, (int)mo->posn); DPRINTF2("\n"); } else { @@ -290,27 +313,27 @@ static void fsq_item_debug(FsqMethod *m, FsqReq *r, static ErrorCode fsq_enqueue(FsqMethod *m, FsqReq *r) { /* P->I; err: P->P */ if (!r) return 0; - fsq_item_debug(m,r,"enqueue",r->h.indep.move); - return fsq_queue_insert_item(fsq_item_queue(m,r), r); + fsq_item_debug(m,r,"enqueue"); + return fsq_queue_insert_item(m, fsq_item_queue(m,r), r); } static void fsq_dequeue(FsqMethod *m, FsqReq *r) { /* I->P */ - if (!r) return 0; - fsq_item_debug(m,r,"dequeue",r->h.indep.move); - fsq_remove_item(fsq_item_queue(m,r), r); + if (!r) return; + fsq_item_debug(m,r,"dequeue"); + fsq_queue_remove_item(fsq_item_queue(m,r), r); } /*---------- method entrypoints ----------*/ -static ErrorCode fsq_prepare(Method *mm, Segment *move, +static ErrorCode fsq_prepare(Method *mm, const Segment *move, int n_motions, const Motion *motions, int ms, int confirmation, - Change *chg_r, int *cost_r) { + Change **chg_r, int *cost_r) { FsqMethod *m= (void*)mm; assert(n_motions > 0); - FsqSlotSigned reldeadline= fsq_maxdelay_reldeadline(m, ms, r->n_motions); + FsqSlotSigned reldeadline= fsq_maxdelay_reldeadline(m, ms, n_motions); if (reldeadline <= 0) return EC_MovFeatTooLate; if (chg_r) { @@ -321,7 +344,7 @@ static ErrorCode fsq_prepare(Method *mm, Segment *move, r->n_motions= n_motions; if (confirmation) { r->deadline= reldeadline + m->f.cslot; - memcpy(r->motions, motions, sizeof(*r->motions)*motions); + memcpy(r->motions, motions, sizeof(*r->motions)*n_motions); } else { r->deadline= reldeadline; r->motions[0].i= 0; @@ -329,19 +352,18 @@ static ErrorCode fsq_prepare(Method *mm, Segment *move, *chg_r= &r->h; } if (cost_r) { - *cost_r= n_motions * m->slot_ms; + *cost_r= n_motions * m->f.slot_ms; } return 0; } static void fsq_dispose(Method *mm, Change *chg) { - FsqMethod *m= (void*)mm; FsqReq *r= (FsqReq*)chg; free(r); } -static void fsq_remove(Method *mm, Change *instchg) { +static void fsq_remove(Method *mm, Change *remvchg) { FsqMethod *m= (void*)mm; FsqReq *remv= (FsqReq*)remvchg; @@ -393,7 +415,7 @@ static void fsq_check_action(FsqMethod *m) { if (!r->n_motions) { fsq_queue_remove_index(&m->f.confirmed, 0); method_change_done(&m->m, &r->h); - m->h.dispose(&m->m, &r->h); + m->m.dispose(&m->m, &r->h); ec= fsq_check_plan(m); assert(!ec); fsq_check_action(m); @@ -435,10 +457,10 @@ static void fsq_check_action(FsqMethod *m) { #define CDU_RECHARGE 350 /*ms*/ #define POINT_MOVEMENT 50 /*ms*/ -static ErrorCode point_prepare(Method *mm, Segment *move, +static ErrorCode point_prepare(Method *mm, const Segment *move, int n_motions, const Motion *motions, int ms, int confirmation, - Change *chg_r, int *cost_r) { + Change **chg_r, int *cost_r) { FsqMethod *m= (void*)mm; assert(m->f.ready>=0); return fsq_prepare(mm,move, n_motions,motions, @@ -471,7 +493,7 @@ void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { } static FsqMethod points_method= { - { "point", mfk_point, + { "point", point_prepare, fsq_dispose, fsq_install, fsq_remove, fsq_check, fsq_execute, points_all_abandon }, @@ -564,7 +586,7 @@ void on_pic_waggled(const PicInsnInfo *pii, const PicInsn *pi, int objnum) { } static FsqMethod waggle= { - { "relay", mfk_relay, + { "relay", fsq_prepare, fsq_dispose, fsq_install, fsq_remove, fsq_check, fsq_execute, ignore_all_abandon }, @@ -575,23 +597,25 @@ static FsqMethod waggle= { typedef struct NomoveChange { Change h; - DLIST_NODE inqueue; - int n_motions; + DLIST_NODE(struct NomoveChange) inqueue; + int n_motions; /* 0 for reservations */ Motion motions[]; -}; +} NomoveChange; typedef struct { Method m; unsigned eventqueued:1; - NomoveChange *queuehead; + NomoveChange *queuehead; /* contains confirmations only */ } NomoveMethod; -static ErrorCode nomove_prepare(Method *meth_in, Segment *move, +static ErrorCode nomove_prepare(Method *meth_in, const Segment *move, int n_motions, const Motion *motions, int ms, int confirming, - Change *chg_r, int *cost_r) { + Change **chg_r, int *cost_r) { if (chg_r) { NomoveChange *chg; + assert(n_motions>0); + if (!confirming) n_motions= 0; chg= mmalloc(sizeof(*chg) + sizeof(Motion)*n_motions); chg->n_motions= n_motions; memcpy(chg->motions, motions, sizeof(Motion)*n_motions); @@ -609,30 +633,37 @@ static void nomove_dispose(Method *mm, Change *remvchg) { static ErrorCode nomove_install(Method *mm, Change *instchg) { NomoveMethod *meth= (void*)mm; NomoveChange *inst= (void*)instchg; - DLIST1_PREPEND(meth->queuehead, inst, inqueue); + if (inst->n_motions) + DLIST1_PREPEND(meth->queuehead, inst, inqueue); return 0; } static void nomove_remove(Method *mm, Change *remvchg) { NomoveMethod *meth= (void*)mm; NomoveChange *remv= (void*)remvchg; - DLIST1_REMOVE(meth->queuehead, remv, inqueue); + if (remv->n_motions) + DLIST1_REMOVE(meth->queuehead, remv, inqueue); } static ErrorCode nomove_check(Method *mm) { return 0; } -static void (*nomove_execute_now)(oop_source *source, struct timeval tv, - void *meth_v) { +static void *nomove_execute_now(oop_source *source, struct timeval tv, + void *meth_v) { NomoveMethod *meth= meth_v; meth->eventqueued= 0; NomoveChange *done; + while ((done= meth->queuehead)) { + assert(done->n_motions); + int i; for (i=0; in_motions; i++) - method_update_feature(&meth->m, done->indep, &done->motions[i]); + method_update_feature(&meth->m, done->h.indep, &done->motions[i]); method_change_done(&meth->m, &done->h); DLIST1_REMOVE(meth->queuehead, done, inqueue); nomove_dispose(&meth->m, &done->h); } + return OOP_CONTINUE; } + static void nomove_execute(Method *mm) { NomoveMethod *meth= (void*)mm; if (!meth->eventqueued) { @@ -642,7 +673,7 @@ static void nomove_execute(Method *mm) { } static Method nomove_method= { - "nomove", mfk_none, + "nomove", nomove_prepare, nomove_dispose, nomove_install, nomove_remove, nomove_check, nomove_execute, ignore_all_abandon @@ -652,8 +683,8 @@ static Method nomove_method= { #define INDEP_DBG_FMT "<%p:%s/%s[%d]>" #define INDEP_DBG_ARGS(in) (in), \ - (in)->move->i->pname, (movpos_pname((in)->move, (in)->target)), \ - (in)->n_changes, + ((in)->move->i->pname), (movpos_pname((in)->move, (in)->target)), \ + ((in)->n_changes) #define INDEP_DPFX_FMT "movpos " INDEP_DBG_FMT " " #define INDEP_DPFX_ARGS(in) INDEP_DBG_ARGS((in)) @@ -671,33 +702,20 @@ static Method *methods[]= { static void method_update_feature(Method *m, MovPosChange *indep, const Motion *mo) { - /* Called from methods' execution logic when an individual feature - * has been moved. This is used by the method-independent code to - * compute the correct delta set of movements from the current - * actual position, when thinking about new plans. It is also sent - * to clients and ultimately used by the UI. - */ ouprintf("movpos %s feat %s %d %s\n", indep->move->i->pname, - mo->i->pname, mo->posn, m.pname); + mo->i->pname, mo->posn, m->pname); if (SOMEP(indep->actual)) indep->actual= movposcomb_update_feature(indep->actual, mo->i, mo->posn); + ouposn_moving(indep); } static void method_change_done(Method *m, Change *chg) { - /* Called from methods' execution logic when a whole change - * has been done. The method-independent code will take care of - * updating move->movposcomb. etc. - * - * REENTRANCY: must NOT be called from within a call to the method's - * execute() (and of course cannot legally be called from within - * prepare, consider, check or dispose). - */ - MovPosComb *indep= chg->indep; + Indep *indep= chg->indep; Change **search; DPRINTF(movpos,meth, METH_DPFX_FMT "method_change_done...\n", - METH_DPFX_ARGS(indep, m->m)); + METH_DPFX_ARGS(indep,*m)); for (search=indep->changes; *search; search++) if ((*search) == chg) goto found; @@ -706,11 +724,12 @@ static void method_change_done(Method *m, Change *chg) { found: indep->n_changes--; if (indep->n_changes) { - *search= indep[n_changes]; + *search= indep->changes[indep->n_changes]; return; } /* all done */ + Segment *move= indep->move; move->moving= 0; move->motion= 0; move->movposcomb= indep->target; @@ -721,7 +740,7 @@ static void method_change_done(Method *m, Change *chg) { /*---------- internal core machinery ----------*/ -static Method *feature_method(MovFeatInfo *feati) { +static Method *feature_method(const MovFeatInfo *feati) { assert(feati->kind >= 0); assert(feati->kind < ARRAY_SIZE(methods)); Method *meth= methods[feati->kind]; @@ -740,7 +759,26 @@ static int change_needed(const MovFeatInfo *feati, feati->posns, feati->weight, startpoint, target, r); return r; -} +} + +static void indep_dispose(MovPosChange *indep) { + if (!indep) return; + + DPRINTF(movpos,intern, INDEP_DPFX_FMT "dispose...\n", + INDEP_DPFX_ARGS(indep)); + + int changei; + for (changei=0; changein_changes; changei++) { + Change *chg= indep->changes[changei]; + Method *meth= chg->meth; + if (chg) { + DPRINTF(movpos,meth, METH_DPFX_FMT "dispose...\n", + METH_DPFX_ARGS(indep,*meth)); + meth->dispose(meth, chg); + } + } + free(indep); +} #define EVAL_MAX_METHODS 2 #define EVAL_MAX_MOTIONS 2 @@ -748,16 +786,15 @@ static int change_needed(const MovFeatInfo *feati, static ErrorCode indep_prepare(Segment *move, MovPosComb target, MovPosComb startpoint, int ms, int confirming, - MovPosChange *indep_r /* 0 ok */, + MovPosChange **indep_r /* 0 ok */, int *cost_r /* 0 ok */) { static int n_meths; static Method *meths[EVAL_MAX_METHODS]; static int n_motions[EVAL_MAX_METHODS]; - static Motion meths[EVAL_MAX_METHODS][EVAL_MAX_MOTIONS]; + static Motion motions[EVAL_MAX_METHODS][EVAL_MAX_MOTIONS]; const SegmentInfo *movei= move->i; int feat, DP; - const MovFeatInfo *feati; MovPosChange *indep=0; @@ -771,9 +808,8 @@ static ErrorCode indep_prepare(Segment *move, MovPosComb target, n_meths= 0; - for (feat=0, feati=movei->movfeats, tchanges=0, kind= mfk_none; - featn_movfeats; - feat++, feati++) { + for (feat=0; featn_movfeats; feat++) { + const MovFeatInfo *feati= &movei->movfeats[feat]; if (!change_needed(feati,startpoint,target)) continue; MovPosComb posn= target / feati->weight % feati->posns; Method *meth= feature_method(feati); @@ -788,7 +824,7 @@ static ErrorCode indep_prepare(Segment *move, MovPosComb target, n_motions[methi]= 0; DPRINTF2(" meths[%d]=%s", methi,meth->pname); - found_method: + found_method:; int motioni= ++n_motions[methi]; if (motioni >= EVAL_MAX_MOTIONS) return EC_MovFeatTooManyMotions; DPRINTF2(" motion[%d][%d]=%s%d", methi, motioni, feati->pname,posn); @@ -803,13 +839,14 @@ static ErrorCode indep_prepare(Segment *move, MovPosComb target, indep->move= move; indep->actual= startpoint; indep->target= target; - indep->considering= 0; } DPRINTF2("\n"); int totalcost= 0; + int changei; + ErrorCode ec; - for (int changei=0; changeineedcheck= 1; @@ -817,16 +854,16 @@ static ErrorCode indep_prepare(Segment *move, MovPosComb target, if (indep_r) DPRINTF(movpos,meth, METH_DPFX_FMT "prepare n_motions=%d...\n", - INDEP_DPFX_ARGS(indep, m->m), n_motions[changei]); + METH_DPFX_ARGS(indep,*meth), n_motions[changei]); else DPRINTF(movpos,meth, "%s prepare (costing) n_motions=%d...\n", - m->m.pname, n_motions[changei]); + meth->pname, n_motions[changei]); ec= meth->prepare(meth,move, n_motions[changei],motions[changei], ms, confirming, indep ? &indep->changes[changei] : 0, - thiscost); + &thiscost); if (ec) goto x; if (indep) { Change *chg= indep->changes[changei]; @@ -858,16 +895,17 @@ static void indep_remove(MovPosChange *remv) { if (!remv) return; DPRINTF(movpos,intern, INDEP_DPFX_FMT "remove...\n", - INDEP_DPFX_ARGS(indep)); + INDEP_DPFX_ARGS(remv)); + int i; for (i=0; in_changes; i++) { - Change *chg= inst->changes[i]; + Change *chg= remv->changes[i]; if (!chg->installed) continue; Method *meth= chg->meth; meth->needexec= 1; DPRINTF(movpos,meth, METH_DPFX_FMT "remove...\n", - METH_DPFX_ARGS(indep,m->m)); + METH_DPFX_ARGS(remv,*meth)); meth->remove(meth, chg); chg->installed= 0; } @@ -879,17 +917,19 @@ indep_install(MovPosChange *inst, int checknow) { if (!inst) return 0; DPRINTF(movpos,intern, INDEP_DPFX_FMT "install checknow=%d...\n", - INDEP_DPFX_ARGS(indep), checknow); + INDEP_DPFX_ARGS(inst), checknow); + int i; + ErrorCode ec; for (i=0; in_changes; i++) { - Change *chg= inst->changes[n_installed]; + Change *chg= inst->changes[i]; assert(!chg->installed); Method *meth= chg->meth; meth->needexec= 1; ec= meth->install(meth, chg); DPRINTF(movpos,meth, METH_DPFX_FMT "install=%s\n", - METH_DPFX_ARGS(inst,m->m), ec2str(ec)); + METH_DPFX_ARGS(inst,*meth), ec2str(ec)); if (ec) goto x; chg->installed= 1; meth->needcheck= 1; @@ -897,11 +937,12 @@ indep_install(MovPosChange *inst, int checknow) { if (checknow) { ec= meth->check(meth); DPRINTF(movpos,meth, METH_DPFX_FMT "check=%s\n", - METH_DPFX_ARGS(inst,m->m), ec2str(ec)); + METH_DPFX_ARGS(inst,*meth), ec2str(ec)); if (ec) goto x; meth->needcheck= 0; } } + return 0; x: return ec; @@ -910,41 +951,22 @@ indep_install(MovPosChange *inst, int checknow) { static void indep_check_execute(void) { DPRINTF(movpos,intern, "movpos indep_check_execute\n"); - for (Method **methwalk= methods; - (meth= *methwalk); - methwalk++) { + Method **methwalk, *meth; + for (methwalk= methods; (meth= *methwalk); methwalk++) { if (meth->needcheck) { - ec= meth->check(meth); - DPRINTF(movpos,meth, "%s check=%s\n", m->m.pname, ec2str(ec)); + ErrorCode ec= meth->check(meth); + DPRINTF(movpos,meth, "%s check=%s\n", meth->pname, ec2str(ec)); assert(!ec); meth->needcheck= 0; } if (meth->needexec) { meth->needexec= 0; - DPRINTF(movpos,meth, "%s execute...\n", m->m.pname); + DPRINTF(movpos,meth, "%s execute...\n", meth->pname); meth->execute(meth); } } } -static void indep_dispose(MovPosChange *indep) { - if (!indep) return; - - DPRINTF(movpos,intern, INDEP_DPFX_FMT "dispose...\n", - INDEP_DPFX_ARGS(indep)); - - for (int changei=0; changein_changes; changei++) { - Change *chg= indep->changes[changei]; - Method *meth= chg->meth; - if (chg) { - DPRINTF(movpos,meth, METH_DPFX_FMT "dispose...\n", - INDEP_DPFX_ARGS(indep,m->m)); - meth->dispose(meth, chg); - } - } - free(indep); -} - /*---------- entrypoints from rest of program ----------*/ ErrorCode @@ -958,7 +980,7 @@ movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r, maxdelay_ms, movpos_pname(move,startpoint)); ec= indep_prepare(move,target, startpoint, - ms,0, + maxdelay_ms,0, &indep, 0); if (ec) return ec; @@ -967,7 +989,7 @@ movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r, indep_check_execute(); - DPRINTF(movpos,reserve, "movpos reserve %s/%s ok\n", + DPRINTF(movpos,entry, "movpos reserve %s/%s ok\n", move->i->pname, movpos_pname(move,target)); return 0; @@ -976,7 +998,7 @@ movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r, indep_dispose(indep); indep_check_execute(); - DPRINTF(movpos,reserve, "movpos reserve %s/%s err=%s\n", + DPRINTF(movpos,entry, "movpos reserve %s/%s err=%s\n", move->i->pname, movpos_pname(move,target), ec2str(ec)); return ec; } @@ -997,7 +1019,7 @@ ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd, if (fwd && !(fwd ==tback || fwd ==tfwd)) continue; /* we have to search for the one which is least effort */ - ec= indep_prepare(move,tcomb,startpoint, -1,0, 0,&tcost); + ErrorCode ec= indep_prepare(move,tcomb,startpoint, -1,0, 0,&tcost); if (ec) return ec; if (tcost >= bestcost) /* prefer low-numbered movposcombs */ @@ -1014,9 +1036,7 @@ ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd, ErrorCode movpos_change(Segment *move, MovPosComb target, int maxdelay_ms, MovPosChange *resv) { - const SegmentInfo *movei= move->i; - const MovFeatInfo *feati; - int feat, DP; + int DP; MovPosComb actual; ErrorCode ec; @@ -1030,11 +1050,10 @@ ErrorCode movpos_change(Segment *move, MovPosComb target, DPRINTF1(movpos,entry, "movpos change %s/%s maxdelay=%dms actual=%s", move->i->pname, movpos_pname(move, target), maxdelay_ms, movpos_pname(move, actual)); - if (resv) DPRINTF2(movpos,entry, " resv=%s:%s/%s", - resv->meth->pname, resv->move->i->pname, + if (resv) DPRINTF2(" resv=%s/%s", + resv->move->i->pname, movpos_pname(resv->move, resv->target)); - if (move->motion) DPRINTF2(movpos,entry, " oldmotion=%s:/%s", - move->motion->meth->pname, + if (move->motion) DPRINTF2(" oldmotion=/%s", movpos_pname(move, move->motion->target)); DPRINTF2("\n"); @@ -1056,9 +1075,10 @@ ErrorCode movpos_change(Segment *move, MovPosComb target, move->motion= inst; + ouposn_moving(inst); indep_check_execute(); - DPRINTF(movpos,change, "movpos change %s/%s ok\n", + DPRINTF(movpos,entry, "movpos change %s/%s ok\n", move->i->pname, movpos_pname(move, target)); return 0; @@ -1075,16 +1095,16 @@ ErrorCode movpos_change(Segment *move, MovPosComb target, } void movpos_unreserve(MovPosChange *resv) { - if (!res) return; + if (!resv) return; DPRINTF(movpos,entry, "movpos unreserve %s/%s...\n", - res->move->i->pname, movpos_pname(res->move, res->intent)); + resv->move->i->pname, movpos_pname(resv->move, resv->target)); indep_remove(resv); indep_dispose(resv); indep_check_execute(); } MovPosComb movpos_change_intent(MovPosChange *indep) { - return indep->intent; + return indep->target; } void motions_all_abandon(void) { diff --git a/hostside/realtime.c b/hostside/realtime.c index 20e7535..1e01430 100644 --- a/hostside/realtime.c +++ b/hostside/realtime.c @@ -334,7 +334,7 @@ void debug_count_event(const char *what) { } #define DEFDFLAGS_safety ~(DBIT_safety_predictplan|DBIT_safety_predictseg) -#define DEFDFLAGS_movpos ~(DBIT_movpos_eval|DBIT_movpos_changeneeded) +#define DEFDFLAGS_movpos ~(DBIT_movpos_entry|DBIT_movpos_eval) #define DEFDFLAGS_speed ~(DBIT_speed_query) #define DEFDFLAGS_retransmit ~(DBIT_retransmit_message) diff --git a/hostside/safety.h b/hostside/safety.h index ba32a9d..9ae0caa 100644 --- a/hostside/safety.h +++ b/hostside/safety.h @@ -224,7 +224,7 @@ movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r, void movpos_unreserve(MovPosChange *reservation /* 0 ok */); -MovPosComb movpos_poscomb_actual(Segment *seg); +MovPosComb movpos_poscomb_actual(const Segment *seg); /* gives actual current position as published by movfeatkind * -1 means not known or cannot be represented as a MovPosComb */ @@ -241,7 +241,7 @@ MovPosComb movpos_change_intent(MovPosChange *chg); * and/or current situation */ -const char *movpos_pname(Segment *move, MovPosComb poscomb); +const char *movpos_pname(const Segment *move, MovPosComb poscomb); MovPosComb movposcomb_update_feature(MovPosComb startpoint, const MovFeatInfo *mfi, -- 2.30.2