void guidance_h_mode_changed(uint8_t new_mode) { if (new_mode == guidance_h.mode) { return; } #if HYBRID_NAVIGATION guidance_hybrid_norm_ref_airspeed = 0; #endif switch (new_mode) { case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_enter(); break; #if USE_STABILIZATION_RATE case GUIDANCE_H_MODE_RATE: stabilization_rate_enter(); break; #endif case GUIDANCE_H_MODE_CARE_FREE: stabilization_attitude_reset_care_free_heading(); case GUIDANCE_H_MODE_FORWARD: case GUIDANCE_H_MODE_ATTITUDE: #if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ if (guidance_h.mode == GUIDANCE_H_MODE_KILL || guidance_h.mode == GUIDANCE_H_MODE_RATE || guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT) #endif stabilization_attitude_enter(); break; case GUIDANCE_H_MODE_GUIDED: case GUIDANCE_H_MODE_HOVER: #if GUIDANCE_INDI guidance_indi_enter(); #endif guidance_h_hover_enter(); #if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ if (guidance_h.mode == GUIDANCE_H_MODE_KILL || guidance_h.mode == GUIDANCE_H_MODE_RATE || guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT) #endif stabilization_attitude_enter(); break; #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE case GUIDANCE_H_MODE_MODULE: guidance_h_module_enter(); break; #endif case GUIDANCE_H_MODE_NAV: guidance_h_nav_enter(); #if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ if (guidance_h.mode == GUIDANCE_H_MODE_KILL || guidance_h.mode == GUIDANCE_H_MODE_RATE || guidance_h.mode == GUIDANCE_H_MODE_RC_DIRECT) #endif stabilization_attitude_enter(); break; case GUIDANCE_H_MODE_FLIP: guidance_flip_enter(); break; default: break; } guidance_h.mode = new_mode; }
void guidance_h_run(bool in_flight) { switch (guidance_h.mode) { case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_run(in_flight); break; #if USE_STABILIZATION_RATE case GUIDANCE_H_MODE_RATE: stabilization_rate_run(in_flight); break; #endif case GUIDANCE_H_MODE_FORWARD: if (transition_percentage < (100 << INT32_PERCENTAGE_FRAC)) { transition_run(); } case GUIDANCE_H_MODE_CARE_FREE: case GUIDANCE_H_MODE_ATTITUDE: stabilization_attitude_run(in_flight); break; case GUIDANCE_H_MODE_HOVER: /* set psi command from RC */ guidance_h.sp.heading = guidance_h.rc_sp.psi; /* fall trough to GUIDED to update ref, run traj and set final attitude setpoint */ case GUIDANCE_H_MODE_GUIDED: /* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */ if (!in_flight) { guidance_h_hover_enter(); } guidance_h_update_reference(); #if GUIDANCE_INDI guidance_indi_run(in_flight, guidance_h.sp.heading); #else /* compute x,y earth commands */ guidance_h_traj_run(in_flight); /* set final attitude setpoint */ stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, guidance_h.sp.heading); #endif stabilization_attitude_run(in_flight); break; case GUIDANCE_H_MODE_NAV: if (!in_flight) { guidance_h_nav_enter(); } if (horizontal_mode == HORIZONTAL_MODE_MANUAL) { stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll; stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch; stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw; } else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) { struct Int32Eulers sp_cmd_i; sp_cmd_i.phi = nav_roll; sp_cmd_i.theta = nav_pitch; sp_cmd_i.psi = nav_heading; stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i); stabilization_attitude_run(in_flight); #if HYBRID_NAVIGATION //make sure the heading is right before leaving horizontal_mode attitude guidance_hybrid_reset_heading(&sp_cmd_i); #endif } else { #if HYBRID_NAVIGATION INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_target); guidance_hybrid_run(); #else INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot); guidance_h_update_reference(); /* set psi command */ guidance_h.sp.heading = nav_heading; INT32_ANGLE_NORMALIZE(guidance_h.sp.heading); #if GUIDANCE_INDI guidance_indi_run(in_flight, guidance_h.sp.heading); #else /* compute x,y earth commands */ guidance_h_traj_run(in_flight); /* set final attitude setpoint */ stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, guidance_h.sp.heading); #endif #endif stabilization_attitude_run(in_flight); } break; #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE case GUIDANCE_H_MODE_MODULE: guidance_h_module_run(in_flight); break; #endif case GUIDANCE_H_MODE_FLIP: guidance_flip_run(); break; default: break; } }