2014-03-02 14:45:17 +01:00
|
|
|
/**
|
|
|
|
******************************************************************************
|
|
|
|
* @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"
|
2014-10-05 01:25:15 +02:00
|
|
|
#include <mathmisc.h>
|
2016-01-14 22:55:12 +01:00
|
|
|
#include <sin_lookup.h>
|
2014-03-02 14:45:17 +01:00
|
|
|
#include <manualcontrolcommand.h>
|
|
|
|
#include <stabilizationdesired.h>
|
|
|
|
#include <flightmodesettings.h>
|
|
|
|
#include <stabilizationbank.h>
|
|
|
|
|
|
|
|
// Private constants
|
|
|
|
|
|
|
|
// Private types
|
|
|
|
|
|
|
|
// Private functions
|
2014-10-03 18:57:16 +02:00
|
|
|
static float applyExpo(float value, float expo);
|
|
|
|
|
2016-01-14 22:55:12 +01:00
|
|
|
// Private variables
|
|
|
|
static uint8_t currentFpvTiltAngle = 0;
|
|
|
|
static float cosAngle = 0.0f;
|
|
|
|
static float sinAngle = 0.0f;
|
2014-10-03 18:57:16 +02:00
|
|
|
|
|
|
|
static float applyExpo(float value, float expo)
|
|
|
|
{
|
2014-10-05 01:25:15 +02:00
|
|
|
// note: fastPow makes a small error, therefore result needs to be bound
|
2016-07-01 15:30:19 +02:00
|
|
|
float exp = boundf(fastPow(1.01395948f, expo), 0.25f, 4.0f);
|
2014-10-03 18:57:16 +02:00
|
|
|
|
|
|
|
// magic number scales expo
|
|
|
|
// so that
|
|
|
|
// expo=100 yields value**10
|
|
|
|
// expo=0 yields value**1
|
|
|
|
// expo=-100 yields value**(1/10)
|
2016-07-01 15:30:19 +02:00
|
|
|
// (pow(4.0,1/100)~=1.01395948)
|
2014-10-03 18:57:16 +02:00
|
|
|
if (value > 0.0f) {
|
2014-10-05 01:25:15 +02:00
|
|
|
return boundf(fastPow(value, exp), 0.0f, 1.0f);
|
|
|
|
} else if (value < -0.0f) {
|
|
|
|
return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
|
2014-10-03 18:57:16 +02:00
|
|
|
} else {
|
2014-10-05 01:25:15 +02:00
|
|
|
return 0.0f;
|
2014-10-03 18:57:16 +02:00
|
|
|
}
|
|
|
|
}
|
2014-03-02 14:45:17 +01:00
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
* @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
|
|
|
|
* @input: ManualControlCommand
|
|
|
|
* @output: StabilizationDesired
|
|
|
|
*/
|
|
|
|
void stabilizedHandler(bool newinit)
|
|
|
|
{
|
|
|
|
if (newinit) {
|
|
|
|
StabilizationDesiredInitialize();
|
|
|
|
StabilizationBankInitialize();
|
|
|
|
}
|
|
|
|
ManualControlCommandData cmd;
|
|
|
|
ManualControlCommandGet(&cmd);
|
|
|
|
|
|
|
|
FlightModeSettingsData settings;
|
|
|
|
FlightModeSettingsGet(&settings);
|
|
|
|
|
|
|
|
StabilizationDesiredData stabilization;
|
|
|
|
StabilizationDesiredGet(&stabilization);
|
|
|
|
|
|
|
|
StabilizationBankData stabSettings;
|
|
|
|
StabilizationBankGet(&stabSettings);
|
|
|
|
|
2014-10-03 18:57:16 +02:00
|
|
|
cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
|
|
|
|
cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
|
|
|
|
cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
|
2016-01-14 22:55:12 +01:00
|
|
|
|
|
|
|
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
|
2016-01-16 11:39:00 +01:00
|
|
|
// 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);
|
2016-01-14 22:55:12 +01:00
|
|
|
}
|
|
|
|
|
2014-03-02 14:45:17 +01:00
|
|
|
uint8_t *stab_settings;
|
|
|
|
FlightStatusData flightStatus;
|
|
|
|
FlightStatusGet(&flightStatus);
|
2015-01-08 03:20:47 +01:00
|
|
|
|
2014-03-02 14:45:17 +01:00
|
|
|
switch (flightStatus.FlightMode) {
|
|
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
2015-05-02 16:46:35 +02:00
|
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
|
2014-03-02 14:45:17 +01:00
|
|
|
break;
|
|
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
|
2015-05-02 16:46:35 +02:00
|
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
|
2014-03-02 14:45:17 +01:00
|
|
|
break;
|
|
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
|
2015-05-02 16:46:35 +02:00
|
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
|
2014-03-02 14:45:17 +01:00
|
|
|
break;
|
2014-05-03 18:38:45 +02:00
|
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
|
2015-05-02 16:46:35 +02:00
|
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
|
2014-05-03 18:38:45 +02:00
|
|
|
break;
|
|
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
|
2015-05-02 16:46:35 +02:00
|
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
|
2014-05-03 18:38:45 +02:00
|
|
|
break;
|
|
|
|
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
2015-05-02 16:46:35 +02:00
|
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
|
2014-05-03 18:38:45 +02:00
|
|
|
break;
|
2014-03-02 14:45:17 +01:00
|
|
|
default:
|
|
|
|
// Major error, this should not occur because only enter this block when one of these is true
|
2014-03-02 20:06:05 +01:00
|
|
|
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
2015-05-02 16:46:35 +02:00
|
|
|
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
|
2014-03-02 14:45:17 +01:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
stabilization.Roll =
|
2014-04-26 20:33:25 +02:00
|
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll :
|
2014-03-02 14:45:17 +01:00
|
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
|
2015-05-04 09:36:04 +02:00
|
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Roll * stabSettings.ManualRate.Roll :
|
2014-04-27 15:15:59 +02:00
|
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
|
2014-03-02 14:45:17 +01:00
|
|
|
(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 :
|
2014-10-02 21:28:43 +02:00
|
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll :
|
2014-04-27 15:15:59 +02:00
|
|
|
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
|
2014-03-02 14:45:17 +01:00
|
|
|
0; // this is an invalid mode
|
|
|
|
|
|
|
|
stabilization.Pitch =
|
2014-04-26 20:33:25 +02:00
|
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch :
|
2014-03-02 14:45:17 +01:00
|
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
|
2015-05-04 09:36:04 +02:00
|
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
|
2014-04-27 15:15:59 +02:00
|
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
|
2014-03-02 14:45:17 +01:00
|
|
|
(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 :
|
2014-10-02 21:28:43 +02:00
|
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
|
2014-04-27 15:15:59 +02:00
|
|
|
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
|
2014-03-02 14:45:17 +01:00
|
|
|
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 =
|
2014-04-26 20:33:25 +02:00
|
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw :
|
2014-03-02 14:45:17 +01:00
|
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
2015-05-04 09:36:04 +02:00
|
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
2014-04-27 15:15:59 +02:00
|
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
|
2014-03-02 14:45:17 +01:00
|
|
|
(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 :
|
2014-10-02 21:28:43 +02:00
|
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
|
2014-04-27 15:15:59 +02:00
|
|
|
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
|
2014-03-02 14:45:17 +01:00
|
|
|
0; // this is an invalid mode
|
|
|
|
}
|
|
|
|
|
2014-05-03 15:57:00 +02:00
|
|
|
stabilization.StabilizationMode.Thrust = stab_settings[3];
|
2015-01-08 03:20:47 +01:00
|
|
|
stabilization.Thrust = cmd.Thrust;
|
2014-03-02 14:45:17 +01:00
|
|
|
StabilizationDesiredSet(&stabilization);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
* @}
|
|
|
|
* @}
|
|
|
|
*/
|