Commit 90c53207 authored by Jean-Claude BAU's avatar Jean-Claude BAU

WR servo: Add raw delayMM calculation

This calculation is done using the timestamps but without any correction.
parent 9bdd4913
......@@ -122,6 +122,8 @@ typedef struct wr_servo_ext {
struct pp_time delta_rxm;
struct pp_time delta_txs;
struct pp_time delta_rxs;
struct pp_time rawT1, rawT2, rawT3, rawT4, rawT5, rawT6; /* raw value of timestamps */
struct pp_time rawDelayMM; // Calculation of delayMM with raw values of timestamps
}wr_servo_ext_t;
......
......@@ -11,6 +11,27 @@ void wr_servo_enable_tracking(int enable)
wr_tracking_enabled = enable;
}
static inline void _calculate_raw_delayMM(struct pp_instance *ppi,
struct pp_time *ta,struct pp_time *tb,
struct pp_time *tc,struct pp_time *td ) {
wr_servo_ext_t *se=WRE_SRV(ppi);
/* The calculation done will be
* (td-ta)-(tc-tb)
*/
struct pp_time *sa=&se->rawDelayMM,sb;
/* sa = td-ta) */
*sa=*td;
pp_time_sub(sa,ta);
/* sb = (tc-tb) */
sb=*tc;
pp_time_sub(&sb,tb);
/* sa-sb */
pp_time_sub(sa,&sb);
}
int wr_servo_init(struct pp_instance *ppi)
{
if ( wrh_servo_init(ppi) ) {
......@@ -25,6 +46,12 @@ int wr_servo_got_sync(struct pp_instance *ppi) {
/* Re-adjust T1 and T2 */
wr_servo_ext_t *se=WRE_SRV(ppi);
se->rawT1=ppi->t1;
se->rawT2=ppi->t2;
if ( is_delayMechanismP2P(ppi) && SRV(ppi)->got_sync) {
// Calculate raw delayMM
_calculate_raw_delayMM(ppi,&se->rawT3,&se->rawT4,&se->rawT5,&se->rawT6);
}
pp_time_add(&ppi->t1,&se->delta_txm);
pp_time_sub(&ppi->t2,&se->delta_rxs);
return wrh_servo_got_sync(ppi);
......@@ -34,6 +61,12 @@ int wr_servo_got_resp(struct pp_instance *ppi) {
/* Re-adjust T3 and T4 */
wr_servo_ext_t *se=WRE_SRV(ppi);
se->rawT3=ppi->t3;
se->rawT4=ppi->t4;
if ( is_delayMechanismE2E(ppi) && SRV(ppi)->got_sync) {
// Calculate raw delayMM
_calculate_raw_delayMM(ppi,&se->rawT1,&se->rawT2,&se->rawT3,&se->rawT4);
}
pp_time_add(&ppi->t3,&se->delta_txs);
pp_time_sub(&ppi->t4,&se->delta_rxm);
return wrh_servo_got_resp(ppi);
......@@ -43,6 +76,10 @@ int wr_servo_got_presp(struct pp_instance *ppi) {
/* Re-adjust T3,T4,T5 and T6 */
wr_servo_ext_t *se=WRE_SRV(ppi);
se->rawT3=ppi->t3;
se->rawT4=ppi->t4;
se->rawT5=ppi->t5;
se->rawT6=ppi->t6;
pp_time_add(&ppi->t3,&se->delta_txs);
pp_time_sub(&ppi->t4,&se->delta_rxm);
pp_time_add(&ppi->t5,&se->delta_txm);
......
......@@ -178,6 +178,13 @@ struct dump_info wr_servo_ext_info [] = {
DUMP_FIELD(time, delta_rxm),
DUMP_FIELD(time, delta_txs),
DUMP_FIELD(time, delta_rxs),
DUMP_FIELD(time, rawT1),
DUMP_FIELD(time, rawT2),
DUMP_FIELD(time, rawT3),
DUMP_FIELD(time, rawT4),
DUMP_FIELD(time, rawT5),
DUMP_FIELD(time, rawT6),
DUMP_FIELD(time, rawDelayMM),
};
#endif
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment