mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-29 14:52:12 +01:00
OP-1309 Implemented control of new stabilization framework (replaces old stabilization module)
This commit is contained in:
Corvus Corax
parent
93f82652b7
commit
82d2c5a308
@ -34,12 +34,16 @@
|
||||
#define STABILIZATION_H
|
||||
|
||||
#include <openpilot.h>
|
||||
#include <pid.h>
|
||||
#include <stabilizationsettings.h>
|
||||
#include <stabilizationbank.h>
|
||||
|
||||
|
||||
int32_t StabilizationInitialize();
|
||||
|
||||
typedef struct {
|
||||
StabilizationSettingsData settings;
|
||||
StabilizationBankData stabBank;
|
||||
float gyro_alpha;
|
||||
struct {
|
||||
float cruise_control_min_thrust;
|
||||
@ -51,11 +55,14 @@ typedef struct {
|
||||
float cruise_control_neutral_thrust;
|
||||
} cruiseControl;
|
||||
float rattitude_mode_transition_stick_position;
|
||||
struct pid innerPids[3], outerPids[3];
|
||||
} StabilizationData;
|
||||
|
||||
|
||||
extern StabilizationData stabSettings;
|
||||
|
||||
#define AXES 4
|
||||
|
||||
|
||||
#endif // STABILIZATION_H
|
||||
|
||||
|
||||
@ -41,6 +41,7 @@
|
||||
#include <airspeedstate.h>
|
||||
#include <stabilizationstatus.h>
|
||||
#include <flightstatus.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <stabilizationbank.h>
|
||||
|
||||
#include <stabilization.h>
|
||||
@ -59,22 +60,17 @@
|
||||
#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 float gyro_filtered[3] = { 0, 0, 0 };
|
||||
static float axis_lock_accum[3] = { 0, 0, 0 };
|
||||
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
|
||||
static PiOSDeltatimeConfig timeval;
|
||||
static float speedScaleFactor = 1.0f;
|
||||
static StabilizationBankData stabBank;
|
||||
|
||||
// Private functions
|
||||
static void stabilizationInnerloopTask();
|
||||
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||
#ifdef REVOLUTION
|
||||
static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||
#endif
|
||||
@ -86,7 +82,7 @@ void stabilizationInnerloopInit()
|
||||
GyroStateInitialize();
|
||||
StabilizationStatusInitialize();
|
||||
FlightStatusInitialize();
|
||||
StabilizationBankInitialize();
|
||||
ManualControlCommandInitialize();
|
||||
#ifdef REVOLUTION
|
||||
AirspeedStateInitialize();
|
||||
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
|
||||
@ -95,8 +91,6 @@ void stabilizationInnerloopInit()
|
||||
|
||||
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationInnerloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION1, STACK_SIZE_BYTES);
|
||||
GyroStateConnectCallback(GyroStateUpdatedCb);
|
||||
StabilizationBankConnectCallback(BankUpdatedCb);
|
||||
BankUpdatedCb(NULL);
|
||||
}
|
||||
|
||||
|
||||
@ -128,7 +122,7 @@ static void stabilizationInnerloopTask()
|
||||
|
||||
if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
|
||||
if (reinit) {
|
||||
pids[t].iAccumulator = 0;
|
||||
stabSettings.innerPids[t].iAccumulator = 0;
|
||||
}
|
||||
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
|
||||
case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
|
||||
@ -136,8 +130,8 @@ static void stabilizationInnerloopTask()
|
||||
break;
|
||||
case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING:
|
||||
rate[t] = boundf(rate[t],
|
||||
-cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t],
|
||||
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t]
|
||||
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
|
||||
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
|
||||
);
|
||||
stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit);
|
||||
break;
|
||||
@ -156,10 +150,10 @@ static void stabilizationInnerloopTask()
|
||||
case STABILIZATIONSTATUS_INNERLOOP_RATE:
|
||||
// limit rate to maximum configured limits (once here instead of 5 times in outer loop)
|
||||
rate[t] = boundf(rate[t],
|
||||
-cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t],
|
||||
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[t]
|
||||
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
|
||||
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
|
||||
);
|
||||
actuatorDesiredAxis[t] = pid_apply_setpoint(&pids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
|
||||
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
|
||||
break;
|
||||
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
|
||||
default:
|
||||
@ -191,6 +185,19 @@ static void stabilizationInnerloopTask()
|
||||
previous_mode[t] = 255;
|
||||
}
|
||||
}
|
||||
{
|
||||
uint8_t armed;
|
||||
FlightStatusArmedGet(&armed);
|
||||
float throttleDesired;
|
||||
ManualControlCommandThrottleGet(&throttleDesired);
|
||||
if (armed != FLIGHTSTATUS_ARMED_ARMED ||
|
||||
((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
|
||||
// Force all axes to reinitialize when engaged
|
||||
for (t = 0; t < AXES; t++) {
|
||||
previous_mode[t] = 255;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -207,29 +214,6 @@ static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||
}
|
||||
|
||||
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
StabilizationBankGet(&stabBank);
|
||||
|
||||
// Set the roll rate PID constants
|
||||
pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_ROLL], stabBank.RollRatePID.Kp,
|
||||
stabBank.RollRatePID.Ki,
|
||||
stabBank.RollRatePID.Kd,
|
||||
stabBank.RollRatePID.ILimit);
|
||||
|
||||
// Set the pitch rate PID constants
|
||||
pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_PITCH], stabBank.PitchRatePID.Kp,
|
||||
stabBank.PitchRatePID.Ki,
|
||||
stabBank.PitchRatePID.Kd,
|
||||
stabBank.PitchRatePID.ILimit);
|
||||
|
||||
// Set the yaw rate PID constants
|
||||
pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_YAW], stabBank.YawRatePID.Kp,
|
||||
stabBank.YawRatePID.Ki,
|
||||
stabBank.YawRatePID.Kd,
|
||||
stabBank.YawRatePID.ILimit);
|
||||
}
|
||||
|
||||
#ifdef REVOLUTION
|
||||
static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
|
||||
@ -39,6 +39,8 @@
|
||||
#include <stabilizationdesired.h>
|
||||
#include <attitudestate.h>
|
||||
#include <stabilizationstatus.h>
|
||||
#include <flightstatus.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <stabilizationbank.h>
|
||||
|
||||
|
||||
@ -57,13 +59,9 @@
|
||||
#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 StabilizationBankData stabBank;
|
||||
|
||||
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
|
||||
static PiOSDeltatimeConfig timeval;
|
||||
@ -71,7 +69,6 @@ static PiOSDeltatimeConfig timeval;
|
||||
// Private functions
|
||||
static void stabilizationOuterloopTask();
|
||||
static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||
|
||||
void stabilizationOuterloopInit()
|
||||
{
|
||||
@ -79,13 +76,13 @@ void stabilizationOuterloopInit()
|
||||
StabilizationDesiredInitialize();
|
||||
AttitudeStateInitialize();
|
||||
StabilizationStatusInitialize();
|
||||
StabilizationBankInitialize();
|
||||
FlightStatusInitialize();
|
||||
ManualControlCommandInitialize();
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
@ -149,20 +146,20 @@ static void stabilizationOuterloopTask()
|
||||
|
||||
if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
|
||||
if (reinit) {
|
||||
pids[t].iAccumulator = 0;
|
||||
stabSettings.outerPids[t].iAccumulator = 0;
|
||||
}
|
||||
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
|
||||
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
|
||||
rateDesiredAxis[t] = pid_apply(&pids[t], local_error[t], dT);
|
||||
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
|
||||
{
|
||||
float stickinput[3];
|
||||
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabBank.RollMax, -1.0f, 1.0f);
|
||||
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabBank.PitchMax, -1.0f, 1.0f);
|
||||
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabBank.YawMax, -1.0f, 1.0f);
|
||||
float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[t];
|
||||
rateDesiredAxis[t] = pid_apply(&pids[t], local_error[t], dT);
|
||||
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
|
||||
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
|
||||
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
|
||||
float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t];
|
||||
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
|
||||
// Compute the weighted average rate desired
|
||||
// Using max() rather than sqrt() for cpu speed;
|
||||
// - this makes the stick region into a square;
|
||||
@ -218,7 +215,7 @@ static void stabilizationOuterloopTask()
|
||||
// That would be changed to Attitude mode max angle affecting Kp
|
||||
// Also does not take dT into account
|
||||
{
|
||||
float rate_input = cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[t] * stabilizationDesiredAxis[t] / cast_struct_to_array(stabBank, stabBank.RollMax)[t];
|
||||
float rate_input = cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t] * stabilizationDesiredAxis[t] / cast_struct_to_array(stabSettings.stabBank, stabSettings.stabBank.RollMax)[t];
|
||||
float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
|
||||
weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
|
||||
|
||||
@ -241,6 +238,19 @@ static void stabilizationOuterloopTask()
|
||||
}
|
||||
|
||||
RateDesiredSet(&rateDesired);
|
||||
{
|
||||
uint8_t armed;
|
||||
FlightStatusArmedGet(&armed);
|
||||
float throttleDesired;
|
||||
ManualControlCommandThrottleGet(&throttleDesired);
|
||||
if (armed != FLIGHTSTATUS_ARMED_ARMED ||
|
||||
((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
|
||||
// Force all axes to reinitialize when engaged
|
||||
for (t = 0; t < AXES; t++) {
|
||||
previous_mode[t] = 255;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// update cruisecontrol based on attitude
|
||||
cruisecontrol_compute_factor(&attitudeState);
|
||||
@ -255,29 +265,6 @@ static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||
}
|
||||
|
||||
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
StabilizationBankGet(&stabBank);
|
||||
|
||||
// Set the roll rate PID constants
|
||||
pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_ROLL], stabBank.RollRatePID.Kp,
|
||||
stabBank.RollRatePID.Ki,
|
||||
stabBank.RollRatePID.Kd,
|
||||
stabBank.RollRatePID.ILimit);
|
||||
|
||||
// Set the pitch rate PID constants
|
||||
pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_PITCH], stabBank.PitchRatePID.Kp,
|
||||
stabBank.PitchRatePID.Ki,
|
||||
stabBank.PitchRatePID.Kd,
|
||||
stabBank.PitchRatePID.ILimit);
|
||||
|
||||
// Set the yaw rate PID constants
|
||||
pid_configure(&pids[STABILIZATIONSTATUS_OUTERLOOP_YAW], stabBank.YawRatePID.Kp,
|
||||
stabBank.YawRatePID.Ki,
|
||||
stabBank.YawRatePID.Kd,
|
||||
stabBank.YawRatePID.ILimit);
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
@ -33,125 +33,56 @@
|
||||
|
||||
#include <openpilot.h>
|
||||
#include <pios_struct_helper.h>
|
||||
#include "stabilization.h"
|
||||
#include "stabilizationsettings.h"
|
||||
#include "stabilizationbank.h"
|
||||
#include "stabilizationsettingsbank1.h"
|
||||
#include "stabilizationsettingsbank2.h"
|
||||
#include "stabilizationsettingsbank3.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "ratedesired.h"
|
||||
#include "relaytuning.h"
|
||||
#include "relaytuningsettings.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "attitudestate.h"
|
||||
#include "airspeedstate.h"
|
||||
#include "gyrostate.h"
|
||||
#include "flightstatus.h"
|
||||
#include "manualcontrolsettings.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "flightmodesettings.h"
|
||||
#include "taskinfo.h"
|
||||
#include <pid.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <flightmodesettings.h>
|
||||
#include <stabilizationsettings.h>
|
||||
#include <stabilizationdesired.h>
|
||||
#include <stabilizationstatus.h>
|
||||
#include <stabilizationbank.h>
|
||||
#include <stabilizationsettingsbank1.h>
|
||||
#include <stabilizationsettingsbank2.h>
|
||||
#include <stabilizationsettingsbank3.h>
|
||||
#include <relaytuning.h>
|
||||
#include <relaytuningsettings.h>
|
||||
#include <ratedesired.h>
|
||||
#include <sin_lookup.h>
|
||||
#include <stabilization.h>
|
||||
#include <innerloop.h>
|
||||
#include <outerloop.h>
|
||||
|
||||
|
||||
// Math libraries
|
||||
#include "CoordinateConversions.h"
|
||||
#include "pid.h"
|
||||
#include "sin_lookup.h"
|
||||
|
||||
// Includes for various stabilization algorithms
|
||||
#include "relay_tuning.h"
|
||||
#include "virtualflybar.h"
|
||||
|
||||
// Includes for various stabilization algorithms
|
||||
#include "relay_tuning.h"
|
||||
|
||||
// Private constants
|
||||
#define UPDATE_EXPECTED (1.0f / 666.0f)
|
||||
#define UPDATE_MIN 1.0e-6f
|
||||
#define UPDATE_MAX 1.0f
|
||||
#define UPDATE_ALPHA 1.0e-2f
|
||||
|
||||
typedef enum { ROLL = 0, PITCH = 1, YAW = 2, MAX_AXES = 3 } blaenum;
|
||||
|
||||
#define MAX_QUEUE_SIZE 1
|
||||
|
||||
#if defined(PIOS_STABILIZATION_STACK_SIZE)
|
||||
#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE
|
||||
#else
|
||||
#define STACK_SIZE_BYTES 860
|
||||
#endif
|
||||
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority
|
||||
#define FAILSAFE_TIMEOUT_MS 30
|
||||
|
||||
// The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode
|
||||
// The PID_RATE set is used by the attitude portion of Attitude mode
|
||||
enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX };
|
||||
enum { RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET };
|
||||
enum { ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET };
|
||||
// Public variables
|
||||
StabilizationData stabSettings;
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
static StabilizationSettingsData settings;
|
||||
static xQueueHandle queue;
|
||||
float gyro_alpha = 0;
|
||||
float axis_lock_accum[3] = { 0, 0, 0 };
|
||||
uint8_t max_axis_lock = 0;
|
||||
uint8_t max_axislock_rate = 0;
|
||||
float weak_leveling_kp = 0;
|
||||
uint8_t weak_leveling_max = 0;
|
||||
bool lowThrottleZeroIntegral;
|
||||
float vbar_decay = 0.991f;
|
||||
struct pid pids[PID_MAX];
|
||||
|
||||
int cur_flight_mode = -1;
|
||||
|
||||
static float rattitude_mode_transition_stick_position;
|
||||
static float cruise_control_min_thrust;
|
||||
static float cruise_control_max_thrust;
|
||||
static uint8_t cruise_control_max_angle;
|
||||
static float cruise_control_max_power_factor;
|
||||
static float cruise_control_power_trim;
|
||||
static int8_t cruise_control_inverted_power_switch;
|
||||
static float cruise_control_neutral_thrust;
|
||||
static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
|
||||
static int cur_flight_mode = -1;
|
||||
|
||||
// Private functions
|
||||
static void stabilizationTask(void *parameters);
|
||||
static void ZeroPids(void);
|
||||
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
||||
static void BankUpdatedCb(UAVObjEvent *ev);
|
||||
static void SettingsBankUpdatedCb(UAVObjEvent *ev);
|
||||
static void FlightModeSwitchUpdatedCb(UAVObjEvent *ev);
|
||||
static void StabilizationDesiredUpdatedCb(UAVObjEvent *ev);
|
||||
|
||||
/**
|
||||
* Module initialization
|
||||
*/
|
||||
int32_t StabilizationStart()
|
||||
{
|
||||
// Initialize variables
|
||||
// Create object queue
|
||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||
|
||||
// Listen for updates.
|
||||
// AttitudeStateConnectQueue(queue);
|
||||
GyroStateConnectQueue(queue);
|
||||
|
||||
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
|
||||
SettingsUpdatedCb(StabilizationSettingsHandle());
|
||||
|
||||
ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb);
|
||||
StabilizationBankConnectCallback(BankUpdatedCb);
|
||||
|
||||
StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
|
||||
StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
|
||||
StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
|
||||
StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb);
|
||||
SettingsUpdatedCb(StabilizationSettingsHandle());
|
||||
StabilizationDesiredUpdatedCb(StabilizationDesiredHandle());
|
||||
FlightModeSwitchUpdatedCb(ManualControlCommandHandle());
|
||||
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle);
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
|
||||
// PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
@ -162,642 +93,186 @@ int32_t StabilizationStart()
|
||||
int32_t StabilizationInitialize()
|
||||
{
|
||||
// Initialize variables
|
||||
ManualControlCommandInitialize();
|
||||
ManualControlSettingsInitialize();
|
||||
FlightStatusInitialize();
|
||||
StabilizationDesiredInitialize();
|
||||
StabilizationSettingsInitialize();
|
||||
StabilizationStatusInitialize();
|
||||
StabilizationBankInitialize();
|
||||
StabilizationSettingsBank1Initialize();
|
||||
StabilizationSettingsBank2Initialize();
|
||||
StabilizationSettingsBank3Initialize();
|
||||
ActuatorDesiredInitialize();
|
||||
#ifdef DIAG_RATEDESIRED
|
||||
RateDesiredInitialize();
|
||||
#endif
|
||||
#ifdef REVOLUTION
|
||||
AirspeedStateInitialize();
|
||||
#endif
|
||||
ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch
|
||||
// Code required for relay tuning
|
||||
sin_lookup_initalize();
|
||||
RelayTuningSettingsInitialize();
|
||||
RelayTuningInitialize();
|
||||
|
||||
stabilizationOuterloopInit();
|
||||
stabilizationInnerloopInit();
|
||||
pid_zero(&stabSettings.outerPids[0]);
|
||||
pid_zero(&stabSettings.outerPids[1]);
|
||||
pid_zero(&stabSettings.outerPids[2]);
|
||||
pid_zero(&stabSettings.innerPids[0]);
|
||||
pid_zero(&stabSettings.innerPids[1]);
|
||||
pid_zero(&stabSettings.innerPids[2]);
|
||||
return 0;
|
||||
}
|
||||
|
||||
MODULE_INITCALL(StabilizationInitialize, StabilizationStart);
|
||||
|
||||
/**
|
||||
* Module task
|
||||
*/
|
||||
static void stabilizationTask(__attribute__((unused)) void *parameters)
|
||||
static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
PiOSDeltatimeConfig timeval;
|
||||
|
||||
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
|
||||
|
||||
ActuatorDesiredData actuatorDesired;
|
||||
StabilizationDesiredData stabDesired;
|
||||
float throttleDesired;
|
||||
RateDesiredData rateDesired;
|
||||
AttitudeStateData attitudeState;
|
||||
GyroStateData gyroStateData;
|
||||
FlightStatusData flightStatus;
|
||||
StabilizationBankData stabBank;
|
||||
|
||||
|
||||
#ifdef REVOLUTION
|
||||
AirspeedStateData airspeedState;
|
||||
#endif
|
||||
|
||||
SettingsUpdatedCb((UAVObjEvent *)NULL);
|
||||
|
||||
// Main task loop
|
||||
ZeroPids();
|
||||
while (1) {
|
||||
float dT;
|
||||
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
|
||||
#endif
|
||||
|
||||
// Wait until the Gyro object is updated, if a timeout then go to failsafe
|
||||
if (xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
|
||||
continue;
|
||||
}
|
||||
|
||||
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
|
||||
FlightStatusGet(&flightStatus);
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
ManualControlCommandThrottleGet(&throttleDesired);
|
||||
AttitudeStateGet(&attitudeState);
|
||||
GyroStateGet(&gyroStateData);
|
||||
StabilizationBankGet(&stabBank);
|
||||
#ifdef DIAG_RATEDESIRED
|
||||
RateDesiredGet(&rateDesired);
|
||||
#endif
|
||||
uint8_t flight_mode_switch_position;
|
||||
ManualControlCommandFlightModeSwitchPositionGet(&flight_mode_switch_position);
|
||||
|
||||
if (cur_flight_mode != flight_mode_switch_position) {
|
||||
cur_flight_mode = flight_mode_switch_position;
|
||||
SettingsBankUpdatedCb(NULL);
|
||||
}
|
||||
|
||||
#ifdef REVOLUTION
|
||||
float speedScaleFactor;
|
||||
// Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
|
||||
AirspeedStateGet(&airspeedState);
|
||||
if (settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
|
||||
// feature has been turned off
|
||||
speedScaleFactor = 1.0f;
|
||||
} else {
|
||||
// scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
|
||||
speedScaleFactor = (settings.ScaleToAirspeed * settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed);
|
||||
if (speedScaleFactor < settings.ScaleToAirspeedLimits.Min) {
|
||||
speedScaleFactor = settings.ScaleToAirspeedLimits.Min;
|
||||
}
|
||||
if (speedScaleFactor > settings.ScaleToAirspeedLimits.Max) {
|
||||
speedScaleFactor = settings.ScaleToAirspeedLimits.Max;
|
||||
}
|
||||
}
|
||||
#else
|
||||
const float speedScaleFactor = 1.0f;
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_QUATERNION_STABILIZATION)
|
||||
// Quaternion calculation of error in each axis. Uses more memory.
|
||||
float rpy_desired[3];
|
||||
float q_desired[4];
|
||||
float q_error[4];
|
||||
float local_error[3];
|
||||
|
||||
// Essentially zero errors for anything in rate or none
|
||||
if (stabDesired.StabilizationMode.Roll == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
|
||||
rpy_desired[0] = stabDesired.Roll;
|
||||
} else {
|
||||
rpy_desired[0] = attitudeState.Roll;
|
||||
}
|
||||
|
||||
if (stabDesired.StabilizationMode.Pitch == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
|
||||
rpy_desired[1] = stabDesired.Pitch;
|
||||
} else {
|
||||
rpy_desired[1] = attitudeState.Pitch;
|
||||
}
|
||||
|
||||
if (stabDesired.StabilizationMode.Yaw == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
|
||||
rpy_desired[2] = stabDesired.Yaw;
|
||||
} else {
|
||||
rpy_desired[2] = attitudeState.Yaw;
|
||||
}
|
||||
|
||||
RPY2Quaternion(rpy_desired, q_desired);
|
||||
quat_inverse(q_desired);
|
||||
quat_mult(q_desired, &attitudeState.q1, q_error);
|
||||
quat_inverse(q_error);
|
||||
Quaternion2RPY(q_error, local_error);
|
||||
|
||||
#else /* if defined(PIOS_QUATERNION_STABILIZATION) */
|
||||
// Simpler algorithm for CC, less memory
|
||||
float local_error[3] = { stabDesired.Roll - attitudeState.Roll,
|
||||
stabDesired.Pitch - attitudeState.Pitch,
|
||||
stabDesired.Yaw - attitudeState.Yaw };
|
||||
// find shortest way
|
||||
float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
|
||||
if (modulo < 0) {
|
||||
local_error[2] = modulo + 180.0f;
|
||||
} else {
|
||||
local_error[2] = modulo - 180.0f;
|
||||
}
|
||||
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
|
||||
|
||||
float gyro_filtered[3];
|
||||
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyroStateData.x * (1 - gyro_alpha);
|
||||
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyroStateData.y * (1 - gyro_alpha);
|
||||
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyroStateData.z * (1 - gyro_alpha);
|
||||
|
||||
float *stabDesiredAxis = &stabDesired.Roll;
|
||||
float *actuatorDesiredAxis = &actuatorDesired.Roll;
|
||||
float *rateDesiredAxis = &rateDesired.Roll;
|
||||
|
||||
ActuatorDesiredGet(&actuatorDesired);
|
||||
|
||||
// A flag to track which stabilization mode each axis is in
|
||||
static uint8_t previous_mode[MAX_AXES] = { 255, 255, 255 };
|
||||
bool error = false;
|
||||
|
||||
// Run the selected stabilization algorithm on each axis:
|
||||
for (uint8_t i = 0; i < MAX_AXES; i++) {
|
||||
// Check whether this axis mode needs to be reinitialized
|
||||
bool reinit = (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i] != previous_mode[i]);
|
||||
previous_mode[i] = cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i];
|
||||
|
||||
// Apply the selected control law
|
||||
switch (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i]) {
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
|
||||
if (reinit) {
|
||||
pids[PID_RATE_ROLL + i].iAccumulator = 0;
|
||||
}
|
||||
|
||||
// Store to rate desired variable for storing to UAVO
|
||||
rateDesiredAxis[i] =
|
||||
boundf(stabDesiredAxis[i], -cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
|
||||
|
||||
// Compute the inner loop
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
|
||||
|
||||
break;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
|
||||
if (reinit) {
|
||||
pids[PID_ROLL + i].iAccumulator = 0;
|
||||
pids[PID_RATE_ROLL + i].iAccumulator = 0;
|
||||
}
|
||||
|
||||
// Compute the outer loop
|
||||
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
|
||||
rateDesiredAxis[i] = boundf(rateDesiredAxis[i],
|
||||
-cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i],
|
||||
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
|
||||
|
||||
// Compute the inner loop
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
|
||||
|
||||
break;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
|
||||
// A parameterization from Attitude mode at center stick
|
||||
// - to Rate mode at full stick
|
||||
// This is done by parameterizing to use the rotation rate that Attitude mode
|
||||
// - would use at center stick to using the rotation rate that Rate mode
|
||||
// - would use at full stick in a weighted average sort of way.
|
||||
{
|
||||
if (reinit) {
|
||||
pids[PID_ROLL + i].iAccumulator = 0;
|
||||
pids[PID_RATE_ROLL + i].iAccumulator = 0;
|
||||
}
|
||||
|
||||
// Compute what Rate mode would give for this stick angle's rate
|
||||
// Save Rate's rate in a temp for later merging with Attitude's rate
|
||||
float rateDesiredAxisRate;
|
||||
rateDesiredAxisRate = boundf(stabDesiredAxis[i], -1.0f, 1.0f)
|
||||
* cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i];
|
||||
|
||||
// Compute what Attitude mode would give for this stick angle's rate
|
||||
|
||||
// stabDesired for this mode is [-1.0f,+1.0f]
|
||||
// - multiply by Attitude mode max angle to get desired angle
|
||||
// - subtract off the actual angle to get the angle error
|
||||
// This is what local_error[] holds for Attitude mode
|
||||
float attitude_error = stabDesiredAxis[i]
|
||||
* cast_struct_to_array(stabBank.RollMax, stabBank.RollMax)[i]
|
||||
- cast_struct_to_array(attitudeState.Roll, attitudeState.Roll)[i];
|
||||
|
||||
// Compute the outer loop just like Attitude mode does
|
||||
float rateDesiredAxisAttitude;
|
||||
rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT);
|
||||
rateDesiredAxisAttitude = boundf(rateDesiredAxisAttitude,
|
||||
-cast_struct_to_array(stabBank.ManualRate,
|
||||
stabBank.ManualRate.Roll)[i],
|
||||
cast_struct_to_array(stabBank.ManualRate,
|
||||
stabBank.ManualRate.Roll)[i]);
|
||||
|
||||
// Compute the weighted average rate desired
|
||||
// Using max() rather than sqrt() for cpu speed;
|
||||
// - this makes the stick region into a square;
|
||||
// - this is a feature!
|
||||
// - hold a roll angle and add just pitch without the stick sensitivity changing
|
||||
// magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch);
|
||||
float magnitude;
|
||||
magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch));
|
||||
|
||||
// modify magnitude to move the Att to Rate transition to the place
|
||||
// specified by the user
|
||||
// we are looking for where the stick angle == transition angle
|
||||
// and the Att rate equals the Rate rate
|
||||
// that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
|
||||
// == Rate x StickAngle [Rate pulling up according to stick angle]
|
||||
// * StickAngle [X Ratt proportion]
|
||||
// so 1-x == x*x or x*x+x-1=0 where xE(0,1)
|
||||
// (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
|
||||
// and quadratic formula says that is 0.618033989f
|
||||
// I tested 14.01 and came up with .61 without even remembering this number
|
||||
// I thought that moving the P,I, and maxangle terms around would change this value
|
||||
// and that I would have to take these into account, but varying
|
||||
// all P's and I's by factors of 1/2 to 2 didn't change it noticeably
|
||||
// and varying maxangle from 4 to 120 didn't either.
|
||||
// so for now I'm not taking these into account
|
||||
// while working with this, it occurred to me that Attitude mode,
|
||||
// set up with maxangle=190 would be similar to Ratt, and it is.
|
||||
#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
|
||||
|
||||
// the following assumes the transition would otherwise be at 0.618033989f
|
||||
// and THAT assumes that Att ramps up to max roll rate
|
||||
// when a small number of degrees off of where it should be
|
||||
|
||||
// if below the transition angle (still in attitude mode)
|
||||
// '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
|
||||
if (magnitude <= rattitude_mode_transition_stick_position) {
|
||||
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position;
|
||||
} else {
|
||||
magnitude = (magnitude - rattitude_mode_transition_stick_position)
|
||||
* (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
|
||||
/ (1.0f - rattitude_mode_transition_stick_position)
|
||||
+ STICK_VALUE_AT_MODE_TRANSITION;
|
||||
}
|
||||
rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
|
||||
+ magnitude * rateDesiredAxisRate;
|
||||
|
||||
// Compute the inner loop for the averaged rate
|
||||
// actuatorDesiredAxis[i] is the weighted average
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor,
|
||||
rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
|
||||
// Store for debugging output
|
||||
rateDesiredAxis[i] = stabDesiredAxis[i];
|
||||
|
||||
// Run a virtual flybar stabilization algorithm on this axis
|
||||
stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings);
|
||||
|
||||
break;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
|
||||
// FIXME: local_error[] is rate - attitude for Weak Leveling
|
||||
// The only ramifications are:
|
||||
// Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
|
||||
// Changing Rate mode max rate currently requires a change to Kp
|
||||
// That would be changed to Attitude mode max angle affecting Kp
|
||||
// Also does not take dT into account
|
||||
{
|
||||
if (reinit) {
|
||||
pids[PID_RATE_ROLL + i].iAccumulator = 0;
|
||||
}
|
||||
|
||||
float weak_leveling = local_error[i] * weak_leveling_kp;
|
||||
weak_leveling = boundf(weak_leveling, -weak_leveling_max, weak_leveling_max);
|
||||
|
||||
// Compute desired rate as input biased towards leveling
|
||||
rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling;
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
|
||||
if (reinit) {
|
||||
pids[PID_RATE_ROLL + i].iAccumulator = 0;
|
||||
}
|
||||
|
||||
if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) {
|
||||
// While getting strong commands act like rate mode
|
||||
rateDesiredAxis[i] = stabDesiredAxis[i];
|
||||
axis_lock_accum[i] = 0;
|
||||
} else {
|
||||
// For weaker commands or no command simply attitude lock (almost) on no gyro change
|
||||
axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT;
|
||||
axis_lock_accum[i] = boundf(axis_lock_accum[i], -max_axis_lock, max_axis_lock);
|
||||
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
|
||||
}
|
||||
|
||||
rateDesiredAxis[i] = boundf(rateDesiredAxis[i],
|
||||
-cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i],
|
||||
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
|
||||
|
||||
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
|
||||
|
||||
break;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
|
||||
// Store to rate desired variable for storing to UAVO
|
||||
rateDesiredAxis[i] = boundf(stabDesiredAxis[i],
|
||||
-cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i],
|
||||
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
|
||||
|
||||
// Run the relay controller which also estimates the oscillation parameters
|
||||
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
|
||||
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
|
||||
|
||||
break;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
|
||||
if (reinit) {
|
||||
pids[PID_ROLL + i].iAccumulator = 0;
|
||||
}
|
||||
|
||||
// Compute the outer loop like attitude mode
|
||||
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
|
||||
rateDesiredAxis[i] = boundf(rateDesiredAxis[i],
|
||||
-cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i],
|
||||
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
|
||||
|
||||
// Run the relay controller which also estimates the oscillation parameters
|
||||
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
|
||||
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
|
||||
|
||||
break;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
|
||||
actuatorDesiredAxis[i] = boundf(stabDesiredAxis[i], -1.0f, 1.0f);
|
||||
break;
|
||||
default:
|
||||
error = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) {
|
||||
stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT);
|
||||
}
|
||||
|
||||
#ifdef DIAG_RATEDESIRED
|
||||
RateDesiredSet(&rateDesired);
|
||||
#endif
|
||||
|
||||
// Save dT
|
||||
actuatorDesired.UpdateTime = dT * 1000;
|
||||
actuatorDesired.Thrust = stabDesired.Thrust;
|
||||
|
||||
// modify thrust according to 1/cos(bank angle)
|
||||
// to maintain same altitdue with changing bank angle
|
||||
// but without manually adjusting thrust
|
||||
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
|
||||
if (flight_mode_switch_position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
|
||||
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
|
||||
&& cruise_control_max_power_factor > 0.0001f) {
|
||||
static uint8_t toggle;
|
||||
static float factor;
|
||||
float angle;
|
||||
// get attitude state and calculate angle
|
||||
// do it every 8th iteration to save CPU
|
||||
if ((toggle++ & 7) == 0) {
|
||||
// spherical right triangle
|
||||
// 0 <= acosf() <= Pi
|
||||
angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch)));
|
||||
// if past the cutoff angle (60 to 180 (180 means never))
|
||||
if (angle > cruise_control_max_angle) {
|
||||
// -1 reversed collective, 0 zero power, or 1 normal power
|
||||
// these are all unboosted
|
||||
factor = cruise_control_inverted_power_switch;
|
||||
} else {
|
||||
// avoid singularity
|
||||
if (angle > 89.999f && angle < 90.001f) {
|
||||
factor = cruise_control_max_power_factor;
|
||||
} else {
|
||||
factor = 1.0f / fabsf(cos_lookup_deg(angle));
|
||||
if (factor > cruise_control_max_power_factor) {
|
||||
factor = cruise_control_max_power_factor;
|
||||
}
|
||||
}
|
||||
// factor in the power trim, no effect at 1.0, linear effect increases with factor
|
||||
factor = (factor - 1.0f) * cruise_control_power_trim + 1.0f;
|
||||
// if inverted and they want negative boost
|
||||
if (angle > 90.0f && cruise_control_inverted_power_switch == (int8_t)-1) {
|
||||
factor = -factor;
|
||||
// as long as thrust is getting reversed
|
||||
// we may as well do pitch and yaw for a complete "invert switch"
|
||||
actuatorDesired.Pitch = -actuatorDesired.Pitch;
|
||||
actuatorDesired.Yaw = -actuatorDesired.Yaw;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// also don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors
|
||||
if (actuatorDesired.Thrust > 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
|
||||
actuatorDesired.Thrust = (actuatorDesired.Thrust - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust;
|
||||
if (actuatorDesired.Thrust > cruise_control_max_thrust) {
|
||||
actuatorDesired.Thrust = cruise_control_max_thrust;
|
||||
} else if (actuatorDesired.Thrust < cruise_control_min_thrust) {
|
||||
actuatorDesired.Thrust = cruise_control_min_thrust;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
|
||||
ActuatorDesiredSet(&actuatorDesired);
|
||||
} else {
|
||||
// Force all axes to reinitialize when engaged
|
||||
for (uint8_t i = 0; i < MAX_AXES; i++) {
|
||||
previous_mode[i] = 255;
|
||||
}
|
||||
}
|
||||
|
||||
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
|
||||
(lowThrottleZeroIntegral && throttleDesired < 0)) {
|
||||
// Force all axes to reinitialize when engaged
|
||||
for (uint8_t i = 0; i < MAX_AXES; i++) {
|
||||
previous_mode[i] = 255;
|
||||
}
|
||||
}
|
||||
|
||||
// Clear or set alarms. Done like this to prevent toggline each cycle
|
||||
// and hammering system alarms
|
||||
if (error) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
|
||||
} else {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
|
||||
StabilizationStatusData status;
|
||||
StabilizationDesiredStabilizationModeData mode;
|
||||
int t;
|
||||
|
||||
StabilizationDesiredStabilizationModeGet(&mode);
|
||||
for (t = 0; t < AXES; t++) {
|
||||
switch (cast_struct_to_array(mode, mode.Roll)[t]) {
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_VERTICALVELOCITY:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
|
||||
break;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
|
||||
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
|
||||
break;
|
||||
}
|
||||
}
|
||||
StabilizationStatusSet(&status);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Clear the accumulators and derivatives for all the axes
|
||||
*/
|
||||
static void ZeroPids(void)
|
||||
static void FlightModeSwitchUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
for (uint32_t i = 0; i < PID_MAX; i++) {
|
||||
pid_zero(&pids[i]);
|
||||
}
|
||||
uint8_t fm;
|
||||
|
||||
ManualControlCommandFlightModeSwitchPositionGet(&fm);
|
||||
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
axis_lock_accum[i] = 0.0f;
|
||||
if (fm == cur_flight_mode) {
|
||||
return;
|
||||
}
|
||||
cur_flight_mode = fm;
|
||||
SettingsBankUpdatedCb(NULL);
|
||||
}
|
||||
|
||||
|
||||
static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
|
||||
return;
|
||||
}
|
||||
if ((ev) && ((settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
|
||||
(settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
|
||||
(settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
|
||||
settings.FlightModeMap[cur_flight_mode] > 2)) {
|
||||
if ((ev) && ((stabSettings.settings.FlightModeMap[cur_flight_mode] == 0 && ev->obj != StabilizationSettingsBank1Handle()) ||
|
||||
(stabSettings.settings.FlightModeMap[cur_flight_mode] == 1 && ev->obj != StabilizationSettingsBank2Handle()) ||
|
||||
(stabSettings.settings.FlightModeMap[cur_flight_mode] == 2 && ev->obj != StabilizationSettingsBank3Handle()) ||
|
||||
stabSettings.settings.FlightModeMap[cur_flight_mode] > 2)) {
|
||||
return;
|
||||
}
|
||||
|
||||
StabilizationBankData bank;
|
||||
|
||||
switch (settings.FlightModeMap[cur_flight_mode]) {
|
||||
switch (stabSettings.settings.FlightModeMap[cur_flight_mode]) {
|
||||
case 0:
|
||||
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
|
||||
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&stabSettings.stabBank);
|
||||
break;
|
||||
|
||||
case 1:
|
||||
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
|
||||
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&stabSettings.stabBank);
|
||||
break;
|
||||
|
||||
case 2:
|
||||
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
|
||||
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&stabSettings.stabBank);
|
||||
break;
|
||||
}
|
||||
StabilizationBankSet(&bank);
|
||||
StabilizationBankSet(&stabSettings.stabBank);
|
||||
}
|
||||
|
||||
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
StabilizationBankData bank;
|
||||
|
||||
StabilizationBankGet(&bank);
|
||||
|
||||
// this code will be needed if any other modules alter stabilizationbank
|
||||
/*
|
||||
StabilizationBankData curBank;
|
||||
if(flight_mode < 0) return;
|
||||
|
||||
switch(cast_struct_to_array(settings.FlightModeMap, settings.FlightModeMap.Stabilized1)[flight_mode])
|
||||
{
|
||||
case 0:
|
||||
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *) &curBank);
|
||||
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
|
||||
{
|
||||
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *) &bank);
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *) &curBank);
|
||||
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
|
||||
{
|
||||
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *) &bank);
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *) &curBank);
|
||||
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
|
||||
{
|
||||
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *) &bank);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
return; //invalid bank number
|
||||
}
|
||||
*/
|
||||
|
||||
StabilizationBankGet(&stabSettings.stabBank);
|
||||
|
||||
// Set the roll rate PID constants
|
||||
pid_configure(&pids[PID_RATE_ROLL], bank.RollRatePID.Kp,
|
||||
bank.RollRatePID.Ki,
|
||||
bank.RollRatePID.Kd,
|
||||
bank.RollRatePID.ILimit);
|
||||
pid_configure(&stabSettings.innerPids[0], stabSettings.stabBank.RollRatePID.Kp,
|
||||
stabSettings.stabBank.RollRatePID.Ki,
|
||||
stabSettings.stabBank.RollRatePID.Kd,
|
||||
stabSettings.stabBank.RollRatePID.ILimit);
|
||||
|
||||
// Set the pitch rate PID constants
|
||||
pid_configure(&pids[PID_RATE_PITCH], bank.PitchRatePID.Kp,
|
||||
bank.PitchRatePID.Ki,
|
||||
bank.PitchRatePID.Kd,
|
||||
bank.PitchRatePID.ILimit);
|
||||
pid_configure(&stabSettings.innerPids[1], stabSettings.stabBank.PitchRatePID.Kp,
|
||||
stabSettings.stabBank.PitchRatePID.Ki,
|
||||
stabSettings.stabBank.PitchRatePID.Kd,
|
||||
stabSettings.stabBank.PitchRatePID.ILimit);
|
||||
|
||||
// Set the yaw rate PID constants
|
||||
pid_configure(&pids[PID_RATE_YAW], bank.YawRatePID.Kp,
|
||||
bank.YawRatePID.Ki,
|
||||
bank.YawRatePID.Kd,
|
||||
bank.YawRatePID.ILimit);
|
||||
pid_configure(&stabSettings.innerPids[2], stabSettings.stabBank.YawRatePID.Kp,
|
||||
stabSettings.stabBank.YawRatePID.Ki,
|
||||
stabSettings.stabBank.YawRatePID.Kd,
|
||||
stabSettings.stabBank.YawRatePID.ILimit);
|
||||
|
||||
// Set the roll attitude PI constants
|
||||
pid_configure(&pids[PID_ROLL], bank.RollPI.Kp,
|
||||
bank.RollPI.Ki,
|
||||
pid_configure(&stabSettings.outerPids[0], stabSettings.stabBank.RollPI.Kp,
|
||||
stabSettings.stabBank.RollPI.Ki,
|
||||
0,
|
||||
bank.RollPI.ILimit);
|
||||
stabSettings.stabBank.RollPI.ILimit);
|
||||
|
||||
// Set the pitch attitude PI constants
|
||||
pid_configure(&pids[PID_PITCH], bank.PitchPI.Kp,
|
||||
bank.PitchPI.Ki,
|
||||
pid_configure(&stabSettings.outerPids[1], stabSettings.stabBank.PitchPI.Kp,
|
||||
stabSettings.stabBank.PitchPI.Ki,
|
||||
0,
|
||||
bank.PitchPI.ILimit);
|
||||
stabSettings.stabBank.PitchPI.ILimit);
|
||||
|
||||
// Set the yaw attitude PI constants
|
||||
pid_configure(&pids[PID_YAW], bank.YawPI.Kp,
|
||||
bank.YawPI.Ki,
|
||||
pid_configure(&stabSettings.outerPids[2], stabSettings.stabBank.YawPI.Kp,
|
||||
stabSettings.stabBank.YawPI.Ki,
|
||||
0,
|
||||
bank.YawPI.ILimit);
|
||||
stabSettings.stabBank.YawPI.ILimit);
|
||||
}
|
||||
|
||||
|
||||
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
StabilizationSettingsGet(&settings);
|
||||
// needs no mutex, as long as eventdispatcher and Stabilization are both TASK_PRIORITY_CRITICAL
|
||||
StabilizationSettingsGet(&stabSettings.settings);
|
||||
|
||||
// Set up the derivative term
|
||||
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
|
||||
|
||||
// Maximum deviation to accumulate for axis lock
|
||||
max_axis_lock = settings.MaxAxisLock;
|
||||
max_axislock_rate = settings.MaxAxisLockRate;
|
||||
|
||||
// Settings for weak leveling
|
||||
weak_leveling_kp = settings.WeakLevelingKp;
|
||||
weak_leveling_max = settings.MaxWeakLevelingRate;
|
||||
|
||||
// Whether to zero the PID integrals while thrust is low
|
||||
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
|
||||
pid_configure_derivative(stabSettings.settings.DerivativeCutoff, stabSettings.settings.DerivativeGamma);
|
||||
|
||||
// The dT has some jitter iteration to iteration that we don't want to
|
||||
// make thie result unpredictable. Still, it's nicer to specify the constant
|
||||
@ -805,37 +280,29 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
// update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
|
||||
// calculation
|
||||
const float fakeDt = 0.0025f;
|
||||
if (settings.GyroTau < 0.0001f) {
|
||||
gyro_alpha = 0; // not trusting this to resolve to 0
|
||||
if (stabSettings.settings.GyroTau < 0.0001f) {
|
||||
stabSettings.gyro_alpha = 0; // not trusting this to resolve to 0
|
||||
} else {
|
||||
gyro_alpha = expf(-fakeDt / settings.GyroTau);
|
||||
stabSettings.gyro_alpha = expf(-fakeDt / stabSettings.settings.GyroTau);
|
||||
}
|
||||
|
||||
// Compute time constant for vbar decay term based on a tau
|
||||
vbar_decay = expf(-fakeDt / settings.VbarTau);
|
||||
|
||||
// force flight mode update
|
||||
cur_flight_mode = -1;
|
||||
|
||||
// Rattitude stick angle where the attitude to rate transition happens
|
||||
if (settings.RattitudeModeTransition < (uint8_t)10) {
|
||||
rattitude_mode_transition_stick_position = 10.0f / 100.0f;
|
||||
if (stabSettings.settings.RattitudeModeTransition < (uint8_t)10) {
|
||||
stabSettings.rattitude_mode_transition_stick_position = 10.0f / 100.0f;
|
||||
} else {
|
||||
rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransition / 100.0f;
|
||||
stabSettings.rattitude_mode_transition_stick_position = (float)stabSettings.settings.RattitudeModeTransition / 100.0f;
|
||||
}
|
||||
|
||||
cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f;
|
||||
cruise_control_max_thrust = (float)settings.CruiseControlMaxThrust / 100.0f;
|
||||
cruise_control_max_angle = settings.CruiseControlMaxAngle;
|
||||
cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor;
|
||||
cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f;
|
||||
cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch;
|
||||
cruise_control_neutral_thrust = (float)settings.CruiseControlNeutralThrust / 100.0f;
|
||||
|
||||
// memcpy( // disabled because removed from uavobject for refactoring (CRITICAL! doesnt fly, just to make it compile!!!)
|
||||
// cruise_control_flight_mode_switch_pos_enable,
|
||||
// settings.CruiseControlFlightModeSwitchPosEnable,
|
||||
// sizeof(cruise_control_flight_mode_switch_pos_enable));
|
||||
stabSettings.cruiseControl.cruise_control_min_thrust = (float)stabSettings.settings.CruiseControlMinThrust / 100.0f;
|
||||
stabSettings.cruiseControl.cruise_control_max_thrust = (float)stabSettings.settings.CruiseControlMaxThrust / 100.0f;
|
||||
stabSettings.cruiseControl.cruise_control_max_angle_cosine = cos_lookup_deg(stabSettings.settings.CruiseControlMaxAngle);
|
||||
stabSettings.cruiseControl.cruise_control_max_power_factor = stabSettings.settings.CruiseControlMaxPowerFactor;
|
||||
stabSettings.cruiseControl.cruise_control_power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f;
|
||||
stabSettings.cruiseControl.cruise_control_inverted_power_switch = stabSettings.settings.CruiseControlInvertedPowerSwitch;
|
||||
stabSettings.cruiseControl.cruise_control_neutral_thrust = (float)stabSettings.settings.CruiseControlNeutralThrust / 100.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -87,6 +87,7 @@ ifndef TESTAPP
|
||||
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank1.c
|
||||
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank2.c
|
||||
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank3.c
|
||||
SRC += $(OPUAVSYNTHDIR)/stabilizationstatus.c
|
||||
SRC += $(OPUAVSYNTHDIR)/stabilizationbank.c
|
||||
SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
|
||||
SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
|
||||
|
||||
@ -87,6 +87,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
|
||||
UAVOBJSRCFILENAMES += stabilizationstatus
|
||||
UAVOBJSRCFILENAMES += stabilizationbank
|
||||
UAVOBJSRCFILENAMES += systemalarms
|
||||
UAVOBJSRCFILENAMES += systemsettings
|
||||
|
||||
@ -87,6 +87,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
|
||||
UAVOBJSRCFILENAMES += stabilizationstatus
|
||||
UAVOBJSRCFILENAMES += stabilizationbank
|
||||
UAVOBJSRCFILENAMES += systemalarms
|
||||
UAVOBJSRCFILENAMES += systemsettings
|
||||
|
||||
@ -86,6 +86,7 @@ UAVOBJSRCFILENAMES += stabilizationsettings
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank1
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank2
|
||||
UAVOBJSRCFILENAMES += stabilizationsettingsbank3
|
||||
UAVOBJSRCFILENAMES += stabilizationstatus
|
||||
UAVOBJSRCFILENAMES += stabilizationbank
|
||||
UAVOBJSRCFILENAMES += systemalarms
|
||||
UAVOBJSRCFILENAMES += systemsettings
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user