rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

Merge branch 'abeck/nano-rc3-fixes' into rel-nano-15.05

Browse Source
This commit is contained in:
abeck70 2015-05-24 21:33:45 +10:00
commit 1fefa9089d
8 changed files with 66 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -280,7 +280,6 @@ static void manualControlTask(void)
// assess throttle state
bool throttleNeutral = false;
bool throttleNeutralOrLow = false;
float neutralThrustOffset = 0.0f;
VtolSelfTuningStatsNeutralThrustOffsetGet(&neutralThrustOffset);
@ -291,9 +290,6 @@ static void manualControlTask(void)
if (cmd.Thrust > throttleNeutralLow && cmd.Thrust < throttleNeutralHi) {
throttleNeutral = true;
}
if (cmd.Thrust < throttleNeutralHi) {
throttleNeutralOrLow = true;
}
// determine default thrust mode for hold/brake states
uint8_t pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
@ -371,7 +367,7 @@ static void manualControlTask(void)
newAssistedThrottleState != FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL) {
// ok auto thrust mode applies in hold unless overridden
if (throttleNeutralOrLow && flagRollPitchHasInput) {
if (flagRollPitchHasInput) {
// throttle is neutral approximately and stick input present so revert to primary mode control
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
@ -410,8 +406,13 @@ static void manualControlTask(void)
// During development the assistedcontrol implementation is optional and set
// set in stabi settings. Once if we decide to always have this on, it can
// can be directly set here...i.e. set the flight mode assist as required.
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
newFlightModeAssist = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
handler = &handler_PATHFOLLOWER;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);

8
flight/modules/PathFollower/vtolbrakecontroller.cpp
View File

@ -290,7 +290,7 @@ int8_t VtolBrakeController::UpdateStabilizationDesired(void)
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
// default thrust mode to cruise control
@ -322,9 +322,9 @@ int8_t VtolBrakeController::UpdateStabilizationDesired(void)
thrustMode = settings.Stabilization6Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
// we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
// is a more appropriate throttle mode. "GPSAssist" adds braking
// and a better throttle management to the standard Position Hold.
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break;
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break;
default:

1
flight/modules/PathFollower/vtolvelocitycontroller.cpp
View File

@ -186,6 +186,7 @@ int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused)
// default thrust mode to altvario
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO;
stabDesired.Thrust = manualControl.Thrust;
StabilizationDesiredSet(&stabDesired);

1
flight/modules/Stabilization/altitudeloop.cpp
View File

@ -85,6 +85,7 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
controlDown.Activate();
newaltitude = true;
// calculate a thrustDemand on reinit only
thrustMode = mode;
altitudeHoldTask();
}

27
flight/modules/Stabilization/outerloop.c
View File

@ -104,25 +104,30 @@ static void stabilizationOuterloopTask()
float *rateDesiredAxis = &rateDesired.Roll;
int t;
float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
StabilizationStatusOuterLoopOptions newThrustMode = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST] != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]);
previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
#ifdef REVOLUTION
if (reinit && (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE ||
previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO)) {
// disable the altvario velocity control loop
stabilizationDisableAltitudeHold();
bool reinit = (newThrustMode != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]);
// Trigger a disable message to the alt hold on reinit to prevent that loop from running when not in use.
if (reinit) {
if (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE ||
previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO) {
if (newThrustMode != STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE && newThrustMode != STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO) {
// disable the altvario velocity control loop
stabilizationDisableAltitudeHold();
}
}
}
#endif /* REVOLUTION */
switch (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]) {
#ifdef REVOLUTION
// update previous mode
previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = newThrustMode;
// calculate the thrust desired
switch (newThrustMode) {
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEHOLD, reinit);
break;
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEVARIO, reinit);
break;
#endif /* REVOLUTION */
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
default:

23
flight/modules/Telemetry/telemetry.c
View File

@ -123,6 +123,8 @@ static channelContext radioChannel;
static int32_t transmitRadioData(uint8_t *data, int32_t length);
static void registerRadioObject(UAVObjHandle obj);
static uint32_t radioPort();
static uint32_t radio_port;
// Telemetry stats
static uint32_t txErrors;
@ -254,6 +256,20 @@ int32_t TelemetryInitialize(void)
{
HwSettingsInitialize();
#ifdef PIOS_INCLUDE_RFM22B
OPLinkSettingsInitialize();
OPLinkSettingsData data;
OPLinkSettingsGet(&data);
if (data.PPMOnly) {
radio_port = 0;
} else {
radio_port = PIOS_COM_RF;
}
#else /* PIOS_INCLUDE_RFM22B */
radio_port = 0;
#endif /* PIOS_INCLUDE_RFM22B */
FlightTelemetryStatsInitialize();
GCSTelemetryStatsInitialize();
@ -636,11 +652,8 @@ static uint32_t localPort()
*/
static uint32_t radioPort()
{
#ifdef PIOS_INCLUDE_RFM22B
uint32_t port = PIOS_COM_RF;
#else /* PIOS_INCLUDE_RFM22B */
uint32_t port = 0;
#endif /* PIOS_INCLUDE_RFM22B */
uint32_t port = radio_port;
#ifdef PIOS_INCLUDE_USB
// if USB is connected, USB takes precedence for telemetry
if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) {

66
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -11,10 +11,8 @@
defaultvalue="Attitude,Attitude,AxisLock,Manual"
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate::RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
/>
<field name="Stabilization2Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw,Thrust"
@ -22,10 +20,8 @@
defaultvalue="Attitude,Attitude,Rate,Manual"
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
/>
<field name="Stabilization3Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw,Thrust"
@ -33,10 +29,8 @@
defaultvalue="Rate,Rate,Rate,Manual"
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
/>
<field name="Stabilization4Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw,Thrust"
@ -44,10 +38,8 @@
defaultvalue="Attitude,Attitude,AxisLock,CruiseControl"
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
/>
<field name="Stabilization5Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw,Thrust"
@ -55,10 +47,8 @@
defaultvalue="Attitude,Attitude,Rate,CruiseControl"
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
/>
<field name="Stabilization6Settings" units="" type="enum"
elementnames="Roll,Pitch,Yaw,Thrust"
@ -66,10 +56,8 @@
defaultvalue="Rate,Rate,Rate,Manual"
limits="%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl; \
%NE:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude,\
%0401NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario,\
%0402NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude:AltitudeHold:AltitudeVario;"
%NE:RateTrainer:AltitudeHold:AltitudeVario:CruiseControl:Attitude:Rattitude:Acro+:WeakLeveling:VirtualBar; \
%NE:Rate:RateTrainer:Attitude:AxisLock:WeakLeveling:VirtualBar:Acro+:Rattitude;"
/>
<!-- Note these options values should be identical to those defined in FlightMode -->
@ -80,30 +68,12 @@
elements="6"
options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff"
defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6"
limits="\
%0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;"/>
limits="%NE:POI:AutoCruise; \
%NE:POI:AutoCruise; \
%NE:POI:AutoCruise; \
%NE:POI:AutoCruise; \
%NE:POI:AutoCruise; \
%NE:POI:AutoCruise;" />
<field name="AlwaysStabilizeWhenArmed" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE" description="For Multirotors. Always stabilize no matter the throttle setting when vehicle is armed. Does not work when vehicle is set to Always Armed."/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>

24
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -46,29 +46,7 @@
<field name="ScaleToAirspeed" units="m/s" type="float" elements="1" defaultvalue="0"/>
<field name="ScaleToAirspeedLimits" units="" type="float" elementnames="Min,Max" defaultvalue="0.05,3"/>
<field name="FlightModeAssistMap" units="" type="enum"
options="None,GPSAssist"
elements="6"
defaultvalue="None,None,None,None,None,None"
limits="\
%0401NE:GPSAssist,\
%0402NE:GPSAssist;\
\
%0401NE:GPSAssist,\
%0402NE:GPSAssist;\
\
%0401NE:GPSAssist,\
%0402NE:GPSAssist;\
\
%0401NE:GPSAssist,\
%0402NE:GPSAssist;\
\
%0401NE:GPSAssist,\
%0402NE:GPSAssist;\
\
%0401NE:GPSAssist,\
%0402NE:GPSAssist;"
/>
<field name="FlightModeAssistMap" units="" type="enum" options="None,GPSAssist" elements="6" defaultvalue="None,None,None,None,None,None" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>