rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/next' into thread/OP-952_Scope_Polish

Browse Source
This commit is contained in:
Fredrik Arvidsson 2013-05-14 19:48:15 +02:00
commit d6d45a860e
3 changed files with 52 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/modules/AltitudeHold/altitudehold.c
View File

@ -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;
}

45
flight/modules/ManualControl/manualcontrol.c
View File

@ -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

2
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -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"/>