mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
Merge branch 'amorale/OP-938_ah_improvements' into next
Conflicts: flight/modules/AltitudeHold/altitudehold.c flight/modules/ManualControl/manualcontrol.c
This commit is contained in:
Alessio Morale
commit
ff16dd03e7
@ -45,18 +45,19 @@
|
||||
|
||||
#include <openpilot.h>
|
||||
|
||||
#include "CoordinateConversions.h"
|
||||
#include "altholdsmoothed.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "altitudeholdsettings.h"
|
||||
#include "altitudeholddesired.h" // object that will be updated by the module
|
||||
#include "baroaltitude.h"
|
||||
#include "positionactual.h"
|
||||
#include "flightstatus.h"
|
||||
#include "stabilizationdesired.h"
|
||||
#include "accels.h"
|
||||
#include "taskinfo.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <CoordinateConversions.h>
|
||||
#include <altholdsmoothed.h>
|
||||
#include <attitudeactual.h>
|
||||
#include <altitudeholdsettings.h>
|
||||
#include <altitudeholddesired.h> // object that will be updated by the module
|
||||
#include <baroaltitude.h>
|
||||
#include <positionactual.h>
|
||||
#include <flightstatus.h>
|
||||
#include <stabilizationdesired.h>
|
||||
#include <accels.h>
|
||||
#include <taskinfo.h>
|
||||
#include <pios_constants.h>
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 2
|
||||
#define STACK_SIZE_BYTES 1024
|
||||
@ -340,7 +341,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
|
||||
// Verify that we are in altitude hold mode
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
|
||||
if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ||
|
||||
flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) {
|
||||
running = false;
|
||||
}
|
||||
|
||||
|
||||
@ -51,7 +51,8 @@
|
||||
#include "stabilizationdesired.h"
|
||||
#include "receiveractivity.h"
|
||||
#include "systemsettings.h"
|
||||
#include "taskinfo.h"
|
||||
#include <altitudeholdsettings.h>
|
||||
#include <taskinfo.h>
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB_RCTX)
|
||||
#include "pios_usb_rctx.h"
|
||||
@ -764,9 +765,9 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
|
||||
portTickType thisSysTime;
|
||||
float dT;
|
||||
|
||||
thisSysTime = xTaskGetTickCount();
|
||||
thisSysTime = xTaskGetTickCount();
|
||||
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
|
||||
lastSysTime = thisSysTime;
|
||||
lastSysTime = thisSysTime;
|
||||
*/
|
||||
|
||||
PositionActualData positionActual;
|
||||
@ -797,22 +798,31 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
|
||||
*/
|
||||
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
|
||||
{
|
||||
const float DEADBAND_HIGH = 0.55f;
|
||||
const float DEADBAND_LOW = 0.45f;
|
||||
const float DEADBAND = 0.10f;
|
||||
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
|
||||
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
|
||||
|
||||
// this is the max speed in m/s at the extents of throttle
|
||||
uint8_t throttleRate;
|
||||
uint8_t throttleExp;
|
||||
|
||||
static portTickType lastSysTime;
|
||||
static bool zeroed = false;
|
||||
portTickType thisSysTime;
|
||||
float dT;
|
||||
|
||||
AltitudeHoldDesiredData altitudeHoldDesired;
|
||||
AltitudeHoldDesiredGet(&altitudeHoldDesired);
|
||||
|
||||
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
|
||||
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
|
||||
|
||||
StabilizationSettingsData stabSettings;
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
|
||||
thisSysTime = xTaskGetTickCount();
|
||||
thisSysTime = xTaskGetTickCount();
|
||||
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
|
||||
lastSysTime = thisSysTime;
|
||||
lastSysTime = thisSysTime;
|
||||
|
||||
altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
|
||||
altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
|
||||
@ -825,9 +835,11 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
|
||||
altitudeHoldDesired.Altitude = 0;
|
||||
zeroed = false;
|
||||
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
|
||||
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
|
||||
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
|
||||
// then apply an "exp" f(x,k) = (k∙x∙x∙x + x∙(256−k)) / 256
|
||||
altitudeHoldDesired.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
|
||||
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
|
||||
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT;
|
||||
altitudeHoldDesired.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
|
||||
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) {
|
||||
// Require the stick to enter the dead band before they can move height
|
||||
zeroed = true;
|
||||
@ -892,7 +904,7 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
|
||||
}
|
||||
|
||||
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
|
||||
return (end_time - start_time) * portTICK_RATE_MS;
|
||||
return (end_time - start_time) * portTICK_RATE_MS;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -903,6 +915,7 @@ static bool okToArm(void)
|
||||
{
|
||||
// read alarms
|
||||
SystemAlarmsData alarms;
|
||||
|
||||
SystemAlarmsGet(&alarms);
|
||||
|
||||
// Check each alarm
|
||||
@ -911,11 +924,23 @@ static bool okToArm(void)
|
||||
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
|
||||
continue;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t flightMode;
|
||||
FlightStatusFlightModeGet(&flightMode);
|
||||
switch(flightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @brief Determine if the aircraft is forced to disarm by an explicit alarm
|
||||
|
||||
@ -8,6 +8,8 @@
|
||||
<field name="PressureNoise" units="m" type="float" elements="1" defaultvalue="0.4"/>
|
||||
<field name="AccelNoise" units="m/s^2" type="float" elements="1" defaultvalue="5"/>
|
||||
<field name="AccelDrift" units="m/s^2" type="float" elements="1" defaultvalue="0.001"/>
|
||||
<field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128"/>
|
||||
<field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user