void guidance_v_mode_changed(uint8_t new_mode) {
if (new_mode == guidance_v_mode)
return;
switch (new_mode) {
case GUIDANCE_V_MODE_HOVER:
guidance_v_z_sp = stateGetPositionNed_i()->z; // set current altitude as setpoint
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0);
break;
case GUIDANCE_V_MODE_RC_CLIMB:
case GUIDANCE_V_MODE_CLIMB:
guidance_v_zd_sp = 0;
case GUIDANCE_V_MODE_NAV:
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
break;
default:
break;
}
guidance_v_mode = new_mode;
}
void guidance_v_mode_changed(uint8_t new_mode) {
if (new_mode == guidance_v_mode)
return;
switch (new_mode) {
case GUIDANCE_V_MODE_HOVER:
guidance_v_z_sp = ins_ltp_pos.z; // set current altitude as setpoint
guidance_v_z_sum_err = 0;
GuidanceVSetRef(ins_ltp_pos.z, 0, 0);
break;
case GUIDANCE_V_MODE_RC_CLIMB:
case GUIDANCE_V_MODE_CLIMB:
guidance_v_zd_sp = 0;
case GUIDANCE_V_MODE_NAV:
guidance_v_z_sum_err = 0;
GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0);
break;
default:
break;
}
guidance_v_mode = new_mode;
}
void guidance_v_mode_changed(uint8_t new_mode)
{
if (new_mode == guidance_v_mode) {
return;
}
switch (new_mode) {
case GUIDANCE_V_MODE_GUIDED:
case GUIDANCE_V_MODE_HOVER:
/* disable vertical velocity setpoints */
guidance_v_guided_vel_enabled = false;
/* set current altitude as setpoint */
guidance_v_z_sp = stateGetPositionNed_i()->z;
/* reset guidance reference */
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0);
/* reset speed setting */
guidance_v_zd_sp = 0;
break;
case GUIDANCE_V_MODE_RC_CLIMB:
case GUIDANCE_V_MODE_CLIMB:
guidance_v_zd_sp = 0;
case GUIDANCE_V_MODE_NAV:
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
break;
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
case GUIDANCE_V_MODE_MODULE:
guidance_v_module_enter();
break;
#endif
case GUIDANCE_V_MODE_FLIP:
break;
default:
break;
}
guidance_v_mode = new_mode;
}
void guidance_v_mode_changed(uint8_t new_mode) {
if (new_mode == guidance_v_mode)
return;
// switch ( guidance_v_mode ) {
//
// }
switch (new_mode) {
case GUIDANCE_V_MODE_RC_CLIMB:
case GUIDANCE_V_MODE_CLIMB:
case GUIDANCE_V_MODE_HOVER:
case GUIDANCE_V_MODE_NAV:
guidance_v_z_sum_err = 0;
GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0);
break;
default:
break;
}
guidance_v_mode = new_mode;
}
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;
}
}
