/**
* 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;
}
}
