void guidance_v_run(bool_t in_flight) { // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT // AKA SUPERVISION and co if (in_flight) { gv_adapt_run(stateGetAccelNed_i()->z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref); } switch (guidance_v_mode) { case GUIDANCE_V_MODE_RC_DIRECT: guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t; break; case GUIDANCE_V_MODE_RC_CLIMB: guidance_v_zd_sp = guidance_v_rc_zd_sp; gv_update_ref_from_zd_sp(guidance_v_zd_sp); run_hover_loop(in_flight); stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; case GUIDANCE_V_MODE_CLIMB: #if USE_FMS if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_CLIMB) { guidance_v_zd_sp = fms.input.v_sp.climb; } #endif gv_update_ref_from_zd_sp(guidance_v_zd_sp); run_hover_loop(in_flight); #if NO_RC_THRUST_LIMIT stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; #else // saturate max authority with RC stick stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); #endif break; case GUIDANCE_V_MODE_HOVER: #if USE_FMS if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER) guidance_v_z_sp = fms.input.v_sp.height; #endif gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); #if NO_RC_THRUST_LIMIT stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; #else // saturate max authority with RC stick stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); #endif break; case GUIDANCE_V_MODE_NAV: { if (vertical_mode == VERTICAL_MODE_ALT) { guidance_v_z_sp = -nav_flight_altitude; gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_CLIMB) { guidance_v_zd_sp = -nav_climb; gv_update_ref_from_zd_sp(guidance_v_zd_sp); nav_flight_altitude = -guidance_v_z_sp; run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_MANUAL) { guidance_v_z_sp = -nav_flight_altitude; // For display only guidance_v_delta_t = nav_throttle; } #if NO_RC_THRUST_LIMIT stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; #else /* use rc limitation if available */ if (radio_control.status == RC_OK) stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); else stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; #endif break; } default: break; } }
void guidance_v_run(bool_t in_flight) { // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT // AKA SUPERVISION and co if (in_flight) { // we should use something after the supervision!!! int32_t cmd_hack = Chop(stabilization_cmd[COMMAND_THRUST], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); gv_adapt_run(ins_ltp_accel.z, cmd_hack); //gv_adapt_run(ins_ltp_accel.z, cmd_hack, guidance_v_zd_ref); } else { // reset vertical filter until takeoff //ins_vf_realign = TRUE; } switch (guidance_v_mode) { case GUIDANCE_V_MODE_RC_DIRECT: guidance_v_z_sp = ins_ltp_pos.z; // not sure why we do that GuidanceVSetRef(ins_ltp_pos.z, 0, 0); // or that - mode enter should take care of it ? stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t; break; case GUIDANCE_V_MODE_RC_CLIMB: guidance_v_zd_sp = guidance_v_rc_zd_sp; gv_update_ref_from_zd_sp(guidance_v_zd_sp); run_hover_loop(in_flight); stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; case GUIDANCE_V_MODE_CLIMB: #ifdef USE_FMS if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_CLIMB) guidance_v_zd_sp = fms.input.v_sp.climb; #endif gv_update_ref_from_zd_sp(guidance_v_zd_sp); run_hover_loop(in_flight); // saturate max authority with RC stick stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t, guidance_v_delta_t); break; case GUIDANCE_V_MODE_HOVER: #ifdef USE_FMS if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER) guidance_v_z_sp = fms.input.v_sp.height; #endif gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); // saturate max authority with RC stick stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t, guidance_v_delta_t); break; case GUIDANCE_V_MODE_NAV: { if (vertical_mode == VERTICAL_MODE_ALT) { guidance_v_z_sp = -nav_flight_altitude; gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_CLIMB) { guidance_v_zd_sp = -nav_climb; gv_update_ref_from_zd_sp(guidance_v_zd_sp); nav_flight_altitude = -guidance_v_z_sp; run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_MANUAL) { guidance_v_z_sp = -nav_flight_altitude; // For display only guidance_v_delta_t = nav_throttle; } /* use rc limitation if available */ if (radio_control.status == RC_OK) stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t, guidance_v_delta_t); else stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; } default: break; } }