rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-10 18: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 <openpilot.h>
#include "CoordinateConversions.h" #include <math.h>
#include "altholdsmoothed.h" #include <CoordinateConversions.h>
#include "attitudeactual.h" #include <altholdsmoothed.h>
#include "altitudeholdsettings.h" #include <attitudeactual.h>
#include "altitudeholddesired.h" // object that will be updated by the module #include <altitudeholdsettings.h>
#include "baroaltitude.h" #include <altitudeholddesired.h> // object that will be updated by the module
#include "positionactual.h" #include <baroaltitude.h>
#include "flightstatus.h" #include <positionactual.h>
#include "stabilizationdesired.h" #include <flightstatus.h>
#include "accels.h" #include <stabilizationdesired.h>
#include "taskinfo.h" #include <accels.h>
#include <taskinfo.h>
#include <pios_constants.h>
// Private constants // Private constants
#define MAX_QUEUE_SIZE 2 #define MAX_QUEUE_SIZE 2
#define STACK_SIZE_BYTES 1024 #define STACK_SIZE_BYTES 1024
@ -340,7 +341,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Verify that we are in altitude hold mode // Verify that we are in altitude hold mode
FlightStatusData flightStatus; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) { if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ||
flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) {
running = false; running = false;
} }

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

@ -51,7 +51,8 @@
#include "stabilizationdesired.h" #include "stabilizationdesired.h"
#include "receiveractivity.h" #include "receiveractivity.h"
#include "systemsettings.h" #include "systemsettings.h"
#include "taskinfo.h" #include <altitudeholdsettings.h>
#include <taskinfo.h>
#if defined(PIOS_INCLUDE_USB_RCTX) #if defined(PIOS_INCLUDE_USB_RCTX)
#include "pios_usb_rctx.h" #include "pios_usb_rctx.h"
@ -764,9 +765,9 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
portTickType thisSysTime; portTickType thisSysTime;
float dT; float dT;
thisSysTime = xTaskGetTickCount(); thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime; lastSysTime = thisSysTime;
*/ */
PositionActualData positionActual; PositionActualData positionActual;
@ -797,22 +798,31 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
*/ */
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
{ {
const float DEADBAND_HIGH = 0.55f; const float DEADBAND = 0.10f;
const float DEADBAND_LOW = 0.45f; 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 portTickType lastSysTime;
static bool zeroed = false; static bool zeroed = false;
portTickType thisSysTime; portTickType thisSysTime;
float dT; float dT;
AltitudeHoldDesiredData altitudeHoldDesired; AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired); AltitudeHoldDesiredGet(&altitudeHoldDesired);
AltitudeHoldSettingsThrottleExpGet(&throttleExp);
AltitudeHoldSettingsThrottleRateGet(&throttleRate);
StabilizationSettingsData stabSettings; StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings); StabilizationSettingsGet(&stabSettings);
thisSysTime = xTaskGetTickCount(); thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f);
lastSysTime = thisSysTime; lastSysTime = thisSysTime;
altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
@ -825,9 +835,11 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
altitudeHoldDesired.Altitude = 0; altitudeHoldDesired.Altitude = 0;
zeroed = false; zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { } 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) { } 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) { } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) {
// Require the stick to enter the dead band before they can move height // Require the stick to enter the dead band before they can move height
zeroed = true; 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) { 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 // read alarms
SystemAlarmsData alarms; SystemAlarmsData alarms;
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
// Check each alarm // Check each alarm
@ -911,11 +924,23 @@ static bool okToArm(void)
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue; continue;
} }
return false; 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; return true;
default:
return false;
}
} }
/** /**
* @brief Determine if the aircraft is forced to disarm by an explicit alarm * @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="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="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="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"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>