FIA platoon driver fixes

Fixed #336
Fixed #335