/** * Main guidance loop * @param[in] in_flight Whether we are in flight or not */ void guidance_h_module_run(bool in_flight) { /* Update the setpoint */ stabilization_attitude_set_rpy_setpoint_i(&opticflow_stab.cmd); /* Run the default attitude stabilization */ stabilization_attitude_run(in_flight); }
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(true); } case GUIDANCE_H_MODE_CARE_FREE: case GUIDANCE_H_MODE_ATTITUDE: if ((!(guidance_h.mode == GUIDANCE_H_MODE_FORWARD)) && transition_percentage > 0) { transition_run(false); } 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_guided_run(in_flight); break; case GUIDANCE_H_MODE_NAV: guidance_h_from_nav(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; } }
void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) { // RunOnceEvery(50, nav_periodic_task_10Hz()) BOOZ2_AUTOPILOT_CHECK_MOTORS_ON(); if(kill) booz2_autopilot_motors_on = FALSE; booz2_autopilot_in_flight = _in_flight; stabilization_attitude_read_rc(booz2_autopilot_in_flight); stabilization_attitude_run(booz2_autopilot_in_flight); booz2_guidance_v_run(booz2_autopilot_in_flight); stabilization_cmd[COMMAND_THRUST] = (int32_t)radio_control.values[RADIO_THROTTLE] * 105 / 7200 + 95; CscSetCommands(stabilization_cmd, booz2_autopilot_in_flight,booz2_autopilot_motors_on); BOOZ2_SUPERVISION_RUN(mixed_commands, commands, booz2_autopilot_motors_on); if(booz2_autopilot_motors_on && props_throttle_pass){ Bound(stabilization_cmd[COMMAND_THRUST],0,255); for(uint8_t i = 0; i < PROPS_NB; i++) mixed_commands[i] = stabilization_cmd[COMMAND_THRUST]; } for(uint8_t i = 0; i < PROPS_NB; i++){ if(props_enable[i]) props_set(i,mixed_commands[i]); else props_set(i,0); } props_commit(); MERCURY_SURFACES_SUPERVISION_RUN(Actuator, stabilization_cmd, mixed_commands, (!booz2_autopilot_in_flight)); ActuatorsCommit(); SendCscFromActuators(); }
void guidance_flip_run(void) { uint32_t timer; int32_t phi; static uint32_t timer_save = 0; timer = (flip_counter++ << 12) / PERIODIC_FREQUENCY; phi = stateGetNedToBodyEulers_i()->phi; switch (flip_state) { case 0: flip_cmd_earth.x = 0; flip_cmd_earth.y = 0; stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth, heading_save); stabilization_attitude_run(autopilot_in_flight); stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first timer_save = 0; if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) { flip_state++; } break; case 1: stabilization_cmd[COMMAND_ROLL] = 9000; // Rolling command stabilization_cmd[COMMAND_PITCH] = 0; stabilization_cmd[COMMAND_YAW] = 0; stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust? if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) { flip_state++; } break; case 2: stabilization_cmd[COMMAND_ROLL] = 0; stabilization_cmd[COMMAND_PITCH] = 0; stabilization_cmd[COMMAND_YAW] = 0; stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust? if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) { timer_save = timer; flip_state++; } break; case 3: flip_cmd_earth.x = 0; flip_cmd_earth.y = 0; stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth, heading_save); stabilization_attitude_run(autopilot_in_flight); stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) { flip_state++; } break; default: autopilot_mode_auto2 = autopilot_mode_old; autopilot_set_mode(autopilot_mode_old); stab_att_sp_euler.psi = heading_save; flip_rollout = false; flip_counter = 0; timer_save = 0; flip_state = 0; stabilization_cmd[COMMAND_ROLL] = 0; stabilization_cmd[COMMAND_PITCH] = 0; stabilization_cmd[COMMAND_YAW] = 0; stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll? break; } }
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; } }