void guidance_h_read_rc(bool in_flight) { switch (guidance_h.mode) { case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_read_rc(); break; #if USE_STABILIZATION_RATE case GUIDANCE_H_MODE_RATE: #if SWITCH_STICKS_FOR_RATE_CONTROL stabilization_rate_read_rc_switched_sticks(); #else stabilization_rate_read_rc(); #endif break; #endif case GUIDANCE_H_MODE_CARE_FREE: stabilization_attitude_read_rc(in_flight, TRUE, FALSE); break; case GUIDANCE_H_MODE_FORWARD: stabilization_attitude_read_rc(in_flight, FALSE, TRUE); break; case GUIDANCE_H_MODE_ATTITUDE: stabilization_attitude_read_rc(in_flight, FALSE, FALSE); break; case GUIDANCE_H_MODE_HOVER: stabilization_attitude_read_rc_setpoint_eulers(&guidance_h.rc_sp, in_flight, FALSE, FALSE); #if GUIDANCE_H_USE_SPEED_REF read_rc_setpoint_speed_i(&guidance_h.sp.speed, in_flight); /* enable x,y velocity setpoints */ SetBit(guidance_h.sp.mask, 5); #endif break; #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE case GUIDANCE_H_MODE_MODULE: guidance_h_module_read_rc(); break; #endif case GUIDANCE_H_MODE_NAV: if (radio_control.status == RC_OK) { stabilization_attitude_read_rc_setpoint_eulers(&guidance_h.rc_sp, in_flight, FALSE, FALSE); } else { INT_EULERS_ZERO(guidance_h.rc_sp); } break; case GUIDANCE_H_MODE_FLIP: stabilization_attitude_read_rc(in_flight, FALSE, FALSE); 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(); }