//see header rc_value_t slow_down(rc_value_t current, rc_value_t goal, dsp16_t k) { if(dsp16_op_abs(goal-current) < k) { return goal; } if((goal-current) < 0) return current - k; else return current + k; }
//see header rc_value_t dead_zone(rc_value_t order, rc_value_t zone) { if(dsp16_op_abs(order) < zone) return 0; else { //substract zone value to avoid having a step around deadzone if(order < 0) return order + zone; else return order - zone; } }
/* * Output range = [-1; 1] corresponding to [-pi; pi] */ dsp16_t dsp16_op_asin(dsp16_t num) { S32 num_sqr, res; dsp16_t num_abs; num_abs = dsp16_op_abs(num); #if DSP16_QB < 15 // Limits if (((S32) num) >= ((S32) DSP16_Q(1.))) return DSP16_Q(0.5); if (((S32) num) <= ((S32) DSP16_Q(-1.))) return DSP16_Q(-0.5); #endif // If |num| > 0.5 if (num_abs > DSP16_Q(0.5)) { num_abs = dsp16_op_sqrt((DSP16_Q(1.)-num_abs) >> 1); num_abs = DSP16_Q(0.5) - (dsp16_op_asin(num_abs) << 1); return (num < 0)?-num_abs:num_abs; }