Pātuhitia tēnei: Advanced Sensing and Safety Control for Connected and Automated Vehicles