program is ok to run. but runtime is 0.6sec
pulled from github.
Merge branch 'JX' of https://github.com/zhao0079/Simulink_for_Quadcopter into JX
started to add constraint.
need to complete the constraint portion.