#include "trajectory_manager_core.h"
/** set speed consign in quadramp filter */
-void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
+void set_quadramp_speed(struct trajectory *traj, double d_speed, double a_speed)
{
struct quadramp_filter * q_d, * q_a;
q_d = traj->csm_distance->consign_filter_params;
}
/** get angle speed consign in quadramp filter */
-uint32_t get_quadramp_angle_speed(struct trajectory *traj)
+double get_quadramp_angle_speed(struct trajectory *traj)
{
struct quadramp_filter *q_a;
q_a = traj->csm_angle->consign_filter_params;
}
/** get distance speed consign in quadramp filter */
-uint32_t get_quadramp_distance_speed(struct trajectory *traj)
+double get_quadramp_distance_speed(struct trajectory *traj)
{
struct quadramp_filter *q_d;
q_d = traj->csm_distance->consign_filter_params;
return q_d->var_1st_ord_pos;
}
+/** set speed consign in quadramp filter */
+void set_quadramp_acc(struct trajectory *traj, double d_acc, double a_acc)
+{
+ struct quadramp_filter * q_d, * q_a;
+ q_d = traj->csm_distance->consign_filter_params;
+ q_a = traj->csm_angle->consign_filter_params;
+ quadramp_set_2nd_order_vars(q_d, ABS(d_acc), ABS(d_acc));
+ quadramp_set_2nd_order_vars(q_a, ABS(a_acc), ABS(a_acc));
+}
+
/** remove event if any */
void delete_event(struct trajectory *traj)
{
set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
+ set_quadramp_acc(traj, traj->d_acc, traj->a_acc);
if ( traj->scheduler_task != -1) {
DEBUG(E_TRAJECTORY, "Delete event");
scheduler_del_event(traj->scheduler_task);
a *= 2.;
return ABS(a) < a_win_rad;
}
+
+/* distance unit conversions */
+
+double pos_mm2imp(struct trajectory *traj, double pos)
+{
+ return pos * traj->position->phys.distance_imp_per_mm;
+}
+
+double pos_imp2mm(struct trajectory *traj, double pos)
+{
+ return pos / traj->position->phys.distance_imp_per_mm;
+}
+
+double speed_mm2imp(struct trajectory *traj, double speed)
+{
+ return speed *
+ traj->position->phys.distance_imp_per_mm /
+ traj->cs_hz;
+}
+
+double speed_imp2mm(struct trajectory *traj, double speed)
+{
+ return speed * traj->cs_hz /
+ traj->position->phys.distance_imp_per_mm;
+}
+
+double acc_mm2imp(struct trajectory *traj, double acc)
+{
+ return acc * traj->position->phys.distance_imp_per_mm /
+ (traj->cs_hz * traj->cs_hz);
+}
+
+double acc_imp2mm(struct trajectory *traj, double acc)
+{
+ return acc * traj->cs_hz * traj->cs_hz /
+ traj->position->phys.distance_imp_per_mm;
+}
+
+/* angle unit conversions */
+
+double pos_rd2imp(struct trajectory *traj, double pos)
+{
+ return pos *
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm / 2.);
+}
+
+double pos_imp2rd(struct trajectory *traj, double pos)
+{
+ return pos /
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm / 2.);
+}
+
+double speed_rd2imp(struct trajectory *traj, double speed)
+{
+ return speed *
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm /
+ (2. * traj->cs_hz));
+}
+
+double speed_imp2rd(struct trajectory *traj, double speed)
+{
+ return speed /
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm /
+ (2. * traj->cs_hz));
+}
+
+double acc_rd2imp(struct trajectory *traj, double acc)
+{
+ return acc *
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm /
+ (2. * traj->cs_hz * traj->cs_hz));
+}
+
+double acc_imp2rd(struct trajectory *traj, double acc)
+{
+ return acc /
+ (traj->position->phys.distance_imp_per_mm *
+ traj->position->phys.track_mm /
+ (2. * traj->cs_hz * traj->cs_hz));
+}
+