直立控制算法,飞思卡尔智能车直立行走控制算法。部分核心