void guidance_v_run(bool_t in_flight) {
// FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
// AKA SUPERVISION and co
if (in_flight) {
gv_adapt_run(ins_ltp_accel.z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref);
}
switch (guidance_v_mode) {
case GUIDANCE_V_MODE_RC_DIRECT:
guidance_v_z_sp = ins_ltp_pos.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;
}
}
