diff --git a/flight/modules/Stabilization/cruisecontrol.c b/flight/modules/Stabilization/cruisecontrol.c new file mode 100644 index 000000000..9b86608c8 --- /dev/null +++ b/flight/modules/Stabilization/cruisecontrol.c @@ -0,0 +1,94 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief cruisecontrol mode + * @note This file implements the logic for a cruisecontrol + * @{ + * + * @file cruisecontrol.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Attitude stabilization module. + * + * @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 +#include +#include + +static float cruisecontrol_factor = 1.0f; + +void cruisecontrol_compute_factor(AttitudeStateData *attitude) +{ + float angleCosine; + + // get attitude state and calculate angle + // spherical right triangle + // 0 <= acosf() <= Pi + + // angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch))); + // more efficient calculation: see coordinateconversion.h Quaternion2R() + angleCosine = (attitude->q1 * attitude->q1) - (attitude->q2 * attitude->q2) - (attitude->q3 * attitude->q3) + (attitude->q4 * attitude->q4); + + // if past the cutoff angle (60 to 180 (180 means never)) + if (angleCosine < stabSettings.cruiseControl.cruise_control_max_angle_cosine) { + // -1 reversed collective, 0 zero power, or 1 normal power + // these are all unboosted + cruisecontrol_factor = stabSettings.cruiseControl.cruise_control_inverted_power_switch; + } else { + // avoid singularity + if (angleCosine > -1e-4f && angleCosine < 1e-4f) { + cruisecontrol_factor = stabSettings.cruiseControl.cruise_control_max_power_factor; + } else { + cruisecontrol_factor = boundf(1.0f / angleCosine, + -stabSettings.cruiseControl.cruise_control_max_power_factor, + stabSettings.cruiseControl.cruise_control_max_power_factor); + } + // factor in the power trim, no effect at 1.0, linear effect increases with factor + cruisecontrol_factor = (cruisecontrol_factor - 1.0f) * stabSettings.cruiseControl.cruise_control_power_trim + 1.0f; + // if inverted and they want negative boost + if (angleCosine < 0.0f && stabSettings.cruiseControl.cruise_control_inverted_power_switch == (int8_t)-1) { + cruisecontrol_factor = -cruisecontrol_factor; + // WARNING: might not go well together with power_trim adjustment above, WARNING: might have bad side effects regarding wound up integral accumulators in all 3 axis at transition stage! + // as long as thrust is getting reversed + // we may as well do pitch and yaw for a complete "invert switch" + // WARNING: currently not implementable in this control scheme, needs to be done elsewhere! + // actuatorDesired.Pitch = -actuatorDesired.Pitch; + // actuatorDesired.Yaw = -actuatorDesired.Yaw; + } + } +} + + +float cruisecontrol_apply_factor(float raw) +{ + if (stabSettings.cruiseControl.cruise_control_max_power_factor > 0.0001f) { + // don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors + if (raw > stabSettings.cruiseControl.cruise_control_min_thrust) { + // quad example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8 + // CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7 + raw = boundf((raw - stabSettings.cruiseControl.cruise_control_neutral_thrust) * cruisecontrol_factor + stabSettings.cruiseControl.cruise_control_neutral_thrust, + stabSettings.cruiseControl.cruise_control_min_thrust, + stabSettings.cruiseControl.cruise_control_max_thrust); + } + } + return raw; +} diff --git a/flight/modules/Stabilization/inc/cruisecontrol.h b/flight/modules/Stabilization/inc/cruisecontrol.h index a212cd230..fdad479dc 100644 --- a/flight/modules/Stabilization/inc/cruisecontrol.h +++ b/flight/modules/Stabilization/inc/cruisecontrol.h @@ -33,6 +33,10 @@ #ifndef CRUISECONTROL_H #define CRUISECONTROL_H +#include +#include + +void cruisecontrol_compute_factor(AttitudeStateData *attitude); float cruisecontrol_apply_factor(float raw); #endif /* CRUISECONTROL_H */ diff --git a/flight/modules/Stabilization/inc/outerloop.h b/flight/modules/Stabilization/inc/outerloop.h new file mode 100644 index 000000000..6a81deddb --- /dev/null +++ b/flight/modules/Stabilization/inc/outerloop.h @@ -0,0 +1,38 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief outerloop mode + * @note This file implements the logic for a outerloop + * @{ + * + * @file outerloop.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Attitude stabilization module. + * + * @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 + */ + +#ifndef OUTERLOOP_H +#define OUTERLOOP_H + +void stabilizationOuterloopInit(); + +#endif /* OUTERLOOP_H */ diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index dab1a616b..d9962309b 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -33,6 +33,7 @@ #ifndef STABILIZATION_H #define STABILIZATION_H +#include #include int32_t StabilizationInitialize(); @@ -40,6 +41,15 @@ int32_t StabilizationInitialize(); typedef struct { StabilizationSettingsData settings; float gyro_alpha; + struct { + float cruise_control_min_thrust; + float cruise_control_max_thrust; + float cruise_control_max_angle_cosine; + float cruise_control_max_power_factor; + float cruise_control_power_trim; + int8_t cruise_control_inverted_power_switch; // WARNING: currently -1 is not fully implemented !!! + float cruise_control_neutral_thrust; + } cruiseControl; } StabilizationData; diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 5d49d325b..44a16d733 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -8,7 +8,7 @@ * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State" * @{ * - * @file rateloop.c + * @file innerloop.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief Attitude stabilization module. * @@ -157,6 +157,9 @@ static void stabilizationInnerloopTask() actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f); } + + actuator.UpdateTime = dT * 1000; + if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { ActuatorDesiredSet(&actuator); } else { diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c new file mode 100644 index 000000000..52db1a1b5 --- /dev/null +++ b/flight/modules/Stabilization/outerloop.c @@ -0,0 +1,172 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup StabilizationModule Stabilization Module + * @brief Stabilization PID loops in an airframe type independent manner + * @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the + * PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State" + * @{ + * + * @file outerloop.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Attitude stabilization module. + * + * @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 +#include +#include +#include +#include +#include +// #include +// #include +#include +// #include +#include + +#include +#include + +// Private constants +#define STACK_SIZE_BYTES 256 + +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR + +#define UPDATE_EXPECTED (1.0f / 666.0f) +#define UPDATE_MIN 1.0e-6f +#define UPDATE_MAX 1.0f +#define UPDATE_ALPHA 1.0e-2f + +#define AXES 4 + +// Private variables +static DelayedCallbackInfo *callbackHandle; +static struct pid pids[3]; +static AttitudeStateData attitude; +static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 }; +static PiOSDeltatimeConfig timeval; + +// Private functions +static void stabilizationOuterloopTask(); +static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); +static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); + +void stabilizationOuterloopInit() +{ + RateDesiredInitialize(); + StabilizationDesiredInitialize(); + AttitudeStateInitialize(); + StabilizationStatusInitialize(); + StabilizationBankInitialize(); + PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); + + callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationOuterloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION0, STACK_SIZE_BYTES); + AttitudeStateConnectCallback(AttitudeStateUpdatedCb); + StabilizationBankConnectCallback(BankUpdatedCb); + BankUpdatedCb(NULL); +} + + +/** + * WARNING! This callback executes with critical flight control priority every + * time a gyroscope update happens do NOT put any time consuming calculations + * in this loop unless they really have to execute with every gyro update + */ +static void stabilizationOuterloopTask() +{ + RateDesiredData rateDesired; + StabilizationDesiredData stabilizationDesired; + StabilizationStatusOuterLoopData enabled; + + StabilizationDesiredGet(&stabilizationDesired); + RateDesiredGet(&rateDesired); + StabilizationStatusOuterLoopGet(&enabled); + float *stabilizationDesiredAxis = &stabilizationDesired.Roll; + float *rateDesiredAxis = &rateDesired.Roll; + int t; + // float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + + for (t = 0; t < AXES; t++) { + bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]); + previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t]; + + if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) { + if (reinit) { + pids[t].iAccumulator = 0; + } + switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { + case STABILIZATIONSTATUS_OUTERLOOP_MANUAL: + default: + rateDesiredAxis[t] = stabilizationDesiredAxis[t]; + break; + } + } else { + switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { + default: + rateDesiredAxis[t] = stabilizationDesiredAxis[t]; + break; + } + } + } + + RateDesiredSet(&rateDesired); +} + + +static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + // this does not need mutex protection as both eventdispatcher and stabi run in same callback task! + AttitudeStateGet(&attitude); + + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); +} + +static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + StabilizationBankData bank; + + StabilizationBankGet(&bank); + + // Set the roll rate PID constants + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_ROLL], bank.RollRatePID.Kp, + bank.RollRatePID.Ki, + bank.RollRatePID.Kd, + bank.RollRatePID.ILimit); + + // Set the pitch rate PID constants + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_PITCH], bank.PitchRatePID.Kp, + bank.PitchRatePID.Ki, + bank.PitchRatePID.Kd, + bank.PitchRatePID.ILimit); + + // Set the yaw rate PID constants + pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_YAW], bank.YawRatePID.Kp, + bank.YawRatePID.Ki, + bank.YawRatePID.Kd, + bank.YawRatePID.ILimit); +} + +/** + * @} + * @} + */ diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 5159a574e..b5491793b 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -53,6 +53,7 @@ #include "flightmodesettings.h" #include "taskinfo.h" + // Math libraries #include "CoordinateConversions.h" #include "pid.h" diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml index f68f8e30b..89c6923cf 100644 --- a/shared/uavobjectdefinition/stabilizationstatus.xml +++ b/shared/uavobjectdefinition/stabilizationstatus.xml @@ -3,7 +3,7 @@ Contains status information to control submodules for stabilization. - + Roll Pitch