mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-04 12:24:11 +01:00
240 lines
11 KiB
C
240 lines
11 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup OpenPilotModules OpenPilot Modules
|
|
* @{
|
|
* @addtogroup ManualControl
|
|
* @brief Interpretes the control input in ManualControlCommand
|
|
* @{
|
|
*
|
|
* @file stabilizedhandler.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
|
*
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
******************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
#include "inc/manualcontrol.h"
|
|
#include <mathmisc.h>
|
|
#include <sin_lookup.h>
|
|
#include <manualcontrolcommand.h>
|
|
#include <stabilizationdesired.h>
|
|
#include <flightmodesettings.h>
|
|
#include <stabilizationbank.h>
|
|
#include <flightstatus.h>
|
|
#include <systemident.h>
|
|
|
|
// Private constants
|
|
|
|
// Private types
|
|
|
|
// Private functions
|
|
static float applyExpo(float value, float expo);
|
|
|
|
// Private variables
|
|
static uint8_t currentFpvTiltAngle = 0;
|
|
static float cosAngle = 0.0f;
|
|
static float sinAngle = 0.0f;
|
|
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
|
static FlightModeSettingsStabilization1SettingsData autotuneSettings = {
|
|
.Roll = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT,
|
|
.Pitch = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT,
|
|
.Yaw = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT,
|
|
.Thrust = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
|
|
};
|
|
#if 0
|
|
static FlightModeSettingsStabilization1SettingsData attitudeSettings = {
|
|
.Roll = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE,
|
|
.Pitch = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE,
|
|
.Yaw = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE,
|
|
.Thrust = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
|
|
};
|
|
#endif
|
|
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
|
|
|
static float applyExpo(float value, float expo)
|
|
{
|
|
// note: fastPow makes a small error, therefore result needs to be bound
|
|
float exp = boundf(fastPow(1.00695f, expo), 0.5f, 2.0f);
|
|
|
|
// magic number scales expo
|
|
// so that
|
|
// expo=100 yields value**10
|
|
// expo=0 yields value**1
|
|
// expo=-100 yields value**(1/10)
|
|
// (pow(2.0,1/100)~=1.00695)
|
|
if (value > 0.0f) {
|
|
return boundf(fastPow(value, exp), 0.0f, 1.0f);
|
|
} else if (value < -0.0f) {
|
|
return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
|
|
} else {
|
|
return 0.0f;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
|
|
* @input: ManualControlCommand
|
|
* @output: StabilizationDesired
|
|
*/
|
|
void stabilizedHandler(__attribute__((unused)) bool newinit)
|
|
{
|
|
static bool inited=false;
|
|
if (!inited) {
|
|
inited = true;
|
|
StabilizationDesiredInitialize();
|
|
StabilizationBankInitialize();
|
|
}
|
|
ManualControlCommandData cmd;
|
|
ManualControlCommandGet(&cmd);
|
|
|
|
FlightModeSettingsData settings;
|
|
FlightModeSettingsGet(&settings);
|
|
|
|
StabilizationDesiredData stabilization;
|
|
StabilizationDesiredGet(&stabilization);
|
|
|
|
StabilizationBankData stabSettings;
|
|
StabilizationBankGet(&stabSettings);
|
|
|
|
cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
|
|
cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
|
|
cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
|
|
|
|
if (stabSettings.FpvCamTiltCompensation > 0) {
|
|
// Reduce Cpu load
|
|
if (currentFpvTiltAngle != stabSettings.FpvCamTiltCompensation) {
|
|
cosAngle = cos_lookup_deg((float)stabSettings.FpvCamTiltCompensation);
|
|
sinAngle = sin_lookup_deg((float)stabSettings.FpvCamTiltCompensation);
|
|
currentFpvTiltAngle = stabSettings.FpvCamTiltCompensation;
|
|
}
|
|
float rollCommand = cmd.Roll;
|
|
float yawCommand = cmd.Yaw;
|
|
|
|
// http://shrediquette.blogspot.de/2016/01/some-thoughts-on-camera-tilt.html
|
|
// When Roll right, add negative Yaw.
|
|
// When Yaw left, add negative Roll.
|
|
cmd.Roll = boundf((cosAngle * rollCommand) + (sinAngle * yawCommand), -1.0f, 1.0f);
|
|
cmd.Yaw = boundf((cosAngle * yawCommand) - (sinAngle * rollCommand), -1.0f, 1.0f);
|
|
}
|
|
|
|
uint8_t *stab_settings;
|
|
FlightStatusData flightStatus;
|
|
FlightStatusGet(&flightStatus);
|
|
switch (flightStatus.FlightMode) {
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
|
|
break;
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
|
|
break;
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
|
|
break;
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
|
|
break;
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
|
|
break;
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
|
|
break;
|
|
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
|
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
|
|
// if (SystemIdentHandle()) {
|
|
stab_settings = (uint8_t *)&autotuneSettings;
|
|
// } else {
|
|
// stab_settings = (uint8_t *)&attitudeSettings;
|
|
// }
|
|
break;
|
|
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
|
default:
|
|
// Major error, this should not occur because only enter this block when one of these is true
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
|
|
return;
|
|
}
|
|
|
|
stabilization.Roll =
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Roll * stabSettings.ManualRate.Roll :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll :
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
|
|
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Roll * stabSettings.RollMax :
|
|
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
|
0; // this is an invalid mode
|
|
|
|
stabilization.Pitch =
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
|
|
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Pitch * stabSettings.PitchMax :
|
|
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
|
0; // this is an invalid mode
|
|
|
|
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
|
|
stabilization.StabilizationMode.Roll = stab_settings[0];
|
|
stabilization.StabilizationMode.Pitch = stab_settings[1];
|
|
// Other axes (yaw) cannot be Rattitude, so use Rate
|
|
// Should really do this for Attitude mode as well?
|
|
if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
|
|
stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
|
|
stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
|
|
} else {
|
|
stabilization.StabilizationMode.Yaw = stab_settings[2];
|
|
stabilization.Yaw =
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
|
|
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
|
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
|
0; // this is an invalid mode
|
|
}
|
|
|
|
stabilization.StabilizationMode.Thrust = stab_settings[3];
|
|
stabilization.Thrust = cmd.Thrust;
|
|
StabilizationDesiredSet(&stabilization);
|
|
}
|
|
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|