SCurve const more local variables rename update to get_jerk_vel_pos_at_time removed unused update debug output in SITL fixes to ensure finished at end of path fixes including validity check includes corrections from peer review