spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

openpilot v0.3.7 release

pull/138/merge
Vehicle Researcher 2017-09-30 03:07:27 -07:00
parent 94fe677f91
commit daf54ad54d
130 changed files with 20545 additions and 1711 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.travis.yml
View File

@ -8,5 +8,5 @@ install:
script:
- docker run --rm
-v "$(pwd)"/selfdrive/test/plant/out:/tmp/openpilot/selfdrive/test/plant/out
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/plant && ./runtest.sh'
-v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'

12
Dockerfile.openpilot
View File

@ -1,8 +1,7 @@
FROM ubuntu:16.04
ENV PYTHONUNBUFFERED 1
RUN apt-get update && apt-get install -y build-essential clang vim screen wget bzip2 git libglib2.0-0 python-pip capnproto libcapnp-dev libzmq5-dev libffi-dev
RUN apt-get update && apt-get install -y build-essential clang vim screen wget bzip2 git libglib2.0-0 python-pip capnproto libcapnp-dev libzmq5-dev libffi-dev libusb-1.0-0
RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib
COPY requirements_openpilot.txt /tmp/
@ -10,4 +9,11 @@ RUN pip install -r /tmp/requirements_openpilot.txt
ENV PYTHONPATH /tmp/openpilot:$PYTHONPATH
COPY . /tmp/openpilot
COPY ./common /tmp/openpilot/common
COPY ./cereal /tmp/openpilot/cereal
COPY ./opendbc /tmp/openpilot/opendbc
COPY ./selfdrive /tmp/openpilot/selfdrive
COPY ./phonelibs /tmp/openpilot/phonelibs
COPY ./pyextra /tmp/openpilot/pyextra
RUN mkdir /tmp/openpilot/selfdrive/test/out

10
README.md
View File

@ -22,12 +22,20 @@ Supported Cars
- Acura ILX 2016 with AcuraWatch Plus
- Due to use of the cruise control for gas, it can only be enabled above 25 mph
- Honda Civic 2016 with Honda Sensing
- Honda Civic 2016-2017 with Honda Sensing
- Due to limitations in steering firmware, steering is disabled below 12 mph
- Note that the hatchback model is not supported
- Honda CR-V Touring 2015-2016 (very alpha!)
- Can only be enabled above 25 mph
In Progress Cars
------
- Chevy Volt 2016-2018 Premier with Driver Confidence II
- All 2017 Toyota Prius, Corolla, and RAV4
Directory structure
------

18
RELEASES.md
View File

@ -1,6 +1,18 @@
Version 0.3.6.1 (2017-08-15)
============================
* Mitigate low speed steering oscillations on some vehicles
Version 0.3.7 (2017-09-30)
==========================
* Improved lateral control using model predictive control
* Improved lane centering
* Improved GPS
* Reduced tendency of path deviation near right side exits
* Enable engagement while the accelerator pedal is pressed
* Enable engagement while the brake pedal is pressed, when stationary and with lead vehicle within 5m
* Disable engagement when park brake or brake hold are active
* Fixed sporadic longitudinal pulsing in Civic
* Cleanups to vehicle interface
Version 0.3.6.1 (2017-08-15)
============================
* Mitigate low speed steering oscillations on some vehicles
* Include board steering check for CR-V
Version 0.3.6 (2017-08-08)

47
cereal/Makefile
View File

@ -1,4 +1,45 @@
-include build_from_src.mk
SRCS := log.capnp car.capnp
GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
UNAME_M ?= $(shell uname -m)
# only generate C++ for docker tests
ifneq ($(OPTEST),1)
GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h
# Dont build java on the phone...
ifeq ($(UNAME_M),x86_64)
GENS += gen/java/Car.java gen/java/Log.java
endif
endif
.PHONY: all
all: $(GENS)
.PHONY: clean
clean:
rm -rf gen
gen/c/%.capnp.c: %.capnp
@echo "[ CAPNPC C ] $@"
mkdir -p gen/c/
capnpc '$<' -o c:gen/c/
gen/cpp/%.capnp.c++: %.capnp
@echo "[ CAPNPC C++ ] $@"
mkdir -p gen/cpp/
capnpc '$<' -o c++:gen/cpp/
gen/java/Car.java gen/java/Log.java: $(SRCS)
@echo "[ CAPNPC java ] $@"
mkdir -p gen/java/
capnpc $^ -o java:gen/java
# c-capnproto needs some empty headers
gen/c/c++.capnp.h gen/c/java.capnp.h:
mkdir -p gen/c/
touch '$@'
release:
@echo "cereal: this is a release"

45
cereal/build_from_src.mk
View File

@ -1,45 +0,0 @@
SRCS := log.capnp car.capnp
GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
UNAME_M ?= $(shell uname -m)
# only generate C++ for docker tests
ifneq ($(OPTEST),1)
GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h
# Dont build java on the phone...
ifeq ($(UNAME_M),x86_64)
GENS += gen/java/Car.java gen/java/Log.java
endif
endif
.PHONY: all
all: $(GENS)
.PHONY: clean
clean:
rm -rf gen
gen/c/%.capnp.c: %.capnp
@echo "[ CAPNPC C ] $@"
mkdir -p gen/c/
capnpc '$<' -o c:gen/c/
gen/cpp/%.capnp.c++: %.capnp
@echo "[ CAPNPC C++ ] $@"
mkdir -p gen/cpp/
capnpc '$<' -o c++:gen/cpp/
gen/java/Car.java gen/java/Log.java: $(SRCS)
@echo "[ CAPNPC java ] $@"
mkdir -p gen/java/
capnpc $^ -o java:gen/java
# c-capnproto needs some empty headers
gen/c/c++.capnp.h gen/c/java.capnp.h:
mkdir -p gen/c/
touch '$@'

185
cereal/car.capnp
View File

@ -7,11 +7,59 @@ $Java.outerClassname("Car");
@0x8e2af1e708af8b8d;
# ******* events causing controls state machine transition *******
struct CarEvent @0x9b1657f34caf3ad3 {
name @0 :EventName;
enable @1 :Bool;
preEnable @7 :Bool;
noEntry @2 :Bool;
warning @3 :Bool;
userDisable @4 :Bool;
softDisable @5 :Bool;
immediateDisable @6 :Bool;
enum EventName @0xbaa8c5d505f727de {
# TODO: copy from error list
commIssue @0;
steerUnavailable @1;
brakeUnavailable @2;
gasUnavailable @3;
wrongGear @4;
doorOpen @5;
seatbeltNotLatched @6;
espDisabled @7;
wrongCarMode @8;
steerTempUnavailable @9;
reverseGear @10;
buttonCancel @11;
buttonEnable @12;
pedalPressed @13;
cruiseDisabled @14;
radarCommIssue @15;
dataNeeded @16;
speedTooLow @17;
outOfSpace @18;
overheat @19;
calibrationInProgress @20;
calibrationInvalid @21;
controlsMismatch @22;
pcmEnable @23;
pcmDisable @24;
noTarget @25;
radarFault @26;
modelCommIssue @27;
brakeHold @28;
parkBrake @29;
}
}
# ******* main car state @ 100hz *******
# all speeds in m/s
struct CarState {
errors @0: List(Error);
errorsDEPRECATED @0 :List(CarEvent.EventName);
events @13 :List(CarEvent);
# car speed
vEgo @1 :Float32; # best estimate of speed
@ -33,6 +81,9 @@ struct CarState {
# cruise state
cruiseState @10 :CruiseState;
# gear
gearShifter @14 :GearShifter;
# button presses
buttonEvents @11 :List(ButtonEvent);
@ -48,31 +99,28 @@ struct CarState {
}
struct CruiseState {
enabled @0: Bool;
speed @1: Float32;
available @2: Bool;
enabled @0 :Bool;
speed @1 :Float32;
available @2 :Bool;
speedOffset @3 :Float32;
}
enum Error {
# TODO: copy from error list
commIssue @0;
steerUnavailable @1;
brakeUnavailable @2;
gasUnavailable @3;
wrongGear @4;
doorOpen @5;
seatbeltNotLatched @6;
espDisabled @7;
wrongCarMode @8;
steerTempUnavailable @9;
reverseGear @10;
# ...
enum GearShifter {
unknown @0;
park @1;
drive @2;
neutral @3;
reverse @4;
sport @5;
low @6;
brake @7;
}
# send on change
struct ButtonEvent {
pressed @0: Bool;
type @1: Type;
pressed @0 :Bool;
type @1 :Type;
enum Type {
unknown @0;
@ -91,29 +139,30 @@ struct CarState {
# ******* radar state @ 20hz *******
struct RadarState {
errors @0: List(Error);
points @1: List(RadarPoint);
errors @0 :List(Error);
points @1 :List(RadarPoint);
# which packets this state came from
canMonoTimes @2: List(UInt64);
canMonoTimes @2 :List(UInt64);
enum Error {
notValid @0;
commIssue @0;
fault @1;
}
# similar to LiveTracks
# is one timestamp valid for all? I think so
struct RadarPoint {
trackId @0: UInt64; # no trackId reuse
trackId @0 :UInt64; # no trackId reuse
# these 3 are the minimum required
dRel @1: Float32; # m from the front bumper of the car
yRel @2: Float32; # m
vRel @3: Float32; # m/s
dRel @1 :Float32; # m from the front bumper of the car
yRel @2 :Float32; # m
vRel @3 :Float32; # m/s
# these are optional and valid if they are not NaN
aRel @4: Float32; # m/s^2
yvRel @5: Float32; # m/s
aRel @4 :Float32; # m/s^2
yvRel @5 :Float32; # m/s
}
}
@ -121,17 +170,24 @@ struct RadarState {
struct CarControl {
# must be true for any actuator commands to work
enabled @0: Bool;
enabled @0 :Bool;
# range from 0.0 - 1.0
gas @1: Float32;
brake @2: Float32;
gasDEPRECATED @1 :Float32;
brakeDEPRECATED @2 :Float32;
steeringTorqueDEPRECATED @3 :Float32;
# range from -1.0 - 1.0
steeringTorque @3 :Float32;
actuators @6 :Actuators;
cruiseControl @4: CruiseControl;
hudControl @5: HUDControl;
cruiseControl @4 :CruiseControl;
hudControl @5 :HUDControl;
struct Actuators {
# range from 0.0 - 1.0
gas @0: Float32;
brake @1: Float32;
# range from -1.0 - 1.0
steer @2: Float32;
}
struct CruiseControl {
cancel @0: Bool;
@ -178,31 +234,48 @@ struct CarControl {
# ****** car param ******
struct CarParams {
carName @0: Text;
radarName @1: Text;
carFingerprint @2: Text;
carName @0 :Text;
radarName @1 :Text;
carFingerprint @2 :Text;
enableSteer @3: Bool;
enableGas @4: Bool;
enableBrake @5: Bool;
enableCruise @6: Bool;
enableSteer @3 :Bool;
enableGas @4 :Bool;
enableBrake @5 :Bool;
enableCruise @6 :Bool;
minEnableSpeed @18 :Float32;
safetyModel @19 :Int16;
steerMaxBP @20 :List(Float32);
steerMaxV @21 :List(Float32);
gasMaxBP @22 :List(Float32);
gasMaxV @23 :List(Float32);
brakeMaxBP @24 :List(Float32);
brakeMaxV @25 :List(Float32);
enum SafetyModels {
# from board
default @0;
honda @1;
toyota @2;
}
# things about the car in the manual
m @7: Float32; # [kg] running weight
l @8: Float32; # [m] wheelbase
sR @9: Float32; # [] steering ratio
aF @10: Float32; # [m] GC distance to front axle
aR @11: Float32; # [m] GC distance to rear axle
chi @12: Float32; # [] rear steering ratio wrt front steering (usually 0)
m @7 :Float32; # [kg] running weight
l @8 :Float32; # [m] wheelbase
sR @9 :Float32; # [] steering ratio
aF @10 :Float32; # [m] GC distance to front axle
aR @11 :Float32; # [m] GC distance to rear axle
chi @12 :Float32; # [] rear steering ratio wrt front steering (usually 0)
# things we can derive
j @13: Float32; # [kg*m2] body rot inertia
cF @14: Float32; # [N/rad] front tire coeff of stiff
cR @15: Float32; # [N/rad] rear tire coeff of stiff
j @13 :Float32; # [kg*m2] body rotational inertia
cF @14 :Float32; # [N/rad] front tire coeff of stiff
cR @15 :Float32; # [N/rad] rear tire coeff of stiff
# Kp and Ki for the lateral control
steerKp @16: Float32;
steerKi @17: Float32;
steerKp @16 :Float32;
steerKi @17 :Float32;
# TODO: Kp and Ki for long control, perhaps not needed?
}

187
cereal/log.capnp
View File

@ -92,10 +92,10 @@ struct InitData {
}
struct PandaInfo {
hasPanda @0: Bool;
dongleId @1: Text;
stVersion @2: Text;
espVersion @3: Text;
hasPanda @0 :Bool;
dongleId @1 :Text;
stVersion @2 :Text;
espVersion @3 :Text;
}
}
@ -110,9 +110,10 @@ struct FrameData {
frameType @7 :FrameType;
timestampSof @8 :UInt64;
transform @10 :List(Float32);
androidCaptureResult @9 :AndroidCaptureResult;
enum FrameType {
unknown @0;
neo @1;
@ -196,10 +197,10 @@ struct GpsLocationData {
timestamp @7 :Int64;
source @8 :SensorSource;
# Represents NED velocity in m/s.
vNED @9 :List(Float32);
# Represents expected vertical accuracy in meters. (presumably 1 sigma?)
verticalAccuracy @10 :Float32;
@ -239,7 +240,9 @@ struct ThermalData {
# not thermal
freeSpace @7 :Float32;
batteryPercent @8 :Int16;
batteryStatus @9: Text;
batteryStatus @9 :Text;
fanSpeed @10 :UInt16;
}
struct HealthData {
@ -254,8 +257,8 @@ struct HealthData {
struct LiveUI {
rearViewCam @0 :Bool;
alertText1 @1 :Text;
alertText2 @2 :Text;
alertText1 @1 :Text;
alertText2 @2 :Text;
awarenessStatus @3 :Float32;
}
@ -264,6 +267,7 @@ struct Live20Data {
mdMonoTime @6 :UInt64;
ftMonoTimeDEPRECATED @7 :UInt64;
l100MonoTime @11 :UInt64;
radarErrors @12 :List(Car.RadarState.Error);
# all deprecated
warpMatrixDEPRECATED @0 :List(Float32);
@ -294,6 +298,7 @@ struct Live20Data {
struct LiveCalibrationData {
warpMatrix @0 :List(Float32);
warpMatrix2 @5 :List(Float32);
calStatus @1 :Int8;
calCycle @2 :Int32;
calPerc @3 :Int8;
@ -340,14 +345,14 @@ struct Live100Data {
hudLeadDEPRECATED @14 :Int32;
cumLagMs @15 :Float32;
enabled @19: Bool;
steerOverride @20: Bool;
enabled @19 :Bool;
steerOverride @20 :Bool;
vCruise @22: Float32;
vCruise @22 :Float32;
rearViewCam @23 :Bool;
alertText1 @24 :Text;
alertText2 @25 :Text;
alertText1 @24 :Text;
alertText2 @25 :Text;
awarenessStatus @26 :Float32;
angleOffset @27 :Float32;
@ -387,6 +392,7 @@ struct ModelData {
bigBoxHeight @3 :UInt16;
boxProjection @4 :List(Float32);
yuvCorrection @5 :List(Float32);
inputTransform @6 :List(Float32);
}
}
@ -399,7 +405,7 @@ struct CalibrationFeatures {
}
struct EncodeIndex {
# picture from camera
# picture from camera
frameId @0 :UInt32;
type @1 :Type;
# index of encoder from start of route
@ -438,69 +444,79 @@ struct LogRotate {
struct Plan {
mdMonoTime @9 :UInt64;
l20MonoTime @10 :UInt64;
events @13 :List(Car.CarEvent);
# lateral, 3rd order polynomial
lateralValid @0: Bool;
lateralValid @0 :Bool;
dPoly @1 :List(Float32);
laneWidth @11 :Float32;
# longitudinal
longitudinalValid @2: Bool;
longitudinalValid @2 :Bool;
vTarget @3 :Float32;
aTargetMin @4 :Float32;
aTargetMax @5 :Float32;
jerkFactor @6 :Float32;
hasLead @7 :Bool;
fcw @8 :Bool;
# gps trajectory in car frame
gpsTrajectory @12 :GpsTrajectory;
struct GpsTrajectory {
x @0 :List(Float32);
y @1 :List(Float32);
}
}
struct LiveLocationData {
status @0: UInt8;
status @0 :UInt8;
# 3D fix
lat @1: Float64;
lon @2: Float64;
alt @3: Float32; # m
# 3D fix
lat @1 :Float64;
lon @2 :Float64;
alt @3 :Float32; # m
# speed
speed @4: Float32; # m/s
speed @4 :Float32; # m/s
# NED velocity components
vNED @5: List(Float32);
vNED @5 :List(Float32);
# roll, pitch, heading (x,y,z)
roll @6: Float32; # WRT to center of earth?
pitch @7: Float32; # WRT to center of earth?
heading @8: Float32; # WRT to north?
roll @6 :Float32; # WRT to center of earth?
pitch @7 :Float32; # WRT to center of earth?
heading @8 :Float32; # WRT to north?
# what are these?
wanderAngle @9: Float32;
trackAngle @10: Float32;
wanderAngle @9 :Float32;
trackAngle @10 :Float32;
# car frame -- https://upload.wikimedia.org/wikipedia/commons/f/f5/RPY_angles_of_cars.png
# gyro, in car frame, deg/s
gyro @11: List(Float32);
gyro @11 :List(Float32);
# accel, in car frame, m/s^2
accel @12: List(Float32);
accel @12 :List(Float32);
accuracy @13: Accuracy;
accuracy @13 :Accuracy;
struct Accuracy {
pNEDError @0: List(Float32);
vNEDError @1: List(Float32);
rollError @2: Float32;
pitchError @3: Float32;
headingError @4: Float32;
ellipsoidSemiMajorError @5: Float32;
ellipsoidSemiMinorError @6: Float32;
ellipsoidOrientationError @7: Float32;
pNEDError @0 :List(Float32);
vNEDError @1 :List(Float32);
rollError @2 :Float32;
pitchError @3 :Float32;
headingError @4 :Float32;
ellipsoidSemiMajorError @5 :Float32;
ellipsoidSemiMinorError @6 :Float32;
ellipsoidOrientationError @7 :Float32;
}
}
struct EthernetPacket {
pkt @0 :Data;
ts @1: Float32;
ts @1 :Float32;
}
struct NavUpdate {
@ -602,7 +618,7 @@ struct AndroidGnss {
hasLeapSecond @4 :Bool;
leapSecond @5 :Int32;
hasFullBiasNanos @6 :Bool;
fullBiasNanos @7 :Int64;
@ -611,10 +627,10 @@ struct AndroidGnss {
hasBiasUncertaintyNanos @10 :Bool;
biasUncertaintyNanos @11 :Float64;
hasDriftNanosPerSecond @12 :Bool;
driftNanosPerSecond @13 :Float64;
hasDriftUncertaintyNanosPerSecond @14 :Bool;
driftUncertaintyNanosPerSecond @15 :Float64;
}
@ -633,7 +649,7 @@ struct AndroidGnss {
accumulatedDeltaRangeState @9 :Int32;
accumulatedDeltaRangeMeters @10 :Float64;
accumulatedDeltaRangeUncertaintyMeters @11 :Float64;
hasCarrierFrequencyHz @12 :Bool;
carrierFrequencyHz @13 :Float32;
hasCarrierCycles @14 :Bool;
@ -705,6 +721,8 @@ struct QcomGnss {
measurementReport @1 :MeasurementReport;
clockReport @2 :ClockReport;
drMeasurementReport @3 :DrMeasurementReport;
drSvPoly @4 :DrSvPolyReport;
rawLog @5 :Data;
}
enum MeasurementSource @0xd71a12b6faada7ee {
@ -742,18 +760,18 @@ struct QcomGnss {
measurementNotUsable @12 :Bool;
sirCheckIsNeeded @13 :Bool;
probationMode @14 :Bool;
glonassMeanderBitEdgeValid @15 :Bool;
glonassTimeMarkValid @16 :Bool;
gpsRoundRobinRxDiversity @17 :Bool;
gpsRxDiversity @18 :Bool;
gpsLowBandwidthRxDiversityCombined @19 :Bool;
gpsHighBandwidthNu4 @20 :Bool;
gpsHighBandwidthNu8 @21 :Bool;
gpsHighBandwidthUniform @22 :Bool;
gpsMultipathIndicator @23 :Bool;
multipathIndicator @23 :Bool;
imdJammingIndicator @24 :Bool;
lteB13TxJammingIndicator @25 :Bool;
freshMeasurementIndicator @26 :Bool;
@ -953,6 +971,37 @@ struct QcomGnss {
goodParity @32 :Bool;
}
}
struct DrSvPolyReport {
svId @0 :UInt16;
frequencyIndex @1 :Int8;
hasPosition @2 :Bool;
hasIono @3 :Bool;
hasTropo @4 :Bool;
hasElevation @5 :Bool;
polyFromXtra @6 :Bool;
hasSbasIono @7 :Bool;
iode @8 :UInt16;
t0 @9 :Float64;
xyz0 @10 :List(Float64);
xyzN @11 :List(Float64);
other @12 :List(Float32);
positionUncertainty @13 :Float32;
ionoDelay @14 :Float32;
ionoDot @15 :Float32;
sbasIonoDelay @16 :Float32;
sbasIonoDot @17 :Float32;
tropoDelay @18 :Float32;
elevation @19 :Float32;
elevationDot @20 :Float32;
elevationUncertainty @21 :Float32;
velocityCoeff @22 :List(Float64);
}
}
struct LidarPts {
@ -1037,12 +1086,12 @@ struct UbloxGnss {
# num of measurements to follow
numMeas @4 :UInt8;
measurements @5 :List(Measurement);
struct ReceiverStatus {
# leap seconds have been determined
# leap seconds have been determined
leapSecValid @0 :Bool;
# Clock reset applied
clkReset @1 : Bool;
clkReset @1 :Bool;
}
struct Measurement {
@ -1060,7 +1109,7 @@ struct UbloxGnss {
# carrier phase locktime counter in ms
locktime @7 :UInt16;
# Carrier-to-noise density ratio (signal strength) in dBHz
cno @8 : UInt8;
cno @8 :UInt8;
# pseudorange standard deviation in meters
pseudorangeStdev @9 :Float32;
# carrier phase standard deviation in cycles
@ -1103,31 +1152,47 @@ struct UbloxGnss {
ecc @15 :Float64;
cus @16 :Float64;
a @17 :Float64; # note that this is not the root!!
toe @18 :Float64;
cic @19 :Float64;
omega0 @20 :Float64;
cis @21 :Float64;
i0 @22 :Float64;
crc @23 :Float64;
omega @24 :Float64;
omegaDot @25 :Float64;
iDot @26 :Float64;
codesL2 @27 :Float64;
gpsWeek @28 :Float64;
l2 @29 :Float64;
svAcc @30 :Float64;
svHealth @31 :Float64;
tgd @32 :Float64;
iodc @33 :Float64;
transmissionTime @34 :Float64;
fitInterval @35 :Float64;
}
}
struct Clocks {
bootTimeNanos @0 :UInt64;
monotonicNanos @1 :UInt64;
monotonicRawNanos @2 :UInt64;
wallTimeNanos @3 :UInt64;
modemUptimeMillis @4 :UInt64;
}
struct LiveMpcData {
x @0 :List(Float32);
y @1 :List(Float32);
psi @2 :List(Float32);
delta @3 :List(Float32);
}
struct Event {
# in nanoseconds?
logMonoTime @0 :UInt64;
@ -1144,7 +1209,7 @@ struct Event {
model @9 :ModelData;
features @10 :CalibrationFeatures;
sensorEvents @11 :List(SensorEventData);
health @12 : HealthData;
health @12 :HealthData;
live20 @13 :Live20Data;
liveUIDEPRECATED @14 :LiveUI;
encodeIdx @15 :EncodeIndex;
@ -1167,5 +1232,7 @@ struct Event {
lidarPts @32 :LidarPts;
procLog @33 :ProcLog;
ubloxGnss @34 :UbloxGnss;
clocks @35 :Clocks;
liveMpc @36 :LiveMpcData;
}
}

4
common/basedir.py 100644
View File

@ -0,0 +1,4 @@
import os
BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")

2
common/crash.py
View File

@ -2,7 +2,7 @@
import os
import sys
if os.getenv("NOLOG"):
if os.getenv("NOLOG") or os.getenv("NOCRASH"):
def capture_exception(*exc_info):
pass
def bind_user(**kwargs):

3
common/fingerprints.py
View File

@ -18,6 +18,9 @@ _FINGERPRINTS = {
57L: 3, 145L: 8, 316L: 8, 340L: 8, 342L: 6, 344L: 8, 380L: 8, 398L: 3, 399L: 6, 401L: 8, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 493L: 3, 507L: 1, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 661L: 4, 773L: 7, 777L: 8, 800L: 8, 804L: 8, 808L: 8, 882L: 2, 884L: 7, 888L: 8, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8,
# sent messages
0x194: 4, 0x1fa: 8, 0x30c: 8, 0x33d: 5,
},
"TOYOTA PRIUS 2017": {
896L: 8, 832L: 8, 898L: 8, 899L: 8, 577L: 8, 528L: 8, 529L: 8, 530L: 8, 531L: 8, 532L: 8, 533L: 8, 534L: 8, 535L: 8, 536L: 8, 537L: 8, 538L: 8, 539L: 8, 540L: 8, 541L: 8, 542L: 8, 543L: 8, 544L: 8, 545L: 8, 546L: 8, 547L: 8, 548L: 8, 549L: 8, 550L: 8, 551L: 8, 296L: 6, 553L: 6, 554L: 6, 555L: 6, 556L: 6, 557L: 6, 558L: 6, 559L: 6, 560L: 7, 561L: 8, 562L: 8, 883L: 8, 837L: 8, 833L: 8, 576L: 8, 321L: 4, 834L: 8, 835L: 8, 580L: 8, 581L: 8, 897L: 8, 584L: 8, 1136L: 4, 976L: 8, 977L: 8, 978L: 8, 291L: 7, 881L: 8, 352L: 8, 353L: 7, 867L: 8, 868L: 8, 869L: 8, 1126L: 3, 304L: 7, 880L: 8, 552L: 6, 882L: 8, 979L: 8, 884L: 8, 885L: 8, 836L: 8
}
}

51
common/params.py
View File

@ -66,6 +66,13 @@ keys = {
# read: radard
"CarParams": TxType.CLEAR_ON_CAR_START}
def fsync_dir(path):
fd = os.open(path, os.O_RDONLY)
try:
os.fsync(fd)
finally:
os.close(fd)
class FileLock(object):
def __init__(self, path, create):
@ -182,15 +189,32 @@ class DBWriter(DBAccessor):
self._check_entered()
try:
# data_path refers to the externally used path to the params. It is a symlink.
# old_data_path is the path currently pointed to by data_path.
# tempdir_path is a path where the new params will go, which the new data path will point to.
# new_data_path is a temporary symlink that will atomically overwrite data_path.
#
# The current situation is:
# data_path -> old_data_path
# We're going to write params data to tempdir_path
# tempdir_path -> params data
# Then point new_data_path to tempdir_path
# new_data_path -> tempdir_path
# Then atomically overwrite data_path with new_data_path
# data_path -> tempdir_path
old_data_path = None
new_data_path = None
tempdir_path = tempfile.mkdtemp(prefix=".tmp", dir=self._path)
try:
# Write back all keys.
os.chmod(tempdir_path, 0o777)
for k, v in self._vals.items():
with open(os.path.join(tempdir_path, k), "wb") as f:
f.write(v)
f.flush()
os.fsync(f.fileno())
fsync_dir(tempdir_path)
data_path = self._data_path()
try:
@ -203,16 +227,21 @@ class DBWriter(DBAccessor):
new_data_path = "{}.link".format(tempdir_path)
os.symlink(os.path.basename(tempdir_path), new_data_path)
os.rename(new_data_path, data_path)
# TODO(mgraczyk): raise useful error when values are bad.
except:
shutil.rmtree(tempdir_path)
if new_data_path is not None:
os.remove(new_data_path)
raise
fsync_dir(self._path)
finally:
# If the rename worked, we can delete the old data. Otherwise delete the new one.
success = new_data_path is not None and os.path.exists(data_path) and (
os.readlink(data_path) == os.path.basename(tempdir_path))
# Keep holding the lock while we clean up the old data.
if old_data_path is not None:
shutil.rmtree(old_data_path)
if success:
if old_data_path is not None:
shutil.rmtree(old_data_path)
else:
shutil.rmtree(tempdir_path)
# Regardless of what happened above, there should be no link at new_data_path.
if new_data_path is not None and os.path.islink(new_data_path):
os.remove(new_data_path)
finally:
os.umask(self._prev_umask)
self._prev_umask = None
@ -249,6 +278,10 @@ class Params(object):
def car_start(self):
self._clear_keys_with_type(TxType.CLEAR_ON_CAR_START)
def delete(self, key):
with self.env.begin(write=True) as txn:
txn.delete(key)
def get(self, key, block=False):
if key not in keys:
raise UnknownKeyName(key)

6
common/profiler.py
View File

@ -14,6 +14,12 @@ class Profiler(object):
self.cp.append((name, tt - self.last_time))
self.last_time = tt
def reset(self, enabled=False):
self.enabled = enabled
self.cp = []
self.start_time = sec_since_boot()
self.last_time = self.start_time
def display(self):
if not self.enabled:
return

2
opendbc

@ -1 +1 @@
Subproject commit 008089045e371a9eebbe79d978ff22ae3e70bdba
Subproject commit 063032ff2b9b878c2cc10301504bad9db54f655f

2
panda

@ -1 +1 @@
Subproject commit 4901d52104e369f2444b5d56846199fdfc3fd695
Subproject commit 92a1c773e763297b7478b5bca44e25fb8cd8bdf2

74
phonelibs/qpoases/EXAMPLES/example1.cpp 100644
View File

@ -0,0 +1,74 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file EXAMPLES/example1.cpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Very simple example for testing qpOASES (using QProblem class).
*/
#include <QProblem.hpp>
/** Example for qpOASES main function using the QProblem class. */
int main( )
{
/* Setup data of first QP. */
real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
real_t A[1*2] = { 1.0, 1.0 };
real_t g[2] = { 1.5, 1.0 };
real_t lb[2] = { 0.5, -2.0 };
real_t ub[2] = { 5.0, 2.0 };
real_t lbA[1] = { -1.0 };
real_t ubA[1] = { 2.0 };
/* Setup data of second QP. */
real_t g_new[2] = { 1.0, 1.5 };
real_t lb_new[2] = { 0.0, -1.0 };
real_t ub_new[2] = { 5.0, -0.5 };
real_t lbA_new[1] = { -2.0 };
real_t ubA_new[1] = { 1.0 };
/* Setting up QProblem object. */
QProblem example( 2,1 );
/* Solve first QP. */
int nWSR = 10;
example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
/* Solve second QP. */
nWSR = 10;
example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
return 0;
}
/*
* end of file
*/

69
phonelibs/qpoases/EXAMPLES/example1b.cpp 100644
View File

@ -0,0 +1,69 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file EXAMPLES/example1b.cpp
* \author Hans Joachim Ferreau
* \version 1.3
* \date 2007-2008
*
* Very simple example for testing qpOASES using the QProblemB class.
*/
#include <QProblemB.hpp>
/** Example for qpOASES main function using the QProblemB class. */
int main( )
{
/* Setup data of first QP. */
real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
real_t g[2] = { 1.5, 1.0 };
real_t lb[2] = { 0.5, -2.0 };
real_t ub[2] = { 5.0, 2.0 };
/* Setup data of second QP. */
real_t g_new[2] = { 1.0, 1.5 };
real_t lb_new[2] = { 0.0, -1.0 };
real_t ub_new[2] = { 5.0, -0.5 };
/* Setting up QProblemB object. */
QProblemB example( 2 );
/* Solve first QP. */
int nWSR = 10;
example.init( H,g,lb,ub, nWSR,0 );
/* Solve second QP. */
nWSR = 10;
example.hotstart( g_new,lb_new,ub_new, nWSR,0 );
return 0;
}
/*
* end of file
*/

189
phonelibs/qpoases/INCLUDE/Bounds.hpp 100644
View File

@ -0,0 +1,189 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Bounds.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the Bounds class designed to manage working sets of
* bounds within a QProblem.
*/
#ifndef QPOASES_BOUNDS_HPP
#define QPOASES_BOUNDS_HPP
#include <SubjectTo.hpp>
/** This class manages working sets of bounds by storing
* index sets and other status information.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class Bounds : public SubjectTo
{
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
Bounds( );
/** Copy constructor (deep copy). */
Bounds( const Bounds& rhs /**< Rhs object. */
);
/** Destructor. */
~Bounds( );
/** Assignment operator (deep copy). */
Bounds& operator=( const Bounds& rhs /**< Rhs object. */
);
/** Pseudo-constructor takes the number of bounds.
* \return SUCCESSFUL_RETURN */
returnValue init( int n /**< Number of bounds. */
);
/** Initially adds number of a new (i.e. not yet in the list) bound to
* given index set.
* \return SUCCESSFUL_RETURN \n
RET_SETUP_BOUND_FAILED \n
RET_INDEX_OUT_OF_BOUNDS \n
RET_INVALID_ARGUMENTS */
returnValue setupBound( int _number, /**< Number of new bound. */
SubjectToStatus _status /**< Status of new bound. */
);
/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
* to the index set of free bounds; the order depends on the SujectToType
* of each index.
* \return SUCCESSFUL_RETURN \n
RET_SETUP_BOUND_FAILED */
returnValue setupAllFree( );
/** Moves index of a bound from index list of fixed to that of free bounds.
* \return SUCCESSFUL_RETURN \n
RET_MOVING_BOUND_FAILED \n
RET_INDEX_OUT_OF_BOUNDS */
returnValue moveFixedToFree( int _number /**< Number of bound to be freed. */
);
/** Moves index of a bound from index list of free to that of fixed bounds.
* \return SUCCESSFUL_RETURN \n
RET_MOVING_BOUND_FAILED \n
RET_INDEX_OUT_OF_BOUNDS */
returnValue moveFreeToFixed( int _number, /**< Number of bound to be fixed. */
SubjectToStatus _status /**< Status of bound to be fixed. */
);
/** Swaps the indices of two free bounds within the index set.
* \return SUCCESSFUL_RETURN \n
RET_SWAPINDEX_FAILED */
returnValue swapFree( int number1, /**< Number of first constraint or bound. */
int number2 /**< Number of second constraint or bound. */
);
/** Returns number of variables.
* \return Number of variables. */
inline int getNV( ) const;
/** Returns number of implicitly fixed variables.
* \return Number of implicitly fixed variables. */
inline int getNFV( ) const;
/** Returns number of bounded (but possibly free) variables.
* \return Number of bounded (but possibly free) variables. */
inline int getNBV( ) const;
/** Returns number of unbounded variables.
* \return Number of unbounded variables. */
inline int getNUV( ) const;
/** Sets number of implicitly fixed variables.
* \return SUCCESSFUL_RETURN */
inline returnValue setNFV( int n /**< Number of implicitly fixed variables. */
);
/** Sets number of bounded (but possibly free) variables.
* \return SUCCESSFUL_RETURN */
inline returnValue setNBV( int n /**< Number of bounded (but possibly free) variables. */
);
/** Sets number of unbounded variables.
* \return SUCCESSFUL_RETURN */
inline returnValue setNUV( int n /**< Number of unbounded variables */
);
/** Returns number of free variables.
* \return Number of free variables. */
inline int getNFR( );
/** Returns number of fixed variables.
* \return Number of fixed variables. */
inline int getNFX( );
/** Returns a pointer to free variables index list.
* \return Pointer to free variables index list. */
inline Indexlist* getFree( );
/** Returns a pointer to fixed variables index list.
* \return Pointer to fixed variables index list. */
inline Indexlist* getFixed( );
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
int nV; /**< Number of variables (nV = nFV + nBV + nUV). */
int nFV; /**< Number of implicitly fixed variables. */
int nBV; /**< Number of bounded (but possibly free) variables. */
int nUV; /**< Number of unbounded variables. */
Indexlist free; /**< Index list of free variables. */
Indexlist fixed; /**< Index list of fixed variables. */
};
#include <Bounds.ipp>
#endif /* QPOASES_BOUNDS_HPP */
/*
* end of file
*/

108
phonelibs/qpoases/INCLUDE/Constants.hpp 100644
View File

@ -0,0 +1,108 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Constants.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2008
*
* Definition of all global constants.
*/
#ifndef QPOASES_CONSTANTS_HPP
#define QPOASES_CONSTANTS_HPP
#ifndef QPOASES_CUSTOM_INTERFACE
#include "acado_qpoases_interface.hpp"
#else
#define XSTR(x) #x
#define STR(x) XSTR(x)
#include STR(QPOASES_CUSTOM_INTERFACE)
#endif
/** Maximum number of variables within a QP formulation.
Note: this value has to be positive! */
const int NVMAX = QPOASES_NVMAX;
/** Maximum number of constraints within a QP formulation.
Note: this value has to be positive! */
const int NCMAX = QPOASES_NCMAX;
/** Redefinition of NCMAX used for memory allocation, to avoid zero sized arrays
and compiler errors. */
const int NCMAX_ALLOC = (NCMAX == 0) ? 1 : NCMAX;
/**< Maximum number of working set recalculations.
Note: this value has to be positive! */
const int NWSRMAX = QPOASES_NWSRMAX;
/** Desired KKT tolerance of QP solution; a warning RET_INACCURATE_SOLUTION is
* issued if this tolerance is not met.
* Note: this value has to be positive! */
const real_t DESIREDACCURACY = (real_t) 1.0e-3;
/** Critical KKT tolerance of QP solution; an error is issued if this
* tolerance is not met.
* Note: this value has to be positive! */
const real_t CRITICALACCURACY = (real_t) 1.0e-2;
/** Numerical value of machine precision (min eps, s.t. 1+eps > 1).
Note: this value has to be positive! */
const real_t EPS = (real_t) QPOASES_EPS;
/** Numerical value of zero (for situations in which it would be
* unreasonable to compare with 0.0).
* Note: this value has to be positive! */
const real_t ZERO = (real_t) 1.0e-50;
/** Numerical value of infinity (e.g. for non-existing bounds).
* Note: this value has to be positive! */
const real_t INFTY = (real_t) 1.0e12;
/** Lower/upper (constraints') bound tolerance (an inequality constraint
* whose lower and upper bound differ by less than BOUNDTOL is regarded
* to be an equality constraint).
* Note: this value has to be positive! */
const real_t BOUNDTOL = (real_t) 1.0e-10;
/** Offset for relaxing (constraints') bounds at beginning of an initial homotopy.
* Note: this value has to be positive! */
const real_t BOUNDRELAXATION = (real_t) 1.0e3;
/** Factor that determines physical lengths of index lists.
* Note: this value has to be greater than 1! */
const int INDEXLISTFACTOR = 5;
#endif /* QPOASES_CONSTANTS_HPP */
/*
* end of file
*/

181
phonelibs/qpoases/INCLUDE/Constraints.hpp 100644
View File

@ -0,0 +1,181 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Constraints.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the Constraints class designed to manage working sets of
* constraints within a QProblem.
*/
#ifndef QPOASES_CONSTRAINTS_HPP
#define QPOASES_CONSTRAINTS_HPP
#include <SubjectTo.hpp>
/** This class manages working sets of constraints by storing
* index sets and other status information.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class Constraints : public SubjectTo
{
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
Constraints( );
/** Copy constructor (deep copy). */
Constraints( const Constraints& rhs /**< Rhs object. */
);
/** Destructor. */
~Constraints( );
/** Assignment operator (deep copy). */
Constraints& operator=( const Constraints& rhs /**< Rhs object. */
);
/** Pseudo-constructor takes the number of constraints.
* \return SUCCESSFUL_RETURN */
returnValue init( int n /**< Number of constraints. */
);
/** Initially adds number of a new (i.e. not yet in the list) constraint to
* a given index set.
* \return SUCCESSFUL_RETURN \n
RET_SETUP_CONSTRAINT_FAILED \n
RET_INDEX_OUT_OF_BOUNDS \n
RET_INVALID_ARGUMENTS */
returnValue setupConstraint( int _number, /**< Number of new constraint. */
SubjectToStatus _status /**< Status of new constraint. */
);
/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
* to the index set of inactive constraints; the order depends on the SujectToType
* of each index. Only disabled constraints are added to index set of disabled constraints!
* \return SUCCESSFUL_RETURN \n
RET_SETUP_CONSTRAINT_FAILED */
returnValue setupAllInactive( );
/** Moves index of a constraint from index list of active to that of inactive constraints.
* \return SUCCESSFUL_RETURN \n
RET_MOVING_CONSTRAINT_FAILED */
returnValue moveActiveToInactive( int _number /**< Number of constraint to become inactive. */
);
/** Moves index of a constraint from index list of inactive to that of active constraints.
* \return SUCCESSFUL_RETURN \n
RET_MOVING_CONSTRAINT_FAILED */
returnValue moveInactiveToActive( int _number, /**< Number of constraint to become active. */
SubjectToStatus _status /**< Status of constraint to become active. */
);
/** Returns the number of constraints.
* \return Number of constraints. */
inline int getNC( ) const;
/** Returns the number of implicit equality constraints.
* \return Number of implicit equality constraints. */
inline int getNEC( ) const;
/** Returns the number of "real" inequality constraints.
* \return Number of "real" inequality constraints. */
inline int getNIC( ) const;
/** Returns the number of unbounded constraints (i.e. without any bounds).
* \return Number of unbounded constraints (i.e. without any bounds). */
inline int getNUC( ) const;
/** Sets number of implicit equality constraints.
* \return SUCCESSFUL_RETURN */
inline returnValue setNEC( int n /**< Number of implicit equality constraints. */
);
/** Sets number of "real" inequality constraints.
* \return SUCCESSFUL_RETURN */
inline returnValue setNIC( int n /**< Number of "real" inequality constraints. */
);
/** Sets number of unbounded constraints (i.e. without any bounds).
* \return SUCCESSFUL_RETURN */
inline returnValue setNUC( int n /**< Number of unbounded constraints (i.e. without any bounds). */
);
/** Returns the number of active constraints.
* \return Number of constraints. */
inline int getNAC( );
/** Returns the number of inactive constraints.
* \return Number of constraints. */
inline int getNIAC( );
/** Returns a pointer to active constraints index list.
* \return Pointer to active constraints index list. */
inline Indexlist* getActive( );
/** Returns a pointer to inactive constraints index list.
* \return Pointer to inactive constraints index list. */
inline Indexlist* getInactive( );
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
int nC; /**< Number of constraints (nC = nEC + nIC + nUC). */
int nEC; /**< Number of implicit equality constraints. */
int nIC; /**< Number of "real" inequality constraints. */
int nUC; /**< Number of unbounded constraints (i.e. without any bounds). */
Indexlist active; /**< Index list of active constraints. */
Indexlist inactive; /**< Index list of inactive constraints. */
};
#include <Constraints.ipp>
#endif /* QPOASES_CONSTRAINTS_HPP */
/*
* end of file
*/

126
phonelibs/qpoases/INCLUDE/CyclingManager.hpp 100644
View File

@ -0,0 +1,126 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/CyclingManager.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the CyclingManager class designed to detect
* and handle possible cycling during QP iterations.
*/
#ifndef QPOASES_CYCLINGMANAGER_HPP
#define QPOASES_CYCLINGMANAGER_HPP
#include <Utils.hpp>
/** This class is intended to detect and handle possible cycling during QP iterations.
* As cycling seems to occur quite rarely, this class is NOT FULLY IMPLEMENTED YET!
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class CyclingManager
{
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
CyclingManager( );
/** Copy constructor (deep copy). */
CyclingManager( const CyclingManager& rhs /**< Rhs object. */
);
/** Destructor. */
~CyclingManager( );
/** Copy asingment operator (deep copy). */
CyclingManager& operator=( const CyclingManager& rhs /**< Rhs object. */
);
/** Pseudo-constructor which takes the number of bounds/constraints.
* \return SUCCESSFUL_RETURN */
returnValue init( int _nV, /**< Number of bounds to be managed. */
int _nC /**< Number of constraints to be managed. */
);
/** Stores index of a bound/constraint that might cause cycling.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
returnValue setCyclingStatus( int number, /**< Number of bound/constraint. */
BooleanType isBound, /**< Flag that indicates if given number corresponds to a
* bound (BT_TRUE) or a constraint (BT_FALSE). */
CyclingStatus _status /**< Cycling status of bound/constraint. */
);
/** Returns if bound/constraint might cause cycling.
* \return BT_TRUE: bound/constraint might cause cycling \n
BT_FALSE: otherwise */
CyclingStatus getCyclingStatus( int number, /**< Number of bound/constraint. */
BooleanType isBound /**< Flag that indicates if given number corresponds to
* a bound (BT_TRUE) or a constraint (BT_FALSE). */
) const;
/** Clears all previous cycling information.
* \return SUCCESSFUL_RETURN */
returnValue clearCyclingData( );
/** Returns if cycling was detected.
* \return BT_TRUE iff cycling was detected. */
inline BooleanType isCyclingDetected( ) const;
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
int nV; /**< Number of managed bounds. */
int nC; /**< Number of managed constraints. */
CyclingStatus status[NVMAX+NCMAX]; /**< Array to store cycling status of all bounds/constraints. */
BooleanType cyclingDetected; /**< Flag if cycling was detected. */
};
#include <CyclingManager.ipp>
#endif /* QPOASES_CYCLINGMANAGER_HPP */
/*
* end of file
*/

107
phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp 100644
View File

@ -0,0 +1,107 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/EXTRAS/SolutionAnalysis.hpp
* \author Milan Vukov, Boris Houska, Hans Joachim Ferreau
* \version 1.3embedded
* \date 2012
*
* Solution analysis class, based on a class in the standard version of the qpOASES
*/
//
#ifndef QPOASES_SOLUTIONANALYSIS_HPP
#define QPOASES_SOLUTIONANALYSIS_HPP
#include <QProblem.hpp>
/** Enables the computation of variance as is in the standard version of qpOASES */
#define QPOASES_USE_OLD_VERSION 0
#if QPOASES_USE_OLD_VERSION
#define KKT_DIM (2 * NVMAX + NCMAX)
#endif
class SolutionAnalysis
{
public:
/** Default constructor. */
SolutionAnalysis( );
/** Copy constructor (deep copy). */
SolutionAnalysis( const SolutionAnalysis& rhs /**< Rhs object. */
);
/** Destructor. */
~SolutionAnalysis( );
/** Copy asingment operator (deep copy). */
SolutionAnalysis& operator=( const SolutionAnalysis& rhs /**< Rhs object. */
);
/** A routine for computation of inverse of the Hessian matrix. */
returnValue getHessianInverse(
QProblem* qp, /** QP */
real_t* hessianInverse /** Inverse of the Hessian matrix*/
);
/** A routine for computation of inverse of the Hessian matrix. */
returnValue getHessianInverse( QProblemB* qp, /** QP */
real_t* hessianInverse /** Inverse of the Hessian matrix*/
);
#if QPOASES_USE_OLD_VERSION
returnValue getVarianceCovariance(
QProblem* qp,
real_t* g_b_bA_VAR,
real_t* Primal_Dual_VAR
);
#endif
private:
real_t delta_g_cov[ NVMAX ]; /** A covariance-vector of g */
real_t delta_lb_cov[ NVMAX ]; /** A covariance-vector of lb */
real_t delta_ub_cov[ NVMAX ]; /** A covariance-vector of ub */
real_t delta_lbA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of lbA */
real_t delta_ubA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of ubA */
#if QPOASES_USE_OLD_VERSION
real_t K[KKT_DIM * KKT_DIM]; /** A matrix to store an intermediate result */
#endif
int FR_idx[ NVMAX ]; /** Index array for free variables */
int FX_idx[ NVMAX ]; /** Index array for fixed variables */
int AC_idx[ NCMAX_ALLOC ]; /** Index array for active constraints */
real_t delta_xFR[ NVMAX ]; /** QP reaction, primal, w.r.t. free */
real_t delta_xFX[ NVMAX ]; /** QP reaction, primal, w.r.t. fixed */
real_t delta_yAC[ NVMAX ]; /** QP reaction, dual, w.r.t. active */
real_t delta_yFX[ NVMAX ]; /** QP reaction, dual, w.r.t. fixed*/
};
#endif // QPOASES_SOLUTIONANALYSIS_HPP

154
phonelibs/qpoases/INCLUDE/Indexlist.hpp 100644
View File

@ -0,0 +1,154 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Indexlist.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the Indexlist class designed to manage index lists of
* constraints and bounds within a SubjectTo object.
*/
#ifndef QPOASES_INDEXLIST_HPP
#define QPOASES_INDEXLIST_HPP
#include <Utils.hpp>
/** This class manages index lists.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class Indexlist
{
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
Indexlist( );
/** Copy constructor (deep copy). */
Indexlist( const Indexlist& rhs /**< Rhs object. */
);
/** Destructor. */
~Indexlist( );
/** Assingment operator (deep copy). */
Indexlist& operator=( const Indexlist& rhs /**< Rhs object. */
);
/** Pseudo-constructor.
* \return SUCCESSFUL_RETURN */
returnValue init( );
/** Creates an array of all numbers within the index set in correct order.
* \return SUCCESSFUL_RETURN \n
RET_INDEXLIST_CORRUPTED */
returnValue getNumberArray( int* const numberarray /**< Output: Array of numbers (NULL on error). */
) const;
/** Determines the index within the index list at with a given number is stored.
* \return >= 0: Index of given number. \n
-1: Number not found. */
int getIndex( int givennumber /**< Number whose index shall be determined. */
) const;
/** Determines the physical index within the index list at with a given number is stored.
* \return >= 0: Index of given number. \n
-1: Number not found. */
int getPhysicalIndex( int givennumber /**< Number whose physical index shall be determined. */
) const;
/** Returns the number stored at a given physical index.
* \return >= 0: Number stored at given physical index. \n
-RET_INDEXLIST_OUTOFBOUNDS */
int getNumber( int physicalindex /**< Physical index of the number to be returned. */
) const;
/** Returns the current length of the index list.
* \return Current length of the index list. */
inline int getLength( );
/** Returns last number within the index list.
* \return Last number within the index list. */
inline int getLastNumber( ) const;
/** Adds number to index list.
* \return SUCCESSFUL_RETURN \n
RET_INDEXLIST_MUST_BE_REORDERD \n
RET_INDEXLIST_EXCEEDS_MAX_LENGTH */
returnValue addNumber( int addnumber /**< Number to be added. */
);
/** Removes number from index list.
* \return SUCCESSFUL_RETURN */
returnValue removeNumber( int removenumber /**< Number to be removed. */
);
/** Swaps two numbers within index list.
* \return SUCCESSFUL_RETURN */
returnValue swapNumbers( int number1,/**< First number for swapping. */
int number2 /**< Second number for swapping. */
);
/** Determines if a given number is contained in the index set.
* \return BT_TRUE iff number is contain in the index set */
inline BooleanType isMember( int _number /**< Number to be tested for membership. */
) const;
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
int number[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store numbers of constraints or bounds. */
int next[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store physical index of successor. */
int previous[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store physical index of predecossor. */
int length; /**< Length of index list. */
int first; /**< Physical index of first element. */
int last; /**< Physical index of last element. */
int lastusedindex; /**< Physical index of last entry in index list. */
int physicallength; /**< Physical length of index list. */
};
#include <Indexlist.ipp>
#endif /* QPOASES_INDEXLIST_HPP */
/*
* end of file
*/

415
phonelibs/qpoases/INCLUDE/MessageHandling.hpp 100644
View File

@ -0,0 +1,415 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/MessageHandling.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the MessageHandling class including global return values.
*/
#ifndef QPOASES_MESSAGEHANDLING_HPP
#define QPOASES_MESSAGEHANDLING_HPP
// #define PC_DEBUG
#ifdef PC_DEBUG
#include <stdio.h>
/** Defines an alias for FILE from stdio.h. */
#define myFILE FILE
/** Defines an alias for stderr from stdio.h. */
#define myStderr stderr
/** Defines an alias for stdout from stdio.h. */
#define myStdout stdout
#else
/** Defines an alias for FILE from stdio.h. */
#define myFILE int
/** Defines an alias for stderr from stdio.h. */
#define myStderr 0
/** Defines an alias for stdout from stdio.h. */
#define myStdout 0
#endif
#include <Types.hpp>
#include <Constants.hpp>
/** Defines symbols for global return values. \n
* Important: All return values are assumed to be nonnegative! */
enum returnValue
{
TERMINAL_LIST_ELEMENT = -1, /**< Terminal list element, internal usage only! */
/* miscellaneous */
SUCCESSFUL_RETURN = 0, /**< Successful return. */
RET_DIV_BY_ZERO, /**< Division by zero. */
RET_INDEX_OUT_OF_BOUNDS, /**< Index out of bounds. */
RET_INVALID_ARGUMENTS, /**< At least one of the arguments is invalid. */
RET_ERROR_UNDEFINED, /**< Error number undefined. */
RET_WARNING_UNDEFINED, /**< Warning number undefined. */
RET_INFO_UNDEFINED, /**< Info number undefined. */
RET_EWI_UNDEFINED, /**< Error/warning/info number undefined. */
RET_AVAILABLE_WITH_LINUX_ONLY, /**< This function is available under Linux only. */
RET_UNKNOWN_BUG, /**< The error occured is not yet known. */
RET_PRINTLEVEL_CHANGED, /**< 10 Print level changed. */
RET_NOT_YET_IMPLEMENTED, /**< Requested function is not yet implemented in this version of qpOASES. */
/* Indexlist */
RET_INDEXLIST_MUST_BE_REORDERD, /**< Index list has to be reordered. */
RET_INDEXLIST_EXCEEDS_MAX_LENGTH, /**< Index list exceeds its maximal physical length. */
RET_INDEXLIST_CORRUPTED, /**< Index list corrupted. */
RET_INDEXLIST_OUTOFBOUNDS, /**< Physical index is out of bounds. */
RET_INDEXLIST_ADD_FAILED, /**< Adding indices from another index set failed. */
RET_INDEXLIST_INTERSECT_FAILED, /**< Intersection with another index set failed. */
/* SubjectTo / Bounds / Constraints */
RET_INDEX_ALREADY_OF_DESIRED_STATUS, /**< Index is already of desired status. */
RET_ADDINDEX_FAILED, /**< Cannot swap between different indexsets. */
RET_SWAPINDEX_FAILED, /**< 20 Adding index to index set failed. */
RET_NOTHING_TO_DO, /**< Nothing to do. */
RET_SETUP_BOUND_FAILED, /**< Setting up bound index failed. */
RET_SETUP_CONSTRAINT_FAILED, /**< Setting up constraint index failed. */
RET_MOVING_BOUND_FAILED, /**< Moving bound between index sets failed. */
RET_MOVING_CONSTRAINT_FAILED, /**< Moving constraint between index sets failed. */
/* QProblem */
RET_QP_ALREADY_INITIALISED, /**< QProblem has already been initialised. */
RET_NO_INIT_WITH_STANDARD_SOLVER, /**< Initialisation via extern QP solver is not yet implemented. */
RET_RESET_FAILED, /**< Reset failed. */
RET_INIT_FAILED, /**< Initialisation failed. */
RET_INIT_FAILED_TQ, /**< 30 Initialisation failed due to TQ factorisation. */
RET_INIT_FAILED_CHOLESKY, /**< Initialisation failed due to Cholesky decomposition. */
RET_INIT_FAILED_HOTSTART, /**< Initialisation failed! QP could not be solved! */
RET_INIT_FAILED_INFEASIBILITY, /**< Initial QP could not be solved due to infeasibility! */
RET_INIT_FAILED_UNBOUNDEDNESS, /**< Initial QP could not be solved due to unboundedness! */
RET_INIT_SUCCESSFUL, /**< Initialisation done. */
RET_OBTAINING_WORKINGSET_FAILED, /**< Failed to obtain working set for auxiliary QP. */
RET_SETUP_WORKINGSET_FAILED, /**< Failed to setup working set for auxiliary QP. */
RET_SETUP_AUXILIARYQP_FAILED, /**< Failed to setup auxiliary QP for initialised homotopy. */
RET_NO_EXTERN_SOLVER, /**< No extern QP solver available. */
RET_QP_UNBOUNDED, /**< 40 QP is unbounded. */
RET_QP_INFEASIBLE, /**< QP is infeasible. */
RET_QP_NOT_SOLVED, /**< Problems occured while solving QP with standard solver. */
RET_QP_SOLVED, /**< QP successfully solved. */
RET_UNABLE_TO_SOLVE_QP, /**< Problems occured while solving QP. */
RET_INITIALISATION_STARTED, /**< Starting problem initialisation. */
RET_HOTSTART_FAILED, /**< Unable to perform homotopy due to internal error. */
RET_HOTSTART_FAILED_TO_INIT, /**< Unable to initialise problem. */
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, /**< Unable to perform homotopy as previous QP is not solved. */
RET_ITERATION_STARTED, /**< Iteration... */
RET_SHIFT_DETERMINATION_FAILED, /**< 50 Determination of shift of the QP data failed. */
RET_STEPDIRECTION_DETERMINATION_FAILED, /**< Determination of step direction failed. */
RET_STEPLENGTH_DETERMINATION_FAILED, /**< Determination of step direction failed. */
RET_OPTIMAL_SOLUTION_FOUND, /**< Optimal solution of neighbouring QP found. */
RET_HOMOTOPY_STEP_FAILED, /**< Unable to perform homotopy step. */
RET_HOTSTART_STOPPED_INFEASIBILITY, /**< Premature homotopy termination because QP is infeasible. */
RET_HOTSTART_STOPPED_UNBOUNDEDNESS, /**< Premature homotopy termination because QP is unbounded. */
RET_WORKINGSET_UPDATE_FAILED, /**< Unable to update working sets according to initial guesses. */
RET_MAX_NWSR_REACHED, /**< Maximum number of working set recalculations performed. */
RET_CONSTRAINTS_NOT_SPECIFIED, /**< Problem does comprise constraints! You also have to specify new constraints' bounds. */
RET_INVALID_FACTORISATION_FLAG, /**< 60 Invalid factorisation flag. */
RET_UNABLE_TO_SAVE_QPDATA, /**< Unable to save QP data. */
RET_STEPDIRECTION_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */
RET_STEPDIRECTION_FAILED_CHOLESKY, /**< Abnormal termination due to Cholesky factorisation. */
RET_CYCLING_DETECTED, /**< Cycling detected. */
RET_CYCLING_NOT_RESOLVED, /**< Cycling cannot be resolved, QP probably infeasible. */
RET_CYCLING_RESOLVED, /**< Cycling probably resolved. */
RET_STEPSIZE, /**< For displaying performed stepsize. */
RET_STEPSIZE_NONPOSITIVE, /**< For displaying non-positive stepsize. */
RET_SETUPSUBJECTTOTYPE_FAILED, /**< Setup of SubjectToTypes failed. */
RET_ADDCONSTRAINT_FAILED, /**< 70 Addition of constraint to working set failed. */
RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, /**< Addition of constraint to working set failed (due to QP infeasibility). */
RET_ADDBOUND_FAILED, /**< Addition of bound to working set failed. */
RET_ADDBOUND_FAILED_INFEASIBILITY, /**< Addition of bound to working set failed (due to QP infeasibility). */
RET_REMOVECONSTRAINT_FAILED, /**< Removal of constraint from working set failed. */
RET_REMOVEBOUND_FAILED, /**< Removal of bound from working set failed. */
RET_REMOVE_FROM_ACTIVESET, /**< Removing from active set... */
RET_ADD_TO_ACTIVESET, /**< Adding to active set... */
RET_REMOVE_FROM_ACTIVESET_FAILED, /**< Removing from active set failed. */
RET_ADD_TO_ACTIVESET_FAILED, /**< Adding to active set failed. */
RET_CONSTRAINT_ALREADY_ACTIVE, /**< 80 Constraint is already active. */
RET_ALL_CONSTRAINTS_ACTIVE, /**< All constraints are active, no further constraint can be added. */
RET_LINEARLY_DEPENDENT, /**< New bound/constraint is linearly dependent. */
RET_LINEARLY_INDEPENDENT, /**< New bound/constraint is linearly independent. */
RET_LI_RESOLVED, /**< Linear independence of active contraint matrix successfully resolved. */
RET_ENSURELI_FAILED, /**< Failed to ensure linear indepence of active contraint matrix. */
RET_ENSURELI_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */
RET_ENSURELI_FAILED_NOINDEX, /**< No index found, QP probably infeasible. */
RET_ENSURELI_FAILED_CYCLING, /**< Cycling detected, QP probably infeasible. */
RET_BOUND_ALREADY_ACTIVE, /**< Bound is already active. */
RET_ALL_BOUNDS_ACTIVE, /**< 90 All bounds are active, no further bound can be added. */
RET_CONSTRAINT_NOT_ACTIVE, /**< Constraint is not active. */
RET_BOUND_NOT_ACTIVE, /**< Bound is not active. */
RET_HESSIAN_NOT_SPD, /**< Projected Hessian matrix not positive definite. */
RET_MATRIX_SHIFT_FAILED, /**< Unable to update matrices or to transform vectors. */
RET_MATRIX_FACTORISATION_FAILED, /**< Unable to calculate new matrix factorisations. */
RET_PRINT_ITERATION_FAILED, /**< Unable to print information on current iteration. */
RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, /**< No global message output file initialised. */
/* Utils */
RET_UNABLE_TO_OPEN_FILE, /**< Unable to open file. */
RET_UNABLE_TO_WRITE_FILE, /**< Unable to write into file. */
RET_UNABLE_TO_READ_FILE, /**< 100 Unable to read from file. */
RET_FILEDATA_INCONSISTENT, /**< File contains inconsistent data. */
/* SolutionAnalysis */
RET_NO_SOLUTION, /**< QP solution does not satisfy KKT optimality conditions. */
RET_INACCURATE_SOLUTION /**< KKT optimality conditions not satisfied to sufficient accuracy. */
};
/** This class handles all kinds of messages (errors, warnings, infos) initiated
* by qpOASES modules and stores the correspoding global preferences.
*
* \author Hans Joachim Ferreau (special thanks to Leonard Wirsching)
* \version 1.3embedded
* \date 2007-2008
*/
class MessageHandling
{
/*
* INTERNAL DATA STRUCTURES
*/
public:
/** Data structure for entries in global message list. */
typedef struct {
returnValue key; /**< Global return value. */
const char* data; /**< Corresponding message. */
VisibilityStatus globalVisibilityStatus; /**< Determines if message can be printed.
* If this value is set to VS_HIDDEN, no message is printed! */
} ReturnValueList;
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
MessageHandling( );
/** Constructor which takes the desired output file. */
MessageHandling( myFILE* _outputFile /**< Output file. */
);
/** Constructor which takes the desired visibility states. */
MessageHandling( VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */
VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
VisibilityStatus _infoVisibility /**< Visibility status for info messages. */
);
/** Constructor which takes the desired output file and desired visibility states. */
MessageHandling( myFILE* _outputFile, /**< Output file. */
VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */
VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
VisibilityStatus _infoVisibility /**< Visibility status for info messages. */
);
/** Copy constructor (deep copy). */
MessageHandling( const MessageHandling& rhs /**< Rhs object. */
);
/** Destructor. */
~MessageHandling( );
/** Assignment operator (deep copy). */
MessageHandling& operator=( const MessageHandling& rhs /**< Rhs object. */
);
/** Prints an error message(a simplified macro THROWERROR is also provided). \n
* Errors are definied as abnormal events which cause an immediate termination of the current (sub) function.
* Errors of a sub function should be commented by the calling function by means of a warning message
* (if this error does not cause an error of the calling function, either)!
* \return Error number returned by sub function call
*/
returnValue throwError(
returnValue Enumber, /**< Error number returned by sub function call. */
const char* additionaltext, /**< Additional error text (0, if none). */
const char* functionname, /**< Name of function which caused the error. */
const char* filename, /**< Name of file which caused the error. */
const unsigned long linenumber, /**< Number of line which caused the error.incompatible binary file */
VisibilityStatus localVisibilityStatus /**< Determines (locally) if error message can be printed to myStderr.
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
* no message is printed, anyway! */
);
/** Prints a warning message (a simplified macro THROWWARNING is also provided).
* Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function.
* \return Warning number returned by sub function call
*/
returnValue throwWarning(
returnValue Wnumber, /**< Warning number returned by sub function call. */
const char* additionaltext, /**< Additional warning text (0, if none). */
const char* functionname, /**< Name of function which caused the warning. */
const char* filename, /**< Name of file which caused the warning. */
const unsigned long linenumber, /**< Number of line which caused the warning. */
VisibilityStatus localVisibilityStatus /**< Determines (locally) if warning message can be printed to myStderr.
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
* no message is printed, anyway! */
);
/** Prints a info message (a simplified macro THROWINFO is also provided).
* \return Info number returned by sub function call
*/
returnValue throwInfo(
returnValue Inumber, /**< Info number returned by sub function call. */
const char* additionaltext, /**< Additional warning text (0, if none). */
const char* functionname, /**< Name of function which submitted the info. */
const char* filename, /**< Name of file which submitted the info. */
const unsigned long linenumber, /**< Number of line which submitted the info. */
VisibilityStatus localVisibilityStatus /**< Determines (locally) if info message can be printed to myStderr.
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
* no message is printed, anyway! */
);
/** Resets all preferences to default values.
* \return SUCCESSFUL_RETURN */
returnValue reset( );
/** Prints a complete list of all messages to output file.
* \return SUCCESSFUL_RETURN */
returnValue listAllMessages( );
/** Returns visibility status for error messages.
* \return Visibility status for error messages. */
inline VisibilityStatus getErrorVisibilityStatus( ) const;
/** Returns visibility status for warning messages.
* \return Visibility status for warning messages. */
inline VisibilityStatus getWarningVisibilityStatus( ) const;
/** Returns visibility status for info messages.
* \return Visibility status for info messages. */
inline VisibilityStatus getInfoVisibilityStatus( ) const;
/** Returns pointer to output file.
* \return Pointer to output file. */
inline myFILE* getOutputFile( ) const;
/** Returns error count value.
* \return Error count value. */
inline int getErrorCount( ) const;
/** Changes visibility status for error messages. */
inline void setErrorVisibilityStatus( VisibilityStatus _errorVisibility /**< New visibility status for error messages. */
);
/** Changes visibility status for warning messages. */
inline void setWarningVisibilityStatus( VisibilityStatus _warningVisibility /**< New visibility status for warning messages. */
);
/** Changes visibility status for info messages. */
inline void setInfoVisibilityStatus( VisibilityStatus _infoVisibility /**< New visibility status for info messages. */
);
/** Changes output file for messages. */
inline void setOutputFile( myFILE* _outputFile /**< New output file for messages. */
);
/** Changes error count.
* \return SUCCESSFUL_RETURN \n
* RET_INVALID_ARGUMENT */
inline returnValue setErrorCount( int _errorCount /**< New error count value. */
);
/** Return the error code string. */
static const char* getErrorString(int error);
/*
* PROTECTED MEMBER FUNCTIONS
*/
protected:
/** Prints a info message to myStderr (auxiliary function).
* \return Error/warning/info number returned by sub function call
*/
returnValue throwMessage(
returnValue RETnumber, /**< Error/warning/info number returned by sub function call. */
const char* additionaltext, /**< Additional warning text (0, if none). */
const char* functionname, /**< Name of function which caused the error/warning/info. */
const char* filename, /**< Name of file which caused the error/warning/info. */
const unsigned long linenumber, /**< Number of line which caused the error/warning/info. */
VisibilityStatus localVisibilityStatus, /**< Determines (locally) if info message can be printed to myStderr.
* If GLOBAL visibility status of the message is set to VS_HIDDEN,
* no message is printed, anyway! */
const char* RETstring /**< Leading string of error/warning/info message. */
);
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
VisibilityStatus errorVisibility; /**< Error messages visible? */
VisibilityStatus warningVisibility; /**< Warning messages visible? */
VisibilityStatus infoVisibility; /**< Info messages visible? */
myFILE* outputFile; /**< Output file for messages. */
int errorCount; /**< Counts number of errors (for nicer output only). */
};
#ifndef __FUNCTION__
/** Ensures that __FUNCTION__ macro is defined. */
#define __FUNCTION__ 0
#endif
#ifndef __FILE__
/** Ensures that __FILE__ macro is defined. */
#define __FILE__ 0
#endif
#ifndef __LINE__
/** Ensures that __LINE__ macro is defined. */
#define __LINE__ 0
#endif
/** Short version of throwError with default values, only returnValue is needed */
#define THROWERROR(retval) ( getGlobalMessageHandler( )->throwError((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
/** Short version of throwWarning with default values, only returnValue is needed */
#define THROWWARNING(retval) ( getGlobalMessageHandler( )->throwWarning((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
/** Short version of throwInfo with default values, only returnValue is needed */
#define THROWINFO(retval) ( getGlobalMessageHandler( )->throwInfo((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
/** Returns a pointer to global message handler.
* \return Pointer to global message handler.
*/
MessageHandling* getGlobalMessageHandler( );
#include <MessageHandling.ipp>
#endif /* QPOASES_MESSAGEHANDLING_HPP */
/*
* end of file
*/

666
phonelibs/qpoases/INCLUDE/QProblem.hpp 100644
View File

@ -0,0 +1,666 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/QProblem.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the QProblem class which is able to use the newly
* developed online active set strategy for parametric quadratic programming.
*/
#ifndef QPOASES_QPROBLEM_HPP
#define QPOASES_QPROBLEM_HPP
#include <QProblemB.hpp>
#include <Constraints.hpp>
#include <CyclingManager.hpp>
/** A class for setting up and solving quadratic programs. The main feature is
* the possibily to use the newly developed online active set strategy for
* parametric quadratic programming.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class QProblem : public QProblemB
{
/* allow SolutionAnalysis class to access private members */
friend class SolutionAnalysis;
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
QProblem( );
/** Constructor which takes the QP dimensions only. */
QProblem( int _nV, /**< Number of variables. */
int _nC /**< Number of constraints. */
);
/** Copy constructor (deep copy). */
QProblem( const QProblem& rhs /**< Rhs object. */
);
/** Destructor. */
~QProblem( );
/** Assignment operator (deep copy). */
QProblem& operator=( const QProblem& rhs /**< Rhs object. */
);
/** Clears all data structures of QProblemB except for QP data.
* \return SUCCESSFUL_RETURN \n
RET_RESET_FAILED */
returnValue reset( );
/** Initialises a QProblem with given QP data and solves it
* using an initial homotopy with empty working set (at most nWSR iterations).
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_TQ \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED \n
RET_INVALID_ARGUMENTS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue init( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _A, /**< Constraint matrix. */
const real_t* const _lb, /**< Lower bound vector (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub, /**< Upper bound vector (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
const real_t* const _ubA, /**< Upper constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy.
Output: Number of performed working set recalculations. */
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
);
/** Initialises a QProblem with given QP data and solves it
* using an initial homotopy with empty working set (at most nWSR iterations).
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_TQ \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED \n
RET_INVALID_ARGUMENTS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue init( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _A, /**< Constraint matrix. */
const real_t* const _lb, /**< Lower bound vector (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub, /**< Upper bound vector (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
const real_t* const _ubA, /**< Upper constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy.
Output: Number of performed working set recalculations. */
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
);
/** Solves QProblem using online active set strategy.
* \return SUCCESSFUL_RETURN \n
RET_MAX_NWSR_REACHED \n
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
RET_HOTSTART_FAILED \n
RET_SHIFT_DETERMINATION_FAILED \n
RET_STEPDIRECTION_DETERMINATION_FAILED \n
RET_STEPLENGTH_DETERMINATION_FAILED \n
RET_HOMOTOPY_STEP_FAILED \n
RET_HOTSTART_STOPPED_INFEASIBILITY \n
RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue hotstart( const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */
const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n
If no upper bounds exist, a NULL pointer can be passed. */
const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n
If no upper constraints' bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
Output: Number of performed working set recalculations. */
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
);
/** Returns constraint matrix of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getA( real_t* const _A /**< Array of appropriate dimension for copying constraint matrix.*/
) const;
/** Returns a single row of constraint matrix of the QP (deep copy).
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue getA( int number, /**< Number of entry to be returned. */
real_t* const row /**< Array of appropriate dimension for copying (number)th constraint. */
) const;
/** Returns lower constraints' bound vector of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getLBA( real_t* const _lbA /**< Array of appropriate dimension for copying lower constraints' bound vector.*/
) const;
/** Returns single entry of lower constraints' bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue getLBA( int number, /**< Number of entry to be returned. */
real_t& value /**< Output: lbA[number].*/
) const;
/** Returns upper constraints' bound vector of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getUBA( real_t* const _ubA /**< Array of appropriate dimension for copying upper constraints' bound vector.*/
) const;
/** Returns single entry of upper constraints' bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue getUBA( int number, /**< Number of entry to be returned. */
real_t& value /**< Output: ubA[number].*/
) const;
/** Returns current constraints object of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getConstraints( Constraints* const _constraints /** Output: Constraints object. */
) const;
/** Returns the number of constraints.
* \return Number of constraints. */
inline int getNC( ) const;
/** Returns the number of (implicitly defined) equality constraints.
* \return Number of (implicitly defined) equality constraints. */
inline int getNEC( ) const;
/** Returns the number of active constraints.
* \return Number of active constraints. */
inline int getNAC( );
/** Returns the number of inactive constraints.
* \return Number of inactive constraints. */
inline int getNIAC( );
/** Returns the dimension of null space.
* \return Dimension of null space. */
int getNZ( );
/** Returns the dual solution vector (deep copy).
* \return SUCCESSFUL_RETURN \n
RET_QP_NOT_SOLVED */
returnValue getDualSolution( real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */
) const;
/*
* PROTECTED MEMBER FUNCTIONS
*/
protected:
/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).
* \return SUCCESSFUL_RETURN \n
RET_SETUPSUBJECTTOTYPE_FAILED */
returnValue setupSubjectToType( );
/** Computes the Cholesky decomposition R of the projected Hessian (i.e. R^T*R = Z^T*H*Z).
* \return SUCCESSFUL_RETURN \n
* RET_INDEXLIST_CORRUPTED */
returnValue setupCholeskyDecompositionProjected( );
/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active.
* \return SUCCESSFUL_RETURN \n
RET_INDEXLIST_CORRUPTED */
returnValue setupTQfactorisation( );
/** Solves a QProblem whose QP data is assumed to be stored in the member variables.
* A guess for its primal/dual optimal solution vectors and the corresponding
* working sets of bounds and constraints can be provided.
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_TQ \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED */
returnValue solveInitialQP( const real_t* const xOpt, /**< Optimal primal solution vector.
* A NULL pointer can be passed. */
const real_t* const yOpt, /**< Optimal dual solution vector.
* A NULL pointer can be passed. */
const Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt).
* A NULL pointer can be passed. */
const Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt).
* A NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
* Output: Number of performed working set recalculations. */
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
);
/** Obtains the desired working set for the auxiliary initial QP in
* accordance with the user specifications
* (assumes that member AX has already been initialised!)
* \return SUCCESSFUL_RETURN \n
RET_OBTAINING_WORKINGSET_FAILED \n
RET_INVALID_ARGUMENTS */
returnValue obtainAuxiliaryWorkingSet( const real_t* const xOpt, /**< Optimal primal solution vector.
* If a NULL pointer is passed, all entries are assumed to be zero. */
const real_t* const yOpt, /**< Optimal dual solution vector.
* If a NULL pointer is passed, all entries are assumed to be zero. */
const Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt). */
const Constraints* const guessedConstraints, /**< Guessed working set for solution (xOpt,yOpt). */
Bounds* auxiliaryBounds, /**< Input: Allocated bound object. \n
* Ouput: Working set of constraints for auxiliary QP. */
Constraints* auxiliaryConstraints /**< Input: Allocated bound object. \n
* Ouput: Working set for auxiliary QP. */
) const;
/** Setups bound and constraints data structures according to auxiliaryBounds/Constraints.
* (If the working set shall be setup afresh, make sure that
* bounds and constraints data structure have been resetted
* and the TQ factorisation has been initialised!)
* \return SUCCESSFUL_RETURN \n
RET_SETUP_WORKINGSET_FAILED \n
RET_INVALID_ARGUMENTS \n
RET_UNKNOWN BUG */
returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */
const Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */
BooleanType setupAfresh /**< Flag indicating if given working set shall be
* setup afresh or by updating the current one. */
);
/** Setups the optimal primal/dual solution of the auxiliary initial QP.
* \return SUCCESSFUL_RETURN */
returnValue setupAuxiliaryQPsolution( const real_t* const xOpt, /**< Optimal primal solution vector.
* If a NULL pointer is passed, all entries are set to zero. */
const real_t* const yOpt /**< Optimal dual solution vector.
* If a NULL pointer is passed, all entries are set to zero. */
);
/** Setups gradient of the auxiliary initial QP for given
* optimal primal/dual solution and given initial working set
* (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
* \return SUCCESSFUL_RETURN */
returnValue setupAuxiliaryQPgradient( );
/** Setups (constraints') bounds of the auxiliary initial QP for given
* optimal primal/dual solution and given initial working set
* (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
* \return SUCCESSFUL_RETURN \n
RET_UNKNOWN BUG */
returnValue setupAuxiliaryQPbounds( const Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */
const Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */
BooleanType useRelaxation /**< Flag indicating if inactive (constraints') bounds shall be relaxed. */
);
/** Adds a constraint to active set.
* \return SUCCESSFUL_RETURN \n
RET_ADDCONSTRAINT_FAILED \n
RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n
RET_ENSURELI_FAILED */
returnValue addConstraint( int number, /**< Number of constraint to be added to active set. */
SubjectToStatus C_status, /**< Status of new active constraint. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Checks if new active constraint to be added is linearly dependent from
* from row of the active constraints matrix.
* \return RET_LINEARLY_DEPENDENT \n
RET_LINEARLY_INDEPENDENT \n
RET_INDEXLIST_CORRUPTED */
returnValue addConstraint_checkLI( int number /**< Number of constraint to be added to active set. */
);
/** Ensures linear independence of constraint matrix when a new constraint is added.
* To this end a bound or constraint is removed simultaneously if necessary.
* \return SUCCESSFUL_RETURN \n
RET_LI_RESOLVED \n
RET_ENSURELI_FAILED \n
RET_ENSURELI_FAILED_TQ \n
RET_ENSURELI_FAILED_NOINDEX \n
RET_REMOVE_FROM_ACTIVESET */
returnValue addConstraint_ensureLI( int number, /**< Number of constraint to be added to active set. */
SubjectToStatus C_status /**< Status of new active bound. */
);
/** Adds a bound to active set.
* \return SUCCESSFUL_RETURN \n
RET_ADDBOUND_FAILED \n
RET_ADDBOUND_FAILED_INFEASIBILITY \n
RET_ENSURELI_FAILED */
returnValue addBound( int number, /**< Number of bound to be added to active set. */
SubjectToStatus B_status, /**< Status of new active bound. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Checks if new active bound to be added is linearly dependent from
* from row of the active constraints matrix.
* \return RET_LINEARLY_DEPENDENT \n
RET_LINEARLY_INDEPENDENT */
returnValue addBound_checkLI( int number /**< Number of bound to be added to active set. */
);
/** Ensures linear independence of constraint matrix when a new bound is added.
* To this end a bound or constraint is removed simultaneously if necessary.
* \return SUCCESSFUL_RETURN \n
RET_LI_RESOLVED \n
RET_ENSURELI_FAILED \n
RET_ENSURELI_FAILED_TQ \n
RET_ENSURELI_FAILED_NOINDEX \n
RET_REMOVE_FROM_ACTIVESET */
returnValue addBound_ensureLI( int number, /**< Number of bound to be added to active set. */
SubjectToStatus B_status /**< Status of new active bound. */
);
/** Removes a constraint from active set.
* \return SUCCESSFUL_RETURN \n
RET_CONSTRAINT_NOT_ACTIVE \n
RET_REMOVECONSTRAINT_FAILED \n
RET_HESSIAN_NOT_SPD */
returnValue removeConstraint( int number, /**< Number of constraint to be removed from active set. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Removes a bounds from active set.
* \return SUCCESSFUL_RETURN \n
RET_BOUND_NOT_ACTIVE \n
RET_HESSIAN_NOT_SPD \n
RET_REMOVEBOUND_FAILED */
returnValue removeBound( int number, /**< Number of bound to be removed from active set. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
* \return SUCCESSFUL_RETURN \n
RET_DIV_BY_ZERO */
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
real_t* const a /**< Output: Solution vector */
);
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
* Special variant for the case that this function is called from within "removeBound()".
* \return SUCCESSFUL_RETURN \n
RET_DIV_BY_ZERO */
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */
real_t* const a /**< Output: Solution vector */
);
/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix.
* \return SUCCESSFUL_RETURN \n
RET_DIV_BY_ZERO */
returnValue backsolveT( const real_t* const b, /**< Right hand side vector. */
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
real_t* const a /**< Output: Solution vector */
);
/** Determines step direction of the shift of the QP data.
* \return SUCCESSFUL_RETURN */
returnValue hotstart_determineDataShift(const int* const FX_idx, /**< Index array of fixed variables. */
const int* const AC_idx, /**< Index array of active constraints. */
const real_t* const g_new, /**< New gradient vector. */
const real_t* const lbA_new,/**< New lower constraints' bounds. */
const real_t* const ubA_new,/**< New upper constraints' bounds. */
const real_t* const lb_new, /**< New lower bounds. */
const real_t* const ub_new, /**< New upper bounds. */
real_t* const delta_g, /**< Output: Step direction of gradient vector. */
real_t* const delta_lbA, /**< Output: Step direction of lower constraints' bounds. */
real_t* const delta_ubA, /**< Output: Step direction of upper constraints' bounds. */
real_t* const delta_lb, /**< Output: Step direction of lower bounds. */
real_t* const delta_ub, /**< Output: Step direction of upper bounds. */
BooleanType& Delta_bC_isZero,/**< Output: Indicates if active constraints' bounds are to be shifted. */
BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
);
/** Determines step direction of the homotopy path.
* \return SUCCESSFUL_RETURN \n
RET_STEPDIRECTION_FAILED_TQ \n
RET_STEPDIRECTION_FAILED_CHOLESKY */
returnValue hotstart_determineStepDirection(const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const int* const AC_idx, /**< Index array of active constraints. */
const real_t* const delta_g, /**< Step direction of gradient vector. */
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
BooleanType Delta_bC_isZero, /**< Indicates if active constraints' bounds are to be shifted. */
BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */
real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */
real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */
real_t* const delta_yAC, /**< Output: Dual homotopy step direction of active constraints' multiplier. */
real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */
);
/** Determines the maximum possible step length along the homotopy path.
* \return SUCCESSFUL_RETURN */
returnValue hotstart_determineStepLength( const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const int* const AC_idx, /**< Index array of active constraints. */
const int* const IAC_idx, /**< Index array of inactive constraints. */
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
real_t* const delta_Ax, /**< Output: Step in vector Ax. */
int& BC_idx, /**< Output: Index of blocking constraint. */
SubjectToStatus& BC_status, /**< Output: Status of blocking constraint. */
BooleanType& BC_isBound /**< Output: Indicates if blocking constraint is a bound. */
);
/** Performs a step along the homotopy path (and updates active set).
* \return SUCCESSFUL_RETURN \n
RET_OPTIMAL_SOLUTION_FOUND \n
RET_REMOVE_FROM_ACTIVESET_FAILED \n
RET_ADD_TO_ACTIVESET_FAILED \n
RET_QP_INFEASIBLE */
returnValue hotstart_performStep( const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const int* const AC_idx, /**< Index array of active constraints. */
const int* const IAC_idx, /**< Index array of inactive constraints. */
const real_t* const delta_g, /**< Step direction of gradient vector. */
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
const real_t* const delta_Ax, /**< Step in vector Ax. */
int BC_idx, /**< Index of blocking constraint. */
SubjectToStatus BC_status, /**< Status of blocking constraint. */
BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */
);
/** Checks if lower/upper (constraints') bounds remain consistent
* (i.e. if lb <= ub and lbA <= ubA ) during the current step.
* \return BT_TRUE iff (constraints") bounds remain consistent
*/
BooleanType areBoundsConsistent( const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */
const real_t* const delta_ubA /**< Step direction of upper constraints' bounds. */
) const;
/** Setups internal QP data.
* \return SUCCESSFUL_RETURN \n
RET_INVALID_ARGUMENTS */
returnValue setupQPdata( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _A, /**< Constraint matrix. */
const real_t* const _lb, /**< Lower bound vector (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub, /**< Upper bound vector (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
const real_t* const _lbA, /**< Lower constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
const real_t* const _ubA /**< Upper constraints' bound vector. \n
If no lower constraints' bounds exist, a NULL pointer can be passed. */
);
#ifdef PC_DEBUG /* Define print functions only for debugging! */
/** Prints concise information on the current iteration.
* \return SUCCESSFUL_RETURN \n */
returnValue printIteration( int iteration, /**< Number of current iteration. */
int BC_idx, /**< Index of blocking constraint. */
SubjectToStatus BC_status, /**< Status of blocking constraint. */
BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */
);
/** Prints concise information on the current iteration.
* NOTE: ONLY DEFINED FOR SUPPRESSING A COMPILER WARNING!!
* \return SUCCESSFUL_RETURN \n */
returnValue printIteration( int iteration, /**< Number of current iteration. */
int BC_idx, /**< Index of blocking bound. */
SubjectToStatus BC_status /**< Status of blocking bound. */
);
#endif /* PC_DEBUG */
/** Determines the maximum violation of the KKT optimality conditions
* of the current iterate within the QProblem object.
* \return SUCCESSFUL_RETURN \n
* RET_INACCURATE_SOLUTION \n
* RET_NO_SOLUTION */
returnValue checkKKTconditions( );
/** Sets constraint matrix of the QP. \n
(Remark: Also internal vector Ax is recomputed!)
* \return SUCCESSFUL_RETURN */
inline returnValue setA( const real_t* const A_new /**< New constraint matrix (with correct dimension!). */
);
/** Changes single row of constraint matrix of the QP. \n
(Remark: Also correponding component of internal vector Ax is recomputed!)
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setA( int number, /**< Number of row to be changed. */
const real_t* const value /**< New (number)th constraint (with correct dimension!). */
);
/** Sets constraints' lower bound vector of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setLBA( const real_t* const lbA_new /**< New constraints' lower bound vector (with correct dimension!). */
);
/** Changes single entry of lower constraints' bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setLBA( int number, /**< Number of entry to be changed. */
real_t value /**< New value for entry of lower constraints' bound vector (with correct dimension!). */
);
/** Sets constraints' upper bound vector of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setUBA( const real_t* const ubA_new /**< New constraints' upper bound vector (with correct dimension!). */
);
/** Changes single entry of upper constraints' bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setUBA( int number, /**< Number of entry to be changed. */
real_t value /**< New value for entry of upper constraints' bound vector (with correct dimension!). */
);
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
real_t A[NCMAX_ALLOC*NVMAX]; /**< Constraint matrix. */
real_t lbA[NCMAX_ALLOC]; /**< Lower constraints' bound vector. */
real_t ubA[NCMAX_ALLOC]; /**< Upper constraints' bound vector. */
Constraints constraints; /**< Data structure for problem's constraints. */
real_t T[NVMAX*NVMAX]; /**< Reverse triangular matrix, A = [0 T]*Q'. */
real_t Q[NVMAX*NVMAX]; /**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
int sizeT; /**< Matrix T is stored in a (sizeT x sizeT) array. */
real_t Ax[NCMAX_ALLOC]; /**< Stores the current product A*x (for increased efficiency only). */
CyclingManager cyclingManager; /**< Data structure for storing (possible) cycling information (NOT YET IMPLEMENTED!). */
};
#include <QProblem.ipp>
#endif /* QPOASES_QPROBLEM_HPP */
/*
* end of file
*/

628
phonelibs/qpoases/INCLUDE/QProblemB.hpp 100644
View File

@ -0,0 +1,628 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/QProblemB.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the QProblemB class which is able to use the newly
* developed online active set strategy for parametric quadratic programming
* for problems with (simple) bounds only.
*/
#ifndef QPOASES_QPROBLEMB_HPP
#define QPOASES_QPROBLEMB_HPP
#include <Bounds.hpp>
class SolutionAnalysis;
/** Class for setting up and solving quadratic programs with (simple) bounds only.
* The main feature is the possibily to use the newly developed online active set strategy
* for parametric quadratic programming.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class QProblemB
{
/* allow SolutionAnalysis class to access private members */
friend class SolutionAnalysis;
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
QProblemB( );
/** Constructor which takes the QP dimension only. */
QProblemB( int _nV /**< Number of variables. */
);
/** Copy constructor (deep copy). */
QProblemB( const QProblemB& rhs /**< Rhs object. */
);
/** Destructor. */
~QProblemB( );
/** Assignment operator (deep copy). */
QProblemB& operator=( const QProblemB& rhs /**< Rhs object. */
);
/** Clears all data structures of QProblemB except for QP data.
* \return SUCCESSFUL_RETURN \n
RET_RESET_FAILED */
returnValue reset( );
/** Initialises a QProblemB with given QP data and solves it
* using an initial homotopy with empty working set (at most nWSR iterations).
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED \n
RET_INVALID_ARGUMENTS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue init( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _lb, /**< Lower bounds (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub, /**< Upper bounds (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n
Output: Number of performed working set recalculations. */
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
);
/** Initialises a QProblemB with given QP data and solves it
* using an initial homotopy with empty working set (at most nWSR iterations).
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED \n
RET_INVALID_ARGUMENTS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue init( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _lb, /**< Lower bounds (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub, /**< Upper bounds (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n
Output: Number of performed working set recalculations. */
const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */
real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */
);
/** Solves an initialised QProblemB using online active set strategy.
* \return SUCCESSFUL_RETURN \n
RET_MAX_NWSR_REACHED \n
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
RET_HOTSTART_FAILED \n
RET_SHIFT_DETERMINATION_FAILED \n
RET_STEPDIRECTION_DETERMINATION_FAILED \n
RET_STEPLENGTH_DETERMINATION_FAILED \n
RET_HOMOTOPY_STEP_FAILED \n
RET_HOTSTART_STOPPED_INFEASIBILITY \n
RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
RET_INACCURATE_SOLUTION \n
RET_NO_SOLUTION */
returnValue hotstart( const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */
const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n
If no upper bounds exist, a NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
Output: Number of performed working set recalculations. */
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
);
/** Returns Hessian matrix of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getH( real_t* const _H /**< Array of appropriate dimension for copying Hessian matrix.*/
) const;
/** Returns gradient vector of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getG( real_t* const _g /**< Array of appropriate dimension for copying gradient vector.*/
) const;
/** Returns lower bound vector of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getLB( real_t* const _lb /**< Array of appropriate dimension for copying lower bound vector.*/
) const;
/** Returns single entry of lower bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue getLB( int number, /**< Number of entry to be returned. */
real_t& value /**< Output: lb[number].*/
) const;
/** Returns upper bound vector of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getUB( real_t* const _ub /**< Array of appropriate dimension for copying upper bound vector.*/
) const;
/** Returns single entry of upper bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue getUB( int number, /**< Number of entry to be returned. */
real_t& value /**< Output: ub[number].*/
) const;
/** Returns current bounds object of the QP (deep copy).
* \return SUCCESSFUL_RETURN */
inline returnValue getBounds( Bounds* const _bounds /** Output: Bounds object. */
) const;
/** Returns the number of variables.
* \return Number of variables. */
inline int getNV( ) const;
/** Returns the number of free variables.
* \return Number of free variables. */
inline int getNFR( );
/** Returns the number of fixed variables.
* \return Number of fixed variables. */
inline int getNFX( );
/** Returns the number of implicitly fixed variables.
* \return Number of implicitly fixed variables. */
inline int getNFV( ) const;
/** Returns the dimension of null space.
* \return Dimension of null space. */
int getNZ( );
/** Returns the optimal objective function value.
* \return finite value: Optimal objective function value (QP was solved) \n
+infinity: QP was not yet solved */
real_t getObjVal( ) const;
/** Returns the objective function value at an arbitrary point x.
* \return Objective function value at point x */
real_t getObjVal( const real_t* const _x /**< Point at which the objective function shall be evaluated. */
) const;
/** Returns the primal solution vector.
* \return SUCCESSFUL_RETURN \n
RET_QP_NOT_SOLVED */
returnValue getPrimalSolution( real_t* const xOpt /**< Output: Primal solution vector (if QP has been solved). */
) const;
/** Returns the dual solution vector.
* \return SUCCESSFUL_RETURN \n
RET_QP_NOT_SOLVED */
returnValue getDualSolution( real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */
) const;
/** Returns status of the solution process.
* \return Status of solution process. */
inline QProblemStatus getStatus( ) const;
/** Returns if the QProblem object is initialised.
* \return BT_TRUE: QProblemB initialised \n
BT_FALSE: QProblemB not initialised */
inline BooleanType isInitialised( ) const;
/** Returns if the QP has been solved.
* \return BT_TRUE: QProblemB solved \n
BT_FALSE: QProblemB not solved */
inline BooleanType isSolved( ) const;
/** Returns if the QP is infeasible.
* \return BT_TRUE: QP infeasible \n
BT_FALSE: QP feasible (or not known to be infeasible!) */
inline BooleanType isInfeasible( ) const;
/** Returns if the QP is unbounded.
* \return BT_TRUE: QP unbounded \n
BT_FALSE: QP unbounded (or not known to be unbounded!) */
inline BooleanType isUnbounded( ) const;
/** Returns the print level.
* \return Print level. */
inline PrintLevel getPrintLevel( ) const;
/** Changes the print level.
* \return SUCCESSFUL_RETURN */
returnValue setPrintLevel( PrintLevel _printlevel /**< New print level. */
);
/** Returns Hessian type flag (type is not determined due to this call!).
* \return Hessian type. */
inline HessianType getHessianType( ) const;
/** Changes the print level.
* \return SUCCESSFUL_RETURN */
inline returnValue setHessianType( HessianType _hessianType /**< New Hessian type. */
);
/*
* PROTECTED MEMBER FUNCTIONS
*/
protected:
/** Checks if Hessian happens to be the identity matrix,
* and sets corresponding status flag (otherwise the flag remains unaltered!).
* \return SUCCESSFUL_RETURN */
returnValue checkForIdentityHessian( );
/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).
* \return SUCCESSFUL_RETURN \n
RET_SETUPSUBJECTTOTYPE_FAILED */
returnValue setupSubjectToType( );
/** Computes the Cholesky decomposition R of the (simply projected) Hessian (i.e. R^T*R = Z^T*H*Z).
* It only works in the case where Z is a simple projection matrix!
* \return SUCCESSFUL_RETURN \n
* RET_INDEXLIST_CORRUPTED */
returnValue setupCholeskyDecomposition( );
/** Solves a QProblemB whose QP data is assumed to be stored in the member variables.
* A guess for its primal/dual optimal solution vectors and the corresponding
* optimal working set can be provided.
* \return SUCCESSFUL_RETURN \n
RET_INIT_FAILED \n
RET_INIT_FAILED_CHOLESKY \n
RET_INIT_FAILED_HOTSTART \n
RET_INIT_FAILED_INFEASIBILITY \n
RET_INIT_FAILED_UNBOUNDEDNESS \n
RET_MAX_NWSR_REACHED */
returnValue solveInitialQP( const real_t* const xOpt, /**< Optimal primal solution vector.
* A NULL pointer can be passed. */
const real_t* const yOpt, /**< Optimal dual solution vector.
* A NULL pointer can be passed. */
const Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt).
* A NULL pointer can be passed. */
int& nWSR, /**< Input: Maximum number of working set recalculations; \n
* Output: Number of performed working set recalculations. */
real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
);
/** Obtains the desired working set for the auxiliary initial QP in
* accordance with the user specifications
* \return SUCCESSFUL_RETURN \n
RET_OBTAINING_WORKINGSET_FAILED \n
RET_INVALID_ARGUMENTS */
returnValue obtainAuxiliaryWorkingSet( const real_t* const xOpt, /**< Optimal primal solution vector.
* If a NULL pointer is passed, all entries are assumed to be zero. */
const real_t* const yOpt, /**< Optimal dual solution vector.
* If a NULL pointer is passed, all entries are assumed to be zero. */
const Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). */
Bounds* auxiliaryBounds /**< Input: Allocated bound object. \n
* Ouput: Working set for auxiliary QP. */
) const;
/** Setups bound data structure according to auxiliaryBounds.
* (If the working set shall be setup afresh, make sure that
* bounds data structure has been resetted!)
* \return SUCCESSFUL_RETURN \n
RET_SETUP_WORKINGSET_FAILED \n
RET_INVALID_ARGUMENTS \n
RET_UNKNOWN BUG */
returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, /**< Working set for auxiliary QP. */
BooleanType setupAfresh /**< Flag indicating if given working set shall be
* setup afresh or by updating the current one. */
);
/** Setups the optimal primal/dual solution of the auxiliary initial QP.
* \return SUCCESSFUL_RETURN */
returnValue setupAuxiliaryQPsolution( const real_t* const xOpt, /**< Optimal primal solution vector.
* If a NULL pointer is passed, all entries are set to zero. */
const real_t* const yOpt /**< Optimal dual solution vector.
* If a NULL pointer is passed, all entries are set to zero. */
);
/** Setups gradient of the auxiliary initial QP for given
* optimal primal/dual solution and given initial working set
* (assumes that members X, Y and BOUNDS have already been initialised!).
* \return SUCCESSFUL_RETURN */
returnValue setupAuxiliaryQPgradient( );
/** Setups bounds of the auxiliary initial QP for given
* optimal primal/dual solution and given initial working set
* (assumes that members X, Y and BOUNDS have already been initialised!).
* \return SUCCESSFUL_RETURN \n
RET_UNKNOWN BUG */
returnValue setupAuxiliaryQPbounds( BooleanType useRelaxation /**< Flag indicating if inactive bounds shall be relaxed. */
);
/** Adds a bound to active set (specialised version for the case where no constraints exist).
* \return SUCCESSFUL_RETURN \n
RET_ADDBOUND_FAILED */
returnValue addBound( int number, /**< Number of bound to be added to active set. */
SubjectToStatus B_status, /**< Status of new active bound. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Removes a bounds from active set (specialised version for the case where no constraints exist).
* \return SUCCESSFUL_RETURN \n
RET_HESSIAN_NOT_SPD \n
RET_REMOVEBOUND_FAILED */
returnValue removeBound( int number, /**< Number of bound to be removed from active set. */
BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */
);
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
* \return SUCCESSFUL_RETURN \n
RET_DIV_BY_ZERO */
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
real_t* const a /**< Output: Solution vector */
);
/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
* Special variant for the case that this function is called from within "removeBound()".
* \return SUCCESSFUL_RETURN \n
RET_DIV_BY_ZERO */
returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */
BooleanType transposed, /**< Indicates if the transposed system shall be solved. */
BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */
real_t* const a /**< Output: Solution vector */
);
/** Determines step direction of the shift of the QP data.
* \return SUCCESSFUL_RETURN */
returnValue hotstart_determineDataShift(const int* const FX_idx, /**< Index array of fixed variables. */
const real_t* const g_new, /**< New gradient vector. */
const real_t* const lb_new, /**< New lower bounds. */
const real_t* const ub_new, /**< New upper bounds. */
real_t* const delta_g, /**< Output: Step direction of gradient vector. */
real_t* const delta_lb, /**< Output: Step direction of lower bounds. */
real_t* const delta_ub, /**< Output: Step direction of upper bounds. */
BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
);
/** Checks if lower/upper bounds remain consistent
* (i.e. if lb <= ub) during the current step.
* \return BT_TRUE iff bounds remain consistent
*/
BooleanType areBoundsConsistent( const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub /**< Step direction of upper bounds. */
) const;
/** Setups internal QP data.
* \return SUCCESSFUL_RETURN \n
RET_INVALID_ARGUMENTS */
returnValue setupQPdata( const real_t* const _H, /**< Hessian matrix. */
const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */
const real_t* const _g, /**< Gradient vector. */
const real_t* const _lb, /**< Lower bounds (on variables). \n
If no lower bounds exist, a NULL pointer can be passed. */
const real_t* const _ub /**< Upper bounds (on variables). \n
If no upper bounds exist, a NULL pointer can be passed. */
);
/** Sets Hessian matrix of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setH( const real_t* const H_new /**< New Hessian matrix (with correct dimension!). */
);
/** Changes gradient vector of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setG( const real_t* const g_new /**< New gradient vector (with correct dimension!). */
);
/** Changes lower bound vector of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setLB( const real_t* const lb_new /**< New lower bound vector (with correct dimension!). */
);
/** Changes single entry of lower bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setLB( int number, /**< Number of entry to be changed. */
real_t value /**< New value for entry of lower bound vector. */
);
/** Changes upper bound vector of the QP.
* \return SUCCESSFUL_RETURN */
inline returnValue setUB( const real_t* const ub_new /**< New upper bound vector (with correct dimension!). */
);
/** Changes single entry of upper bound vector of the QP.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setUB( int number, /**< Number of entry to be changed. */
real_t value /**< New value for entry of upper bound vector. */
);
/** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0]
* \return SUCCESSFUL_RETURN */
inline void computeGivens( real_t xold, /**< Matrix entry to be normalised. */
real_t yold, /**< Matrix entry to be annihilated. */
real_t& xnew, /**< Output: Normalised matrix entry. */
real_t& ynew, /**< Output: Annihilated matrix entry. */
real_t& c, /**< Output: Cosine entry of Givens matrix. */
real_t& s /**< Output: Sine entry of Givens matrix. */
) const;
/** Applies Givens matrix determined by c and s (cf. computeGivens).
* \return SUCCESSFUL_RETURN */
inline void applyGivens( real_t c, /**< Cosine entry of Givens matrix. */
real_t s, /**< Sine entry of Givens matrix. */
real_t xold, /**< Matrix entry to be transformed corresponding to
* the normalised entry of the original matrix. */
real_t yold, /**< Matrix entry to be transformed corresponding to
* the annihilated entry of the original matrix. */
real_t& xnew, /**< Output: Transformed matrix entry corresponding to
* the normalised entry of the original matrix. */
real_t& ynew /**< Output: Transformed matrix entry corresponding to
* the annihilated entry of the original matrix. */
) const;
/*
* PRIVATE MEMBER FUNCTIONS
*/
private:
/** Determines step direction of the homotopy path.
* \return SUCCESSFUL_RETURN \n
RET_STEPDIRECTION_FAILED_CHOLESKY */
returnValue hotstart_determineStepDirection(const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const real_t* const delta_g, /**< Step direction of gradient vector. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */
real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */
real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */
real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */
);
/** Determines the maximum possible step length along the homotopy path.
* \return SUCCESSFUL_RETURN */
returnValue hotstart_determineStepLength( const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
int& BC_idx, /**< Output: Index of blocking constraint. */
SubjectToStatus& BC_status /**< Output: Status of blocking constraint. */
);
/** Performs a step along the homotopy path (and updates active set).
* \return SUCCESSFUL_RETURN \n
RET_OPTIMAL_SOLUTION_FOUND \n
RET_REMOVE_FROM_ACTIVESET_FAILED \n
RET_ADD_TO_ACTIVESET_FAILED \n
RET_QP_INFEASIBLE */
returnValue hotstart_performStep( const int* const FR_idx, /**< Index array of free variables. */
const int* const FX_idx, /**< Index array of fixed variables. */
const real_t* const delta_g, /**< Step direction of gradient vector. */
const real_t* const delta_lb, /**< Step direction of lower bounds. */
const real_t* const delta_ub, /**< Step direction of upper bounds. */
const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */
const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */
const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */
int BC_idx, /**< Index of blocking constraint. */
SubjectToStatus BC_status /**< Status of blocking constraint. */
);
#ifdef PC_DEBUG /* Define print functions only for debugging! */
/** Prints concise information on the current iteration.
* \return SUCCESSFUL_RETURN \n */
returnValue printIteration( int iteration, /**< Number of current iteration. */
int BC_idx, /**< Index of blocking bound. */
SubjectToStatus BC_status /**< Status of blocking bound. */
);
#endif /* PC_DEBUG */
/** Determines the maximum violation of the KKT optimality conditions
* of the current iterate within the QProblemB object.
* \return SUCCESSFUL_RETURN \n
* RET_INACCURATE_SOLUTION \n
* RET_NO_SOLUTION */
returnValue checkKKTconditions( );
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
real_t H[NVMAX*NVMAX]; /**< Hessian matrix. */
BooleanType hasHessian; /**< Flag indicating whether H contains Hessian or corresponding Cholesky factor R; \sa init. */
real_t g[NVMAX]; /**< Gradient. */
real_t lb[NVMAX]; /**< Lower bound vector (on variables). */
real_t ub[NVMAX]; /**< Upper bound vector (on variables). */
Bounds bounds; /**< Data structure for problem's bounds. */
real_t R[NVMAX*NVMAX]; /**< Cholesky decomposition of H (i.e. H = R^T*R). */
BooleanType hasCholesky; /**< Flag indicating whether Cholesky decomposition has already been setup. */
real_t x[NVMAX]; /**< Primal solution vector. */
real_t y[NVMAX+NCMAX]; /**< Dual solution vector. */
real_t tau; /**< Last homotopy step length. */
QProblemStatus status; /**< Current status of the solution process. */
BooleanType infeasible; /**< QP infeasible? */
BooleanType unbounded; /**< QP unbounded? */
HessianType hessianType; /**< Type of Hessian matrix. */
PrintLevel printlevel; /**< Print level. */
int count; /**< Counts the number of hotstart function calls (internal usage only!). */
};
#include <QProblemB.ipp>
#endif /* QPOASES_QPROBLEMB_HPP */
/*
* end of file
*/

178
phonelibs/qpoases/INCLUDE/SubjectTo.hpp 100644
View File

@ -0,0 +1,178 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/SubjectTo.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of the SubjectTo class designed to manage working sets of
* constraints and bounds within a QProblem.
*/
#ifndef QPOASES_SUBJECTTO_HPP
#define QPOASES_SUBJECTTO_HPP
#include <Indexlist.hpp>
/** This class manages working sets of constraints and bounds by storing
* index sets and other status information.
*
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*/
class SubjectTo
{
/*
* PUBLIC MEMBER FUNCTIONS
*/
public:
/** Default constructor. */
SubjectTo( );
/** Copy constructor (deep copy). */
SubjectTo( const SubjectTo& rhs /**< Rhs object. */
);
/** Destructor. */
~SubjectTo( );
/** Assignment operator (deep copy). */
SubjectTo& operator=( const SubjectTo& rhs /**< Rhs object. */
);
/** Pseudo-constructor takes the number of constraints or bounds.
* \return SUCCESSFUL_RETURN */
returnValue init( int n /**< Number of constraints or bounds. */
);
/** Returns type of (constraints') bound.
* \return Type of (constraints') bound \n
RET_INDEX_OUT_OF_BOUNDS */
inline SubjectToType getType( int i /**< Number of (constraints') bound. */
) const ;
/** Returns status of (constraints') bound.
* \return Status of (constraints') bound \n
ST_UNDEFINED */
inline SubjectToStatus getStatus( int i /**< Number of (constraints') bound. */
) const;
/** Sets type of (constraints') bound.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setType( int i, /**< Number of (constraints') bound. */
SubjectToType value /**< Type of (constraints') bound. */
);
/** Sets status of (constraints') bound.
* \return SUCCESSFUL_RETURN \n
RET_INDEX_OUT_OF_BOUNDS */
inline returnValue setStatus( int i, /**< Number of (constraints') bound. */
SubjectToStatus value /**< Status of (constraints') bound. */
);
/** Sets status of lower (constraints') bounds. */
inline void setNoLower( BooleanType _status /**< Status of lower (constraints') bounds. */
);
/** Sets status of upper (constraints') bounds. */
inline void setNoUpper( BooleanType _status /**< Status of upper (constraints') bounds. */
);
/** Returns status of lower (constraints') bounds.
* \return BT_TRUE if there is no lower (constraints') bound on any variable. */
inline BooleanType isNoLower( ) const;
/** Returns status of upper bounds.
* \return BT_TRUE if there is no upper (constraints') bound on any variable. */
inline BooleanType isNoUpper( ) const;
/*
* PROTECTED MEMBER FUNCTIONS
*/
protected:
/** Adds the index of a new constraint or bound to index set.
* \return SUCCESSFUL_RETURN \n
RET_ADDINDEX_FAILED */
returnValue addIndex( Indexlist* const indexlist, /**< Index list to which the new index shall be added. */
int newnumber, /**< Number of new constraint or bound. */
SubjectToStatus newstatus /**< Status of new constraint or bound. */
);
/** Removes the index of a constraint or bound from index set.
* \return SUCCESSFUL_RETURN \n
RET_UNKNOWN_BUG */
returnValue removeIndex( Indexlist* const indexlist, /**< Index list from which the new index shall be removed. */
int removenumber /**< Number of constraint or bound to be removed. */
);
/** Swaps the indices of two constraints or bounds within the index set.
* \return SUCCESSFUL_RETURN \n
RET_SWAPINDEX_FAILED */
returnValue swapIndex( Indexlist* const indexlist, /**< Index list in which the indices shold be swapped. */
int number1, /**< Number of first constraint or bound. */
int number2 /**< Number of second constraint or bound. */
);
/*
* PROTECTED MEMBER VARIABLES
*/
protected:
SubjectToType type[NVMAX+NCMAX]; /**< Type of constraints/bounds. */
SubjectToStatus status[NVMAX+NCMAX]; /**< Status of constraints/bounds. */
BooleanType noLower; /**< This flag indicates if there is no lower bound on any variable. */
BooleanType noUpper; /**< This flag indicates if there is no upper bound on any variable. */
/*
* PRIVATE MEMBER VARIABLES
*/
private:
int size;
};
#include <SubjectTo.ipp>
#endif /* QPOASES_SUBJECTTO_HPP */
/*
* end of file
*/

131
phonelibs/qpoases/INCLUDE/Types.hpp 100644
View File

@ -0,0 +1,131 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Types.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2008
*
* Declaration of all non-built-in types (except for classes).
*/
#ifndef QPOASES_TYPES_HPP
#define QPOASES_TYPES_HPP
/** Define real_t for facilitating switching between double and float. */
// typedef double real_t;
/** Summarises all possible logical values. */
enum BooleanType
{
BT_FALSE, /**< Logical value for "false". */
BT_TRUE /**< Logical value for "true". */
};
/** Summarises all possible print levels. Print levels are used to describe
* the desired amount of output during runtime of qpOASES. */
enum PrintLevel
{
PL_NONE, /**< No output. */
PL_LOW, /**< Print error messages only. */
PL_MEDIUM, /**< Print error and warning messages as well as concise info messages. */
PL_HIGH /**< Print all messages with full details. */
};
/** Defines visibility status of a message. */
enum VisibilityStatus
{
VS_VISIBLE, /**< Message visible. */
VS_HIDDEN /**< Message not visible. */
};
/** Summarises all possible states of the (S)QProblem(B) object during the
solution process of a QP sequence. */
enum QProblemStatus
{
QPS_NOTINITIALISED, /**< QProblem object is freshly instantiated or reset. */
QPS_PREPARINGAUXILIARYQP, /**< An auxiliary problem is currently setup, either at the very beginning
* via an initial homotopy or after changing the QP matrices. */
QPS_AUXILIARYQPSOLVED, /**< An auxilary problem was solved, either at the very beginning
* via an initial homotopy or after changing the QP matrices. */
QPS_PERFORMINGHOMOTOPY, /**< A homotopy according to the main idea of the online active
* set strategy is performed. */
QPS_HOMOTOPYQPSOLVED, /**< An intermediate QP along the homotopy path was solved. */
QPS_SOLVED /**< The solution of the actual QP was found. */
};
/** Summarises all possible types of bounds and constraints. */
enum SubjectToType
{
ST_UNBOUNDED, /**< Bound/constraint is unbounded. */
ST_BOUNDED, /**< Bound/constraint is bounded but not fixed. */
ST_EQUALITY, /**< Bound/constraint is fixed (implicit equality bound/constraint). */
ST_UNKNOWN /**< Type of bound/constraint unknown. */
};
/** Summarises all possible states of bounds and constraints. */
enum SubjectToStatus
{
ST_INACTIVE, /**< Bound/constraint is inactive. */
ST_LOWER, /**< Bound/constraint is at its lower bound. */
ST_UPPER, /**< Bound/constraint is at its upper bound. */
ST_UNDEFINED /**< Status of bound/constraint undefined. */
};
/** Summarises all possible cycling states of bounds and constraints. */
enum CyclingStatus
{
CYC_NOT_INVOLVED, /**< Bound/constraint is not involved in current cycling. */
CYC_PREV_ADDED, /**< Bound/constraint has previously been added during the current cycling. */
CYC_PREV_REMOVED /**< Bound/constraint has previously been removed during the current cycling. */
};
/** Summarises all possible types of the QP's Hessian matrix. */
enum HessianType
{
HST_SEMIDEF, /**< Hessian is positive semi-definite. */
HST_POSDEF_NULLSPACE, /**< Hessian is positive definite on null space of active bounds/constraints. */
HST_POSDEF, /**< Hessian is (strictly) positive definite. */
HST_IDENTITY /**< Hessian is identity matrix. */
};
#endif /* QPOASES_TYPES_HPP */
/*
* end of file
*/

197
phonelibs/qpoases/INCLUDE/Utils.hpp 100644
View File

@ -0,0 +1,197 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file INCLUDE/Utils.hpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of global utility functions for working with qpOASES.
*/
#ifndef QPOASES_UTILS_HPP
#define QPOASES_UTILS_HPP
#include <MessageHandling.hpp>
#ifdef PC_DEBUG /* Define print functions only for debugging! */
/** Prints a vector.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const v, /**< Vector to be printed. */
int n /**< Length of vector. */
);
/** Prints a permuted vector.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const v, /**< Vector to be printed. */
int n, /**< Length of vector. */
const int* const V_idx /**< Pemutation vector. */
);
/** Prints a named vector.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const v, /**< Vector to be printed. */
int n, /**< Length of vector. */
const char* name /** Name of vector. */
);
/** Prints a matrix.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const M, /**< Matrix to be printed. */
int nrow, /**< Row number of matrix. */
int ncol /**< Column number of matrix. */
);
/** Prints a permuted matrix.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const M, /**< Matrix to be printed. */
int nrow, /**< Row number of matrix. */
int ncol , /**< Column number of matrix. */
const int* const ROW_idx, /**< Row pemutation vector. */
const int* const COL_idx /**< Column pemutation vector. */
);
/** Prints a named matrix.
* \return SUCCESSFUL_RETURN */
returnValue print( const real_t* const M, /**< Matrix to be printed. */
int nrow, /**< Row number of matrix. */
int ncol, /**< Column number of matrix. */
const char* name /** Name of matrix. */
);
/** Prints an index array.
* \return SUCCESSFUL_RETURN */
returnValue print( const int* const index, /**< Index array to be printed. */
int n /**< Length of index array. */
);
/** Prints a named index array.
* \return SUCCESSFUL_RETURN */
returnValue print( const int* const index, /**< Index array to be printed. */
int n, /**< Length of index array. */
const char* name /**< Name of index array. */
);
/** Prints a string to desired output target (useful also for MATLAB output!).
* \return SUCCESSFUL_RETURN */
returnValue myPrintf( const char* s /**< String to be written. */
);
/** Prints qpOASES copyright notice.
* \return SUCCESSFUL_RETURN */
returnValue printCopyrightNotice( );
/** Reads a real_t matrix from file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE \n
RET_UNABLE_TO_READ_FILE */
returnValue readFromFile( real_t* data, /**< Matrix to be read from file. */
int nrow, /**< Row number of matrix. */
int ncol, /**< Column number of matrix. */
const char* datafilename /**< Data file name. */
);
/** Reads a real_t vector from file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE \n
RET_UNABLE_TO_READ_FILE */
returnValue readFromFile( real_t* data, /**< Vector to be read from file. */
int n, /**< Length of vector. */
const char* datafilename /**< Data file name. */
);
/** Reads an integer (column) vector from file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE \n
RET_UNABLE_TO_READ_FILE */
returnValue readFromFile( int* data, /**< Vector to be read from file. */
int n, /**< Length of vector. */
const char* datafilename /**< Data file name. */
);
/** Writes a real_t matrix into a file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE */
returnValue writeIntoFile( const real_t* const data, /**< Matrix to be written into file. */
int nrow, /**< Row number of matrix. */
int ncol, /**< Column number of matrix. */
const char* datafilename, /**< Data file name. */
BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
);
/** Writes a real_t vector into a file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE */
returnValue writeIntoFile( const real_t* const data, /**< Vector to be written into file. */
int n, /**< Length of vector. */
const char* datafilename, /**< Data file name. */
BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
);
/** Writes an integer (column) vector into a file.
* \return SUCCESSFUL_RETURN \n
RET_UNABLE_TO_OPEN_FILE */
returnValue writeIntoFile( const int* const integer, /**< Integer vector to be written into file. */
int n, /**< Length of vector. */
const char* datafilename, /**< Data file name. */
BooleanType append /**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */
);
#endif /* PC_DEBUG */
/** Returns the current system time.
* \return current system time */
real_t getCPUtime( );
/** Returns the Euclidean norm of a vector.
* \return 0: successful */
real_t getNorm( const real_t* const v, /**< Vector. */
int n /**< Vector's dimension. */
);
/** Returns the absolute value of a real_t.
* \return Absolute value of a real_t */
inline real_t getAbs( real_t x /**< Input argument. */
);
#include <Utils.ipp>
#endif /* QPOASES_UTILS_HPP */
/*
* end of file
*/

504
phonelibs/qpoases/LICENSE.txt 100644
View File

@ -0,0 +1,504 @@
GNU LESSER GENERAL PUBLIC LICENSE
Version 2.1, February 1999
Copyright (C) 1991, 1999 Free Software Foundation, Inc.
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
[This is the first released version of the Lesser GPL. It also counts
as the successor of the GNU Library Public License, version 2, hence
the version number 2.1.]
Preamble
The licenses for most software are designed to take away your
freedom to share and change it. By contrast, the GNU General Public
Licenses are intended to guarantee your freedom to share and change
free software--to make sure the software is free for all its users.
This license, the Lesser General Public License, applies to some
specially designated software packages--typically libraries--of the
Free Software Foundation and other authors who decide to use it. You
can use it too, but we suggest you first think carefully about whether
this license or the ordinary General Public License is the better
strategy to use in any particular case, based on the explanations below.
When we speak of free software, we are referring to freedom of use,
not price. Our General Public Licenses are designed to make sure that
you have the freedom to distribute copies of free software (and charge
for this service if you wish); that you receive source code or can get
it if you want it; that you can change the software and use pieces of
it in new free programs; and that you are informed that you can do
these things.
To protect your rights, we need to make restrictions that forbid
distributors to deny you these rights or to ask you to surrender these
rights. These restrictions translate to certain responsibilities for
you if you distribute copies of the library or if you modify it.
For example, if you distribute copies of the library, whether gratis
or for a fee, you must give the recipients all the rights that we gave
you. You must make sure that they, too, receive or can get the source
code. If you link other code with the library, you must provide
complete object files to the recipients, so that they can relink them
with the library after making changes to the library and recompiling
it. And you must show them these terms so they know their rights.
We protect your rights with a two-step method: (1) we copyright the
library, and (2) we offer you this license, which gives you legal
permission to copy, distribute and/or modify the library.
To protect each distributor, we want to make it very clear that
there is no warranty for the free library. Also, if the library is
modified by someone else and passed on, the recipients should know
that what they have is not the original version, so that the original
author's reputation will not be affected by problems that might be
introduced by others.
Finally, software patents pose a constant threat to the existence of
any free program. We wish to make sure that a company cannot
effectively restrict the users of a free program by obtaining a
restrictive license from a patent holder. Therefore, we insist that
any patent license obtained for a version of the library must be
consistent with the full freedom of use specified in this license.
Most GNU software, including some libraries, is covered by the
ordinary GNU General Public License. This license, the GNU Lesser
General Public License, applies to certain designated libraries, and
is quite different from the ordinary General Public License. We use
this license for certain libraries in order to permit linking those
libraries into non-free programs.
When a program is linked with a library, whether statically or using
a shared library, the combination of the two is legally speaking a
combined work, a derivative of the original library. The ordinary
General Public License therefore permits such linking only if the
entire combination fits its criteria of freedom. The Lesser General
Public License permits more lax criteria for linking other code with
the library.
We call this license the "Lesser" General Public License because it
does Less to protect the user's freedom than the ordinary General
Public License. It also provides other free software developers Less
of an advantage over competing non-free programs. These disadvantages
are the reason we use the ordinary General Public License for many
libraries. However, the Lesser license provides advantages in certain
special circumstances.
For example, on rare occasions, there may be a special need to
encourage the widest possible use of a certain library, so that it becomes
a de-facto standard. To achieve this, non-free programs must be
allowed to use the library. A more frequent case is that a free
library does the same job as widely used non-free libraries. In this
case, there is little to gain by limiting the free library to free
software only, so we use the Lesser General Public License.
In other cases, permission to use a particular library in non-free
programs enables a greater number of people to use a large body of
free software. For example, permission to use the GNU C Library in
non-free programs enables many more people to use the whole GNU
operating system, as well as its variant, the GNU/Linux operating
system.
Although the Lesser General Public License is Less protective of the
users' freedom, it does ensure that the user of a program that is
linked with the Library has the freedom and the wherewithal to run
that program using a modified version of the Library.
The precise terms and conditions for copying, distribution and
modification follow. Pay close attention to the difference between a
"work based on the library" and a "work that uses the library". The
former contains code derived from the library, whereas the latter must
be combined with the library in order to run.
GNU LESSER GENERAL PUBLIC LICENSE
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
0. This License Agreement applies to any software library or other
program which contains a notice placed by the copyright holder or
other authorized party saying it may be distributed under the terms of
this Lesser General Public License (also called "this License").
Each licensee is addressed as "you".
A "library" means a collection of software functions and/or data
prepared so as to be conveniently linked with application programs
(which use some of those functions and data) to form executables.
The "Library", below, refers to any such software library or work
which has been distributed under these terms. A "work based on the
Library" means either the Library or any derivative work under
copyright law: that is to say, a work containing the Library or a
portion of it, either verbatim or with modifications and/or translated
straightforwardly into another language. (Hereinafter, translation is
included without limitation in the term "modification".)
"Source code" for a work means the preferred form of the work for
making modifications to it. For a library, complete source code means
all the source code for all modules it contains, plus any associated
interface definition files, plus the scripts used to control compilation
and installation of the library.
Activities other than copying, distribution and modification are not
covered by this License; they are outside its scope. The act of
running a program using the Library is not restricted, and output from
such a program is covered only if its contents constitute a work based
on the Library (independent of the use of the Library in a tool for
writing it). Whether that is true depends on what the Library does
and what the program that uses the Library does.
1. You may copy and distribute verbatim copies of the Library's
complete source code as you receive it, in any medium, provided that
you conspicuously and appropriately publish on each copy an
appropriate copyright notice and disclaimer of warranty; keep intact
all the notices that refer to this License and to the absence of any
warranty; and distribute a copy of this License along with the
Library.
You may charge a fee for the physical act of transferring a copy,
and you may at your option offer warranty protection in exchange for a
fee.
2. You may modify your copy or copies of the Library or any portion
of it, thus forming a work based on the Library, and copy and
distribute such modifications or work under the terms of Section 1
above, provided that you also meet all of these conditions:
a) The modified work must itself be a software library.
b) You must cause the files modified to carry prominent notices
stating that you changed the files and the date of any change.
c) You must cause the whole of the work to be licensed at no
charge to all third parties under the terms of this License.
d) If a facility in the modified Library refers to a function or a
table of data to be supplied by an application program that uses
the facility, other than as an argument passed when the facility
is invoked, then you must make a good faith effort to ensure that,
in the event an application does not supply such function or
table, the facility still operates, and performs whatever part of
its purpose remains meaningful.
(For example, a function in a library to compute square roots has
a purpose that is entirely well-defined independent of the
application. Therefore, Subsection 2d requires that any
application-supplied function or table used by this function must
be optional: if the application does not supply it, the square
root function must still compute square roots.)
These requirements apply to the modified work as a whole. If
identifiable sections of that work are not derived from the Library,
and can be reasonably considered independent and separate works in
themselves, then this License, and its terms, do not apply to those
sections when you distribute them as separate works. But when you
distribute the same sections as part of a whole which is a work based
on the Library, the distribution of the whole must be on the terms of
this License, whose permissions for other licensees extend to the
entire whole, and thus to each and every part regardless of who wrote
it.
Thus, it is not the intent of this section to claim rights or contest
your rights to work written entirely by you; rather, the intent is to
exercise the right to control the distribution of derivative or
collective works based on the Library.
In addition, mere aggregation of another work not based on the Library
with the Library (or with a work based on the Library) on a volume of
a storage or distribution medium does not bring the other work under
the scope of this License.
3. You may opt to apply the terms of the ordinary GNU General Public
License instead of this License to a given copy of the Library. To do
this, you must alter all the notices that refer to this License, so
that they refer to the ordinary GNU General Public License, version 2,
instead of to this License. (If a newer version than version 2 of the
ordinary GNU General Public License has appeared, then you can specify
that version instead if you wish.) Do not make any other change in
these notices.
Once this change is made in a given copy, it is irreversible for
that copy, so the ordinary GNU General Public License applies to all
subsequent copies and derivative works made from that copy.
This option is useful when you wish to copy part of the code of
the Library into a program that is not a library.
4. You may copy and distribute the Library (or a portion or
derivative of it, under Section 2) in object code or executable form
under the terms of Sections 1 and 2 above provided that you accompany
it with the complete corresponding machine-readable source code, which
must be distributed under the terms of Sections 1 and 2 above on a
medium customarily used for software interchange.
If distribution of object code is made by offering access to copy
from a designated place, then offering equivalent access to copy the
source code from the same place satisfies the requirement to
distribute the source code, even though third parties are not
compelled to copy the source along with the object code.
5. A program that contains no derivative of any portion of the
Library, but is designed to work with the Library by being compiled or
linked with it, is called a "work that uses the Library". Such a
work, in isolation, is not a derivative work of the Library, and
therefore falls outside the scope of this License.
However, linking a "work that uses the Library" with the Library
creates an executable that is a derivative of the Library (because it
contains portions of the Library), rather than a "work that uses the
library". The executable is therefore covered by this License.
Section 6 states terms for distribution of such executables.
When a "work that uses the Library" uses material from a header file
that is part of the Library, the object code for the work may be a
derivative work of the Library even though the source code is not.
Whether this is true is especially significant if the work can be
linked without the Library, or if the work is itself a library. The
threshold for this to be true is not precisely defined by law.
If such an object file uses only numerical parameters, data
structure layouts and accessors, and small macros and small inline
functions (ten lines or less in length), then the use of the object
file is unrestricted, regardless of whether it is legally a derivative
work. (Executables containing this object code plus portions of the
Library will still fall under Section 6.)
Otherwise, if the work is a derivative of the Library, you may
distribute the object code for the work under the terms of Section 6.
Any executables containing that work also fall under Section 6,
whether or not they are linked directly with the Library itself.
6. As an exception to the Sections above, you may also combine or
link a "work that uses the Library" with the Library to produce a
work containing portions of the Library, and distribute that work
under terms of your choice, provided that the terms permit
modification of the work for the customer's own use and reverse
engineering for debugging such modifications.
You must give prominent notice with each copy of the work that the
Library is used in it and that the Library and its use are covered by
this License. You must supply a copy of this License. If the work
during execution displays copyright notices, you must include the
copyright notice for the Library among them, as well as a reference
directing the user to the copy of this License. Also, you must do one
of these things:
a) Accompany the work with the complete corresponding
machine-readable source code for the Library including whatever
changes were used in the work (which must be distributed under
Sections 1 and 2 above); and, if the work is an executable linked
with the Library, with the complete machine-readable "work that
uses the Library", as object code and/or source code, so that the
user can modify the Library and then relink to produce a modified
executable containing the modified Library. (It is understood
that the user who changes the contents of definitions files in the
Library will not necessarily be able to recompile the application
to use the modified definitions.)
b) Use a suitable shared library mechanism for linking with the
Library. A suitable mechanism is one that (1) uses at run time a
copy of the library already present on the user's computer system,
rather than copying library functions into the executable, and (2)
will operate properly with a modified version of the library, if
the user installs one, as long as the modified version is
interface-compatible with the version that the work was made with.
c) Accompany the work with a written offer, valid for at
least three years, to give the same user the materials
specified in Subsection 6a, above, for a charge no more
than the cost of performing this distribution.
d) If distribution of the work is made by offering access to copy
from a designated place, offer equivalent access to copy the above
specified materials from the same place.
e) Verify that the user has already received a copy of these
materials or that you have already sent this user a copy.
For an executable, the required form of the "work that uses the
Library" must include any data and utility programs needed for
reproducing the executable from it. However, as a special exception,
the materials to be distributed need not include anything that is
normally distributed (in either source or binary form) with the major
components (compiler, kernel, and so on) of the operating system on
which the executable runs, unless that component itself accompanies
the executable.
It may happen that this requirement contradicts the license
restrictions of other proprietary libraries that do not normally
accompany the operating system. Such a contradiction means you cannot
use both them and the Library together in an executable that you
distribute.
7. You may place library facilities that are a work based on the
Library side-by-side in a single library together with other library
facilities not covered by this License, and distribute such a combined
library, provided that the separate distribution of the work based on
the Library and of the other library facilities is otherwise
permitted, and provided that you do these two things:
a) Accompany the combined library with a copy of the same work
based on the Library, uncombined with any other library
facilities. This must be distributed under the terms of the
Sections above.
b) Give prominent notice with the combined library of the fact
that part of it is a work based on the Library, and explaining
where to find the accompanying uncombined form of the same work.
8. You may not copy, modify, sublicense, link with, or distribute
the Library except as expressly provided under this License. Any
attempt otherwise to copy, modify, sublicense, link with, or
distribute the Library is void, and will automatically terminate your
rights under this License. However, parties who have received copies,
or rights, from you under this License will not have their licenses
terminated so long as such parties remain in full compliance.
9. You are not required to accept this License, since you have not
signed it. However, nothing else grants you permission to modify or
distribute the Library or its derivative works. These actions are
prohibited by law if you do not accept this License. Therefore, by
modifying or distributing the Library (or any work based on the
Library), you indicate your acceptance of this License to do so, and
all its terms and conditions for copying, distributing or modifying
the Library or works based on it.
10. Each time you redistribute the Library (or any work based on the
Library), the recipient automatically receives a license from the
original licensor to copy, distribute, link with or modify the Library
subject to these terms and conditions. You may not impose any further
restrictions on the recipients' exercise of the rights granted herein.
You are not responsible for enforcing compliance by third parties with
this License.
11. If, as a consequence of a court judgment or allegation of patent
infringement or for any other reason (not limited to patent issues),
conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot
distribute so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you
may not distribute the Library at all. For example, if a patent
license would not permit royalty-free redistribution of the Library by
all those who receive copies directly or indirectly through you, then
the only way you could satisfy both it and this License would be to
refrain entirely from distribution of the Library.
If any portion of this section is held invalid or unenforceable under any
particular circumstance, the balance of the section is intended to apply,
and the section as a whole is intended to apply in other circumstances.
It is not the purpose of this section to induce you to infringe any
patents or other property right claims or to contest validity of any
such claims; this section has the sole purpose of protecting the
integrity of the free software distribution system which is
implemented by public license practices. Many people have made
generous contributions to the wide range of software distributed
through that system in reliance on consistent application of that
system; it is up to the author/donor to decide if he or she is willing
to distribute software through any other system and a licensee cannot
impose that choice.
This section is intended to make thoroughly clear what is believed to
be a consequence of the rest of this License.
12. If the distribution and/or use of the Library is restricted in
certain countries either by patents or by copyrighted interfaces, the
original copyright holder who places the Library under this License may add
an explicit geographical distribution limitation excluding those countries,
so that distribution is permitted only in or among countries not thus
excluded. In such case, this License incorporates the limitation as if
written in the body of this License.
13. The Free Software Foundation may publish revised and/or new
versions of the Lesser General Public License from time to time.
Such new versions will be similar in spirit to the present version,
but may differ in detail to address new problems or concerns.
Each version is given a distinguishing version number. If the Library
specifies a version number of this License which applies to it and
"any later version", you have the option of following the terms and
conditions either of that version or of any later version published by
the Free Software Foundation. If the Library does not specify a
license version number, you may choose any version ever published by
the Free Software Foundation.
14. If you wish to incorporate parts of the Library into other free
programs whose distribution conditions are incompatible with these,
write to the author to ask for permission. For software which is
copyrighted by the Free Software Foundation, write to the Free
Software Foundation; we sometimes make exceptions for this. Our
decision will be guided by the two goals of preserving the free status
of all derivatives of our free software and of promoting the sharing
and reuse of software generally.
NO WARRANTY
15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
DAMAGES.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Libraries
If you develop a new library, and you want it to be of the greatest
possible use to the public, we recommend making it free software that
everyone can redistribute and change. You can do so by permitting
redistribution under these terms (or, alternatively, under the terms of the
ordinary General Public License).
To apply these terms, attach the following notices to the library. It is
safest to attach them to the start of each source file to most effectively
convey the exclusion of warranty; and each file should have at least the
"copyright" line and a pointer to where the full notice is found.
<one line to give the library's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
Also add information on how to contact you by electronic and paper mail.
You should also get your employer (if you work as a programmer) or your
school, if any, to sign a "copyright disclaimer" for the library, if
necessary. Here is a sample; alter the names:
Yoyodyne, Inc., hereby disclaims all copyright interest in the
library `Frob' (a library for tweaking knobs) written by James Random Hacker.
<signature of Ty Coon>, 1 April 1990
Ty Coon, President of Vice
That's all there is to it!

92
phonelibs/qpoases/README.txt 100644
View File

@ -0,0 +1,92 @@
##
## qpOASES -- An Implementation of the Online Active Set Strategy.
## Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
##
## qpOASES is free software; you can redistribute it and/or
## modify it under the terms of the GNU Lesser General Public
## License as published by the Free Software Foundation; either
## version 2.1 of the License, or (at your option) any later version.
##
## qpOASES is distributed in the hope that it will be useful,
## but WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
## Lesser General Public License for more details.
##
## You should have received a copy of the GNU Lesser General Public
## License along with qpOASES; if not, write to the Free Software
## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
##
INTRODUCTION
=============
qpOASES is an open-source C++ implementation of the recently proposed
online active set strategy (see [1], [2]), which was inspired by important
observations from the field of parametric quadratic programming. It has
several theoretical features that make it particularly suited for model
predictive control (MPC) applications.
The software package qpOASES implements these ideas and has already been
successfully used for closed-loop control of a real-world Diesel engine [3].
References:
[1] H.J. Ferreau. An Online Active Set Strategy for Fast Solution of
Parametric Quadratic Programs with Applications to Predictive Engine Control.
Diplom thesis, University of Heidelberg, 2006.
[2] H.J. Ferreau, H.G. Bock, M. Diehl. An online active set strategy to
overcome the limitations of explicit MPC. International Journal of Robust
and Nonlinear Control, 18 (8), pp. 816-830, 2008.
[3] H.J. Ferreau, P. Ortner, P. Langthaler, L. del Re, M. Diehl. Predictive
Control of a Real-World Diesel Engine using an Extended Online Active Set
Strategy. Annual Reviews in Control, 31 (2), pp. 293-301, 2007.
GETTING STARTED
================
1. For installation, usage and additional information on this software package
see the qpOASES User's Manual located at ./DOC/manual.pdf!
2. The file ./LICENSE.txt contains a copy of the GNU Lesser General Public
License. Please read it carefully before using qpOASES!
3. The whole software package can be downloaded from
http://homes.esat.kuleuven.be/~optec/software/qpOASES/
On this webpage you will also find a list of frequently asked questions.
CONTACT THE AUTHORS
====================
If you have got questions, remarks or comments on qpOASES
please contact the main author:
Hans Joachim Ferreau
Katholieke Universiteit Leuven
Department of Electrical Engineering (ESAT)
Kasteelpark Arenberg 10, bus 2446
B-3001 Leuven-Heverlee, Belgium
Phone: +32 16 32 03 63
E-mail: joachim.ferreau@esat.kuleuven.be
qpOASES@esat.kuleuven.be
Also bug reports and source code extensions are most welcome!
##
## end of file
##

252
phonelibs/qpoases/SRC/Bounds.cpp 100644
View File

@ -0,0 +1,252 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/Bounds.cpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of the Bounds class designed to manage working sets of
* bounds within a QProblem.
*/
#include <Bounds.hpp>
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* B o u n d s
*/
Bounds::Bounds( ) : SubjectTo( ),
nV( 0 ),
nFV( 0 ),
nBV( 0 ),
nUV( 0 )
{
}
/*
* B o u n d s
*/
Bounds::Bounds( const Bounds& rhs ) : SubjectTo( rhs ),
nV( rhs.nV ),
nFV( rhs.nFV ),
nBV( rhs.nBV ),
nUV( rhs.nUV )
{
free = rhs.free;
fixed = rhs.fixed;
}
/*
* ~ B o u n d s
*/
Bounds::~Bounds( )
{
}
/*
* o p e r a t o r =
*/
Bounds& Bounds::operator=( const Bounds& rhs )
{
if ( this != &rhs )
{
SubjectTo::operator=( rhs );
nV = rhs.nV;
nFV = rhs.nFV;
nBV = rhs.nBV;
nUV = rhs.nUV;
free = rhs.free;
fixed = rhs.fixed;
}
return *this;
}
/*
* i n i t
*/
returnValue Bounds::init( int n )
{
nV = n;
nFV = 0;
nBV = 0;
nUV = 0;
free.init( );
fixed.init( );
return SubjectTo::init( n );
}
/*
* s e t u p B o u n d
*/
returnValue Bounds::setupBound( int _number, SubjectToStatus _status
)
{
/* consistency check */
if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
/* Add bound index to respective index list. */
switch ( _status )
{
case ST_INACTIVE:
if ( this->addIndex( this->getFree( ),_number,_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_BOUND_FAILED );
break;
case ST_LOWER:
if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_BOUND_FAILED );
break;
case ST_UPPER:
if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_BOUND_FAILED );
break;
default:
return THROWERROR( RET_INVALID_ARGUMENTS );
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p A l l F r e e
*/
returnValue Bounds::setupAllFree( )
{
int i;
/* 1) Place unbounded variables at the beginning of the index list of free variables. */
for( i=0; i<nV; ++i )
{
if ( getType( i ) == ST_UNBOUNDED )
{
if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_BOUND_FAILED );
}
}
/* 2) Add remaining (i.e. bounded but possibly free) variables to the index list of free variables. */
for( i=0; i<nV; ++i )
{
if ( getType( i ) == ST_BOUNDED )
{
if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_BOUND_FAILED );
}
}
/* 3) Place implicitly fixed variables at the end of the index list of free variables. */
for( i=0; i<nV; ++i )
{
if ( getType( i ) == ST_EQUALITY )
{
if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_BOUND_FAILED );
}
}
return SUCCESSFUL_RETURN;
}
/*
* m o v e F i x e d T o F r e e
*/
returnValue Bounds::moveFixedToFree( int _number )
{
/* consistency check */
if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
/* Move index from indexlist of fixed variables to that of free ones. */
if ( this->removeIndex( this->getFixed( ),_number ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_MOVING_BOUND_FAILED );
if ( this->addIndex( this->getFree( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_MOVING_BOUND_FAILED );
return SUCCESSFUL_RETURN;
}
/*
* m o v e F r e e T o F i x e d
*/
returnValue Bounds::moveFreeToFixed( int _number, SubjectToStatus _status
)
{
/* consistency check */
if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
/* Move index from indexlist of free variables to that of fixed ones. */
if ( this->removeIndex( this->getFree( ),_number ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_MOVING_BOUND_FAILED );
if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_MOVING_BOUND_FAILED );
return SUCCESSFUL_RETURN;
}
/*
* s w a p F r e e
*/
returnValue Bounds::swapFree( int number1, int number2
)
{
/* consistency check */
if ( ( number1 < 0 ) || ( number1 >= getNV( ) ) || ( number2 < 0 ) || ( number2 >= getNV( ) ) )
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
/* Swap index within indexlist of free variables. */
return this->swapIndex( this->getFree( ),number1,number2 );
}
/*
* end of file
*/

144
phonelibs/qpoases/SRC/Bounds.ipp 100644
View File

@ -0,0 +1,144 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/Bounds.ipp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of inlined member functions of the Bounds class designed
* to manage working sets of bounds within a QProblem.
*/
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* g e t N V
*/
inline int Bounds::getNV( ) const
{
return nV;
}
/*
* g e t N F X
*/
inline int Bounds::getNFV( ) const
{
return nFV;
}
/*
* g e t N B V
*/
inline int Bounds::getNBV( ) const
{
return nBV;
}
/*
* g e t N U V
*/
inline int Bounds::getNUV( ) const
{
return nUV;
}
/*
* s e t N F X
*/
inline returnValue Bounds::setNFV( int n )
{
nFV = n;
return SUCCESSFUL_RETURN;
}
/*
* s e t N B V
*/
inline returnValue Bounds::setNBV( int n )
{
nBV = n;
return SUCCESSFUL_RETURN;
}
/*
* s e t N U V
*/
inline returnValue Bounds::setNUV( int n )
{
nUV = n;
return SUCCESSFUL_RETURN;
}
/*
* g e t N F R
*/
inline int Bounds::getNFR( )
{
return free.getLength( );
}
/*
* g e t N F X
*/
inline int Bounds::getNFX( )
{
return fixed.getLength( );
}
/*
* g e t F r e e
*/
inline Indexlist* Bounds::getFree( )
{
return &free;
}
/*
* g e t F i x e d
*/
inline Indexlist* Bounds::getFixed( )
{
return &fixed;
}
/*
* end of file
*/

248
phonelibs/qpoases/SRC/Constraints.cpp 100644
View File

@ -0,0 +1,248 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/Constraints.cpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of the Constraints class designed to manage working sets of
* constraints within a QProblem.
*/
#include <Constraints.hpp>
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* C o n s t r a i n t s
*/
Constraints::Constraints( ) : SubjectTo( ),
nC( 0 ),
nEC( 0 ),
nIC( 0 ),
nUC( 0 )
{
}
/*
* C o n s t r a i n t s
*/
Constraints::Constraints( const Constraints& rhs ) : SubjectTo( rhs ),
nC( rhs.nC ),
nEC( rhs.nEC ),
nIC( rhs.nIC ),
nUC( rhs.nUC )
{
active = rhs.active;
inactive = rhs.inactive;
}
/*
* ~ C o n s t r a i n t s
*/
Constraints::~Constraints( )
{
}
/*
* o p e r a t o r =
*/
Constraints& Constraints::operator=( const Constraints& rhs )
{
if ( this != &rhs )
{
SubjectTo::operator=( rhs );
nC = rhs.nC;
nEC = rhs.nEC;
nIC = rhs.nIC;
nUC = rhs.nUC;
active = rhs.active;
inactive = rhs.inactive;
}
return *this;
}
/*
* i n i t
*/
returnValue Constraints::init( int n )
{
nC = n;
nEC = 0;
nIC = 0;
nUC = 0;
active.init( );
inactive.init( );
return SubjectTo::init( n );
}
/*
* s e t u p C o n s t r a i n t
*/
returnValue Constraints::setupConstraint( int _number, SubjectToStatus _status
)
{
/* consistency check */
if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
/* Add constraint index to respective index list. */
switch ( _status )
{
case ST_INACTIVE:
if ( this->addIndex( this->getInactive( ),_number,_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
break;
case ST_LOWER:
if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
break;
case ST_UPPER:
if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
break;
default:
return THROWERROR( RET_INVALID_ARGUMENTS );
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p A l l I n a c t i v e
*/
returnValue Constraints::setupAllInactive( )
{
int i;
/* 1) Place unbounded constraints at the beginning of the index list of inactive constraints. */
for( i=0; i<nC; ++i )
{
if ( getType( i ) == ST_UNBOUNDED )
{
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
}
}
/* 2) Add remaining (i.e. "real" inequality) constraints to the index list of inactive constraints. */
for( i=0; i<nC; ++i )
{
if ( getType( i ) == ST_BOUNDED )
{
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
}
}
/* 3) Place implicit equality constraints at the end of the index list of inactive constraints. */
for( i=0; i<nC; ++i )
{
if ( getType( i ) == ST_EQUALITY )
{
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
}
}
/* 4) Moreover, add all constraints of unknown type. */
for( i=0; i<nC; ++i )
{
if ( getType( i ) == ST_UNKNOWN )
{
if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
}
}
return SUCCESSFUL_RETURN;
}
/*
* m o v e A c t i v e T o I n a c t i v e
*/
returnValue Constraints::moveActiveToInactive( int _number )
{
/* consistency check */
if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
/* Move index from indexlist of active constraints to that of inactive ones. */
if ( this->removeIndex( this->getActive( ),_number ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_MOVING_BOUND_FAILED );
if ( this->addIndex( this->getInactive( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_MOVING_BOUND_FAILED );
return SUCCESSFUL_RETURN;
}
/*
* m o v e I n a c t i v e T o A c t i v e
*/
returnValue Constraints::moveInactiveToActive( int _number, SubjectToStatus _status
)
{
/* consistency check */
if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
/* Move index from indexlist of inactive constraints to that of active ones. */
if ( this->removeIndex( this->getInactive( ),_number ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_MOVING_BOUND_FAILED );
if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_MOVING_BOUND_FAILED );
return SUCCESSFUL_RETURN;
}
/*
* end of file
*/

144
phonelibs/qpoases/SRC/Constraints.ipp 100644
View File

@ -0,0 +1,144 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/Constraints.ipp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Declaration of inlined member functions of the Constraints class designed
* to manage working sets of constraints within a QProblem.
*/
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* g e t N C
*/
inline int Constraints::getNC( ) const
{
return nC;
}
/*
* g e t N E C
*/
inline int Constraints::getNEC( ) const
{
return nEC;
}
/*
* g e t N I C
*/
inline int Constraints::getNIC( ) const
{
return nIC;
}
/*
* g e t N U C
*/
inline int Constraints::getNUC( ) const
{
return nUC;
}
/*
* s e t N E C
*/
inline returnValue Constraints::setNEC( int n )
{
nEC = n;
return SUCCESSFUL_RETURN;
}
/*
* s e t N I C
*/
inline returnValue Constraints::setNIC( int n )
{
nIC = n;
return SUCCESSFUL_RETURN;
}
/*
* s e t N U C
*/
inline returnValue Constraints::setNUC( int n )
{
nUC = n;
return SUCCESSFUL_RETURN;
}
/*
* g e t N A C
*/
inline int Constraints::getNAC( )
{
return active.getLength( );
}
/*
* g e t N I A C
*/
inline int Constraints::getNIAC( )
{
return inactive.getLength( );
}
/*
* g e t A c t i v e
*/
inline Indexlist* Constraints::getActive( )
{
return &active;
}
/*
* g e t I n a c t i v e
*/
inline Indexlist* Constraints::getInactive( )
{
return &inactive;
}
/*
* end of file
*/

188
phonelibs/qpoases/SRC/CyclingManager.cpp 100644
View File

@ -0,0 +1,188 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/CyclingManager.cpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of the CyclingManager class designed to detect
* and handle possible cycling during QP iterations.
*
*/
#include <CyclingManager.hpp>
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* C y c l i n g M a n a g e r
*/
CyclingManager::CyclingManager( ) : nV( 0 ),
nC( 0 )
{
cyclingDetected = BT_FALSE;
}
/*
* C y c l i n g M a n a g e r
*/
CyclingManager::CyclingManager( const CyclingManager& rhs ) : nV( rhs.nV ),
nC( rhs.nC ),
cyclingDetected( rhs.cyclingDetected )
{
int i;
for( i=0; i<nV+nC; ++i )
status[i] = rhs.status[i];
}
/*
* ~ C y c l i n g M a n a g e r
*/
CyclingManager::~CyclingManager( )
{
}
/*
* o p e r a t o r =
*/
CyclingManager& CyclingManager::operator=( const CyclingManager& rhs )
{
int i;
if ( this != &rhs )
{
nV = rhs.nV;
nC = rhs.nC;
for( i=0; i<nV+nC; ++i )
status[i] = rhs.status[i];
cyclingDetected = rhs.cyclingDetected;
}
return *this;
}
/*
* i n i t
*/
returnValue CyclingManager::init( int _nV, int _nC )
{
nV = _nV;
nC = _nC;
cyclingDetected = BT_FALSE;
return SUCCESSFUL_RETURN;
}
/*
* s e t C y c l i n g S t a t u s
*/
returnValue CyclingManager::setCyclingStatus( int number,
BooleanType isBound, CyclingStatus _status
)
{
if ( isBound == BT_TRUE )
{
/* Set cycling status of a bound. */
if ( ( number >= 0 ) && ( number < nV ) )
{
status[number] = _status;
return SUCCESSFUL_RETURN;
}
else
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
else
{
/* Set cycling status of a constraint. */
if ( ( number >= 0 ) && ( number < nC ) )
{
status[nV+number] = _status;
return SUCCESSFUL_RETURN;
}
else
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
}
/*
* g e t C y c l i n g S t a t u s
*/
CyclingStatus CyclingManager::getCyclingStatus( int number, BooleanType isBound ) const
{
if ( isBound == BT_TRUE )
{
/* Return cycling status of a bound. */
if ( ( number >= 0 ) && ( number < nV ) )
return status[number];
}
else
{
/* Return cycling status of a constraint. */
if ( ( number >= 0 ) && ( number < nC ) )
return status[nV+number];
}
return CYC_NOT_INVOLVED;
}
/*
* c l e a r C y c l i n g D a t a
*/
returnValue CyclingManager::clearCyclingData( )
{
int i;
/* Reset all status values ... */
for( i=0; i<nV+nC; ++i )
status[i] = CYC_NOT_INVOLVED;
/* ... and the main cycling flag. */
cyclingDetected = BT_FALSE;
return SUCCESSFUL_RETURN;
}
/*
* end of file
*/

51
phonelibs/qpoases/SRC/CyclingManager.ipp 100644
View File

@ -0,0 +1,51 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/CyclingManager.ipp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of inlined member functions of the CyclingManager class
* designed to detect and handle possible cycling during QP iterations.
*
*/
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* i s C y c l i n g D e t e c t e d
*/
inline BooleanType CyclingManager::isCyclingDetected( ) const
{
return cyclingDetected;
}
/*
* end of file
*/

434
phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp 100644
View File

@ -0,0 +1,434 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/EXTRAS/SolutionAnalysis.cpp
* \author Milan Vukov, Boris Houska, Hans Joachim Ferreau
* \version 1.3embedded
* \date 2012
*
* Solution analysis class, based on a class in the standard version of the qpOASES
*/
#include <EXTRAS/SolutionAnalysis.hpp>
/*
* S o l u t i o n A n a l y s i s
*/
SolutionAnalysis::SolutionAnalysis( )
{
}
/*
* S o l u t i o n A n a l y s i s
*/
SolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs )
{
}
/*
* ~ S o l u t i o n A n a l y s i s
*/
SolutionAnalysis::~SolutionAnalysis( )
{
}
/*
* o p e r a t o r =
*/
SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs )
{
if ( this != &rhs )
{
}
return *this;
}
/*
* g e t H e s s i a n I n v e r s e
*/
returnValue SolutionAnalysis::getHessianInverse( QProblem* qp, real_t* hessianInverse )
{
returnValue returnvalue; /* the return value */
BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
register int run1, run2, run3;
register int nFR, nFX;
/* Ask for the number of free and fixed variables, assumes that active set
* is constant for the covariance evaluation */
nFR = qp->getNFR( );
nFX = qp->getNFX( );
/* Ask for the corresponding index arrays: */
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
/* Initialization: */
for( run1 = 0; run1 < NVMAX; run1++ )
delta_g_cov[ run1 ] = 0.0;
for( run1 = 0; run1 < NVMAX; run1++ )
delta_lb_cov[ run1 ] = 0.0;
for( run1 = 0; run1 < NVMAX; run1++ )
delta_ub_cov[ run1 ] = 0.0;
for( run1 = 0; run1 < NCMAX; run1++ )
delta_lbA_cov[ run1 ] = 0.0;
for( run1 = 0; run1 < NCMAX; run1++ )
delta_ubA_cov[ run1 ] = 0.0;
/* The following loop solves the following:
*
* KKT * x =
* [delta_g_cov', delta_lbA_cov', delta_ubA_cov', delta_lb_cov', delta_ub_cov]'
*
* for the first NVMAX (negative) elementary vectors in order to get
* transposed inverse of the Hessian. Assuming that the Hessian is
* symmetric, the function will return transposed inverse, instead of the
* true inverse.
*
* Note, that we use negative elementary vectors due because internal
* implementation of the function hotstart_determineStepDirection requires
* so.
*
* */
for( run3 = 0; run3 < NVMAX; run3++ )
{
/* Line wise loading of the corresponding (negative) elementary vector: */
delta_g_cov[ run3 ] = -1.0;
/* Evaluation of the step: */
returnvalue = qp->hotstart_determineStepDirection(
FR_idx, FX_idx, AC_idx,
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
Delta_bC_isZero, Delta_bB_isZero,
delta_xFX, delta_xFR, delta_yAC, delta_yFX
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
return returnvalue;
}
/* Line wise storage of the QP reaction: */
for( run1 = 0; run1 < nFR; run1++ )
{
run2 = FR_idx[ run1 ];
hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
}
for( run1 = 0; run1 < nFX; run1++ )
{
run2 = FX_idx[ run1 ];
hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
}
/* Prepare for the next iteration */
delta_g_cov[ run3 ] = 0.0;
}
// TODO: Perform the transpose of the inverse of the Hessian matrix
return SUCCESSFUL_RETURN;
}
/*
* g e t H e s s i a n I n v e r s e
*/
returnValue SolutionAnalysis::getHessianInverse( QProblemB* qp, real_t* hessianInverse )
{
returnValue returnvalue; /* the return value */
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
register int run1, run2, run3;
register int nFR, nFX;
/* Ask for the number of free and fixed variables, assumes that active set
* is constant for the covariance evaluation */
nFR = qp->getNFR( );
nFX = qp->getNFX( );
/* Ask for the corresponding index arrays: */
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
/* Initialization: */
for( run1 = 0; run1 < NVMAX; run1++ )
delta_g_cov[ run1 ] = 0.0;
for( run1 = 0; run1 < NVMAX; run1++ )
delta_lb_cov[ run1 ] = 0.0;
for( run1 = 0; run1 < NVMAX; run1++ )
delta_ub_cov[ run1 ] = 0.0;
/* The following loop solves the following:
*
* KKT * x =
* [delta_g_cov', delta_lb_cov', delta_ub_cov']'
*
* for the first NVMAX (negative) elementary vectors in order to get
* transposed inverse of the Hessian. Assuming that the Hessian is
* symmetric, the function will return transposed inverse, instead of the
* true inverse.
*
* Note, that we use negative elementary vectors due because internal
* implementation of the function hotstart_determineStepDirection requires
* so.
*
* */
for( run3 = 0; run3 < NVMAX; run3++ )
{
/* Line wise loading of the corresponding (negative) elementary vector: */
delta_g_cov[ run3 ] = -1.0;
/* Evaluation of the step: */
returnvalue = qp->hotstart_determineStepDirection(
FR_idx, FX_idx,
delta_g_cov, delta_lb_cov, delta_ub_cov,
Delta_bB_isZero,
delta_xFX, delta_xFR, delta_yFX
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
return returnvalue;
}
/* Line wise storage of the QP reaction: */
for( run1 = 0; run1 < nFR; run1++ )
{
run2 = FR_idx[ run1 ];
hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
}
for( run1 = 0; run1 < nFX; run1++ )
{
run2 = FX_idx[ run1 ];
hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
}
/* Prepare for the next iteration */
delta_g_cov[ run3 ] = 0.0;
}
// TODO: Perform the transpose of the inverse of the Hessian matrix
return SUCCESSFUL_RETURN;
}
/*
* g e t V a r i a n c e C o v a r i a n c e
*/
#if QPOASES_USE_OLD_VERSION
returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR )
{
int run1, run2, run3; /* simple run variables (for loops). */
returnValue returnvalue; /* the return value */
BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
/* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:
* (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE
* VARIANCE-COVARIANCE EVALUATION)
* ----------------------------------------------- */
int nFR, nFX, nAC;
nFR = qp->getNFR( );
nFX = qp->getNFX( );
nAC = qp->getNAC( );
if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
/* SOME INITIALIZATIONS:
* --------------------- */
for( run1 = 0; run1 < KKT_DIM * KKT_DIM; run1++ )
{
K [run1] = 0.0;
Primal_Dual_VAR[run1] = 0.0;
}
/* ================================================================= */
/* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT
* K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )
* THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP
* WITH RESPECT TO THE CURRENT ACTIVE SET
* USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS
* cf. THE (protected) FUNCTION determineStepDirection. */
for( run3 = 0; run3 < KKT_DIM; run3++ )
{
for( run1 = 0; run1 < NVMAX; run1++ )
{
delta_g_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+run1];
delta_lb_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* LINE-WISE LOADING OF THE INPUT */
delta_ub_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* VARIANCE-COVARIANCE */
}
for( run1 = 0; run1 < NCMAX; run1++ )
{
delta_lbA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
delta_ubA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
}
/* EVALUATION OF THE STEP:
* ------------------------------------------------------------------------------ */
returnvalue = qp->hotstart_determineStepDirection(
FR_idx, FX_idx, AC_idx,
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
delta_yAC,delta_yFX );
/* ------------------------------------------------------------------------------ */
/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
* ------------------------------------------------------ */
if ( returnvalue != SUCCESSFUL_RETURN )
{
return returnvalue;
}
/* LINE WISE */
/* STORAGE OF THE QP-REACTION */
/* (uses the index list) */
for( run1=0; run1<nFR; run1++ )
{
run2 = FR_idx[run1];
K[run3*KKT_DIM+run2] = delta_xFR[run1];
}
for( run1=0; run1<nFX; run1++ )
{
run2 = FX_idx[run1];
K[run3*KKT_DIM+run2] = delta_xFX[run1];
K[run3*KKT_DIM+NVMAX+run2] = delta_yFX[run1];
}
for( run1=0; run1<nAC; run1++ )
{
run2 = AC_idx[run1];
K[run3*KKT_DIM+2*NVMAX+run2] = delta_yAC[run1];
}
}
/* ================================================================= */
/* SECOND MATRIX MULTIPLICATION (OBTAINS THE FINAL RESULT
* Primal_Dual_VAR := ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * K )
* THE APPLICATION OF THE KKT-INVERSE IS AGAIN REALIZED
* BY USING THE PROTECTED FUNCTION
* determineStepDirection */
for( run3 = 0; run3 < KKT_DIM; run3++ )
{
for( run1 = 0; run1 < NVMAX; run1++ )
{
delta_g_cov [run1] = K[run3+ run1*KKT_DIM];
delta_lb_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /* ROW WISE LOADING OF THE */
delta_ub_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /* INTERMEDIATE RESULT K */
}
for( run1 = 0; run1 < NCMAX; run1++ )
{
delta_lbA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
delta_ubA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
}
/* EVALUATION OF THE STEP:
* ------------------------------------------------------------------------------ */
returnvalue = qp->hotstart_determineStepDirection(
FR_idx, FX_idx, AC_idx,
delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
delta_yAC,delta_yFX );
/* ------------------------------------------------------------------------------ */
/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
* ------------------------------------------------------ */
if ( returnvalue != SUCCESSFUL_RETURN )
{
return returnvalue;
}
/* ROW-WISE STORAGE */
/* OF THE RESULT. */
for( run1=0; run1<nFR; run1++ )
{
run2 = FR_idx[run1];
Primal_Dual_VAR[run3+run2*KKT_DIM] = delta_xFR[run1];
}
for( run1=0; run1<nFX; run1++ )
{
run2 = FX_idx[run1];
Primal_Dual_VAR[run3+run2*KKT_DIM ] = delta_xFX[run1];
Primal_Dual_VAR[run3+(NVMAX+run2)*KKT_DIM] = delta_yFX[run1];
}
for( run1=0; run1<nAC; run1++ )
{
run2 = AC_idx[run1];
Primal_Dual_VAR[run3+(2*NVMAX+run2)*KKT_DIM] = delta_yAC[run1];
}
}
return SUCCESSFUL_RETURN;
}
#endif

342
phonelibs/qpoases/SRC/Indexlist.cpp 100644
View File

@ -0,0 +1,342 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/Indexlist.cpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of the Indexlist class designed to manage index lists of
* constraints and bounds within a QProblem_SubjectTo.
*/
#include <Indexlist.hpp>
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* I n d e x l i s t
*/
Indexlist::Indexlist( ) : length( 0 ),
first( -1 ),
last( -1 ),
lastusedindex( -1 ),
physicallength( INDEXLISTFACTOR*(NVMAX+NCMAX) )
{
int i;
for( i=0; i<physicallength; ++i )
{
number[i] = -1;
next[i] = -1;
previous[i] = -1;
}
}
/*
* I n d e x l i s t
*/
Indexlist::Indexlist( const Indexlist& rhs ) : length( rhs.length ),
first( rhs.first ),
last( rhs.last ),
lastusedindex( rhs.lastusedindex ),
physicallength( rhs.physicallength )
{
int i;
for( i=0; i<physicallength; ++i )
{
number[i] = rhs.number[i];
next[i] = rhs.next[i];
previous[i] = rhs.previous[i];
}
}
/*
* ~ I n d e x l i s t
*/
Indexlist::~Indexlist( )
{
}
/*
* o p e r a t o r =
*/
Indexlist& Indexlist::operator=( const Indexlist& rhs )
{
int i;
if ( this != &rhs )
{
length = rhs.length;
first = rhs.first;
last = rhs.last;
lastusedindex = rhs.lastusedindex;
physicallength = rhs.physicallength;
for( i=0; i<physicallength; ++i )
{
number[i] = rhs.number[i];
next[i] = rhs.next[i];
previous[i] = rhs.previous[i];
}
}
return *this;
}
/*
* i n i t
*/
returnValue Indexlist::init( )
{
int i;
length = 0;
first = -1;
last = -1;
lastusedindex = -1;
physicallength = INDEXLISTFACTOR*(NVMAX+NCMAX);
for( i=0; i<physicallength; ++i )
{
number[i] = -1;
next[i] = -1;
previous[i] = -1;
}
return SUCCESSFUL_RETURN;
}
/*
* g e t N u m b e r A r r a y
*/
returnValue Indexlist::getNumberArray( int* const numberarray ) const
{
int i;
int n = first;
/* Run trough indexlist and store numbers in numberarray. */
for( i=0; i<length; ++i )
{
if ( ( n >= 0 ) && ( number[n] >= 0 ) )
numberarray[i] = number[n];
else
return THROWERROR( RET_INDEXLIST_CORRUPTED );
n = next[n];
}
return SUCCESSFUL_RETURN;
}
/*
* g e t I n d e x
*/
int Indexlist::getIndex( int givennumber ) const
{
int i;
int n = first;
int index = -1; /* return -1 by default */
/* Run trough indexlist until number is found, if so return it index. */
for ( i=0; i<length; ++i )
{
if ( number[n] == givennumber )
{
index = i;
break;
}
n = next[n];
}
return index;
}
/*
* g e t P h y s i c a l I n d e x
*/
int Indexlist::getPhysicalIndex( int givennumber ) const
{
int i;
int n = first;
int index = -1; /* return -1 by default */
/* Run trough indexlist until number is found, if so return it physicalindex. */
for ( i=0; i<length; ++i )
{
if ( number[n] == givennumber )
{
index = n;
break;
}
n = next[n];
}
return index;
}
/*
* a d d N u m b e r
*/
returnValue Indexlist::addNumber( int addnumber )
{
int i;
if ( lastusedindex+1 < physicallength )
{
/* If there is enough storage, add number to indexlist. */
++lastusedindex;
number[lastusedindex] = addnumber;
next[lastusedindex] = 0;
if ( length == 0 )
{
first = lastusedindex;
previous[lastusedindex] = 0;
}
else
{
next[last] = lastusedindex;
previous[lastusedindex] = last;
}
last = lastusedindex;
++length;
return SUCCESSFUL_RETURN;
}
else
{
/* Rearrangement of index list necessary! */
if ( length == physicallength )
return THROWERROR( RET_INDEXLIST_EXCEEDS_MAX_LENGTH );
else
{
int numberArray[NVMAX+NCMAX];
getNumberArray( numberArray );
/* copy existing elements */
for ( i=0; i<length; ++i )
{
number[i] = numberArray[i];
next[i] = i+1;
previous[i] = i-1;
}
/* add new number at end of list */
number[length] = addnumber;
next[length] = -1;
previous[length] = length-1;
/* and set remaining entries to empty */
for ( i=length+1; i<physicallength; ++i )
{
number[i] = -1;
next[i] = -1;
previous[i] = -1;
}
first = 0;
last = length;
lastusedindex = length;
++length;
return THROWWARNING( RET_INDEXLIST_MUST_BE_REORDERD );
}
}
}
/*
* r e m o v e N u m b e r
*/
returnValue Indexlist::removeNumber( int removenumber )
{
int i = getPhysicalIndex( removenumber );
/* nothing to be done if number is not contained in index set */
if ( i < 0 )
return SUCCESSFUL_RETURN;
int p = previous[i];
int n = next[i];
if ( i == last )
last = p;
else
previous[n] = p;
if ( i == first )
first = n;
else
next[p] = n;
number[i] = -1;
next[i] = -1;
previous[i] = -1;
--length;
return SUCCESSFUL_RETURN;
}
/*
* s w a p N u m b e r s
*/
returnValue Indexlist::swapNumbers( int number1, int number2 )
{
int index1 = getPhysicalIndex( number1 );
int index2 = getPhysicalIndex( number2 );
/* consistency check */
if ( ( index1 < 0 ) || ( index2 < 0 ) )
return THROWERROR( RET_INDEXLIST_CORRUPTED );
int tmp = number[index1];
number[index1] = number[index2];
number[index2] = tmp;
return SUCCESSFUL_RETURN;
}
/*
* end of file
*/

85
phonelibs/qpoases/SRC/Indexlist.ipp 100644
View File

@ -0,0 +1,85 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/Indexlist.ipp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of inlined member functions of the Indexlist class designed
* to manage index lists of constraints and bounds within a QProblem_SubjectTo.
*/
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* g e t N u m b e r
*/
inline int Indexlist::getNumber( int physicalindex ) const
{
/* consistency check */
if ( ( physicalindex < 0 ) || ( physicalindex > length ) )
return -RET_INDEXLIST_OUTOFBOUNDS;
return number[physicalindex];
}
/*
* g e t L e n g t h
*/
inline int Indexlist::getLength( )
{
return length;
}
/*
* g e t L a s t N u m b e r
*/
inline int Indexlist::getLastNumber( ) const
{
return number[last];
}
/*
* g e t L a s t N u m b e r
*/
inline BooleanType Indexlist::isMember( int _number ) const
{
if ( getIndex( _number ) >= 0 )
return BT_TRUE;
else
return BT_FALSE;
}
/*
* end of file
*/

529
phonelibs/qpoases/SRC/MessageHandling.cpp 100644
View File

@ -0,0 +1,529 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/MessageHandling.cpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of the MessageHandling class including global return values.
*
*/
#include <MessageHandling.hpp>
#include <Utils.hpp>
/** Defines pairs of global return values and messages. */
MessageHandling::ReturnValueList returnValueList[] =
{
/* miscellaneous */
{ SUCCESSFUL_RETURN, "Successful return", VS_VISIBLE },
{ RET_DIV_BY_ZERO, "Division by zero", VS_VISIBLE },
{ RET_INDEX_OUT_OF_BOUNDS, "Index out of bounds", VS_VISIBLE },
{ RET_INVALID_ARGUMENTS, "At least one of the arguments is invalid", VS_VISIBLE },
{ RET_ERROR_UNDEFINED, "Error number undefined", VS_VISIBLE },
{ RET_WARNING_UNDEFINED, "Warning number undefined", VS_VISIBLE },
{ RET_INFO_UNDEFINED, "Info number undefined", VS_VISIBLE },
{ RET_EWI_UNDEFINED, "Error/warning/info number undefined", VS_VISIBLE },
{ RET_AVAILABLE_WITH_LINUX_ONLY, "This function is available under Linux only", VS_HIDDEN },
{ RET_UNKNOWN_BUG, "The error occured is not yet known", VS_VISIBLE },
{ RET_PRINTLEVEL_CHANGED, "Print level changed", VS_VISIBLE },
{ RET_NOT_YET_IMPLEMENTED, "Requested function is not yet implemented.", VS_VISIBLE },
/* Indexlist */
{ RET_INDEXLIST_MUST_BE_REORDERD, "Index list has to be reordered", VS_VISIBLE },
{ RET_INDEXLIST_EXCEEDS_MAX_LENGTH, "Index list exceeds its maximal physical length", VS_VISIBLE },
{ RET_INDEXLIST_CORRUPTED, "Index list corrupted", VS_VISIBLE },
{ RET_INDEXLIST_OUTOFBOUNDS, "Physical index is out of bounds", VS_VISIBLE },
{ RET_INDEXLIST_ADD_FAILED, "Adding indices from another index set failed", VS_VISIBLE },
{ RET_INDEXLIST_INTERSECT_FAILED, "Intersection with another index set failed", VS_VISIBLE },
/* SubjectTo / Bounds / Constraints */
{ RET_INDEX_ALREADY_OF_DESIRED_STATUS, "Index is already of desired status", VS_VISIBLE },
{ RET_SWAPINDEX_FAILED, "Cannot swap between different indexsets", VS_VISIBLE },
{ RET_ADDINDEX_FAILED, "Adding index to index set failed", VS_VISIBLE },
{ RET_NOTHING_TO_DO, "Nothing to do", VS_VISIBLE },
{ RET_SETUP_BOUND_FAILED, "Setting up bound index failed", VS_VISIBLE },
{ RET_SETUP_CONSTRAINT_FAILED, "Setting up constraint index failed", VS_VISIBLE },
{ RET_MOVING_BOUND_FAILED, "Moving bound between index sets failed", VS_VISIBLE },
{ RET_MOVING_CONSTRAINT_FAILED, "Moving constraint between index sets failed", VS_VISIBLE },
/* QProblem */
{ RET_QP_ALREADY_INITIALISED, "QProblem has already been initialised", VS_VISIBLE },
{ RET_NO_INIT_WITH_STANDARD_SOLVER, "Initialisation via extern QP solver is not yet implemented", VS_VISIBLE },
{ RET_RESET_FAILED, "Reset failed", VS_VISIBLE },
{ RET_INIT_FAILED, "Initialisation failed", VS_VISIBLE },
{ RET_INIT_FAILED_TQ, "Initialisation failed due to TQ factorisation", VS_VISIBLE },
{ RET_INIT_FAILED_CHOLESKY, "Initialisation failed due to Cholesky decomposition", VS_VISIBLE },
{ RET_INIT_FAILED_HOTSTART, "Initialisation failed! QP could not be solved!", VS_VISIBLE },
{ RET_INIT_FAILED_INFEASIBILITY, "Initial QP could not be solved due to infeasibility!", VS_VISIBLE },
{ RET_INIT_FAILED_UNBOUNDEDNESS, "Initial QP could not be solved due to unboundedness!", VS_VISIBLE },
{ RET_INIT_SUCCESSFUL, "Initialisation done", VS_VISIBLE },
{ RET_OBTAINING_WORKINGSET_FAILED, "Failed to obtain working set for auxiliary QP", VS_VISIBLE },
{ RET_SETUP_WORKINGSET_FAILED, "Failed to setup working set for auxiliary QP", VS_VISIBLE },
{ RET_SETUP_AUXILIARYQP_FAILED, "Failed to setup auxiliary QP for initialised homotopy", VS_VISIBLE },
{ RET_NO_EXTERN_SOLVER, "No extern QP solver available", VS_VISIBLE },
{ RET_QP_UNBOUNDED, "QP is unbounded", VS_VISIBLE },
{ RET_QP_INFEASIBLE, "QP is infeasible", VS_VISIBLE },
{ RET_QP_NOT_SOLVED, "Problems occured while solving QP with standard solver", VS_VISIBLE },
{ RET_QP_SOLVED, "QP successfully solved", VS_VISIBLE },
{ RET_UNABLE_TO_SOLVE_QP, "Problems occured while solving QP", VS_VISIBLE },
{ RET_INITIALISATION_STARTED, "Starting problem initialisation...", VS_VISIBLE },
{ RET_HOTSTART_FAILED, "Unable to perform homotopy due to internal error", VS_VISIBLE },
{ RET_HOTSTART_FAILED_TO_INIT, "Unable to initialise problem", VS_VISIBLE },
{ RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, "Unable to perform homotopy as previous QP is not solved", VS_VISIBLE },
{ RET_ITERATION_STARTED, "Iteration", VS_VISIBLE },
{ RET_SHIFT_DETERMINATION_FAILED, "Determination of shift of the QP data failed", VS_VISIBLE },
{ RET_STEPDIRECTION_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
{ RET_STEPLENGTH_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
{ RET_OPTIMAL_SOLUTION_FOUND, "Optimal solution of neighbouring QP found", VS_VISIBLE },
{ RET_HOMOTOPY_STEP_FAILED, "Unable to perform homotopy step", VS_VISIBLE },
{ RET_HOTSTART_STOPPED_INFEASIBILITY, "Premature homotopy termination because QP is infeasible", VS_VISIBLE },
{ RET_HOTSTART_STOPPED_UNBOUNDEDNESS, "Premature homotopy termination because QP is unbounded", VS_VISIBLE },
{ RET_WORKINGSET_UPDATE_FAILED, "Unable to update working sets according to initial guesses", VS_VISIBLE },
{ RET_MAX_NWSR_REACHED, "Maximum number of working set recalculations performed", VS_VISIBLE },
{ RET_CONSTRAINTS_NOT_SPECIFIED, "Problem does comprise constraints! You have to specify new constraints' bounds", VS_VISIBLE },
{ RET_INVALID_FACTORISATION_FLAG, "Invalid factorisation flag", VS_VISIBLE },
{ RET_UNABLE_TO_SAVE_QPDATA, "Unable to save QP data", VS_VISIBLE },
{ RET_STEPDIRECTION_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
{ RET_STEPDIRECTION_FAILED_CHOLESKY, "Abnormal termination due to Cholesky factorisation", VS_VISIBLE },
{ RET_CYCLING_DETECTED, "Cycling detected", VS_VISIBLE },
{ RET_CYCLING_NOT_RESOLVED, "Cycling cannot be resolved, QP is probably infeasible", VS_VISIBLE },
{ RET_CYCLING_RESOLVED, "Cycling probably resolved", VS_VISIBLE },
{ RET_STEPSIZE, "", VS_VISIBLE },
{ RET_STEPSIZE_NONPOSITIVE, "", VS_VISIBLE },
{ RET_SETUPSUBJECTTOTYPE_FAILED, "Setup of SubjectToTypes failed", VS_VISIBLE },
{ RET_ADDCONSTRAINT_FAILED, "Addition of constraint to working set failed", VS_VISIBLE },
{ RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, "Addition of constraint to working set failed", VS_VISIBLE },
{ RET_ADDBOUND_FAILED, "Addition of bound to working set failed", VS_VISIBLE },
{ RET_ADDBOUND_FAILED_INFEASIBILITY, "Addition of bound to working set failed", VS_VISIBLE },
{ RET_REMOVECONSTRAINT_FAILED, "Removal of constraint from working set failed", VS_VISIBLE },
{ RET_REMOVEBOUND_FAILED, "Removal of bound from working set failed", VS_VISIBLE },
{ RET_REMOVE_FROM_ACTIVESET, "Removing from active set:", VS_VISIBLE },
{ RET_ADD_TO_ACTIVESET, "Adding to active set:", VS_VISIBLE },
{ RET_REMOVE_FROM_ACTIVESET_FAILED, "Removing from active set failed", VS_VISIBLE },
{ RET_ADD_TO_ACTIVESET_FAILED, "Adding to active set failed", VS_VISIBLE },
{ RET_CONSTRAINT_ALREADY_ACTIVE, "Constraint is already active", VS_VISIBLE },
{ RET_ALL_CONSTRAINTS_ACTIVE, "All constraints are active, no further constraint can be added", VS_VISIBLE },
{ RET_LINEARLY_DEPENDENT, "New bound/constraint is linearly dependent", VS_VISIBLE },
{ RET_LINEARLY_INDEPENDENT, "New bound/constraint is linearly independent", VS_VISIBLE },
{ RET_LI_RESOLVED, "Linear independence of active contraint matrix successfully resolved", VS_VISIBLE },
{ RET_ENSURELI_FAILED, "Failed to ensure linear indepence of active contraint matrix", VS_VISIBLE },
{ RET_ENSURELI_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
{ RET_ENSURELI_FAILED_NOINDEX, "No index found, QP is probably infeasible", VS_VISIBLE },
{ RET_ENSURELI_FAILED_CYCLING, "Cycling detected, QP is probably infeasible", VS_VISIBLE },
{ RET_BOUND_ALREADY_ACTIVE, "Bound is already active", VS_VISIBLE },
{ RET_ALL_BOUNDS_ACTIVE, "All bounds are active, no further bound can be added", VS_VISIBLE },
{ RET_CONSTRAINT_NOT_ACTIVE, "Constraint is not active", VS_VISIBLE },
{ RET_BOUND_NOT_ACTIVE, "Bound is not active", VS_VISIBLE },
{ RET_HESSIAN_NOT_SPD, "Projected Hessian matrix not positive definite", VS_VISIBLE },
{ RET_MATRIX_SHIFT_FAILED, "Unable to update matrices or to transform vectors", VS_VISIBLE },
{ RET_MATRIX_FACTORISATION_FAILED, "Unable to calculate new matrix factorisations", VS_VISIBLE },
{ RET_PRINT_ITERATION_FAILED, "Unable to print information on current iteration", VS_VISIBLE },
{ RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, "No global message output file initialised", VS_VISIBLE },
/* Utils */
{ RET_UNABLE_TO_OPEN_FILE, "Unable to open file", VS_VISIBLE },
{ RET_UNABLE_TO_WRITE_FILE, "Unable to write into file", VS_VISIBLE },
{ RET_UNABLE_TO_READ_FILE, "Unable to read from file", VS_VISIBLE },
{ RET_FILEDATA_INCONSISTENT, "File contains inconsistent data", VS_VISIBLE },
/* SolutionAnalysis */
{ RET_NO_SOLUTION, "QP solution does not satisfy KKT optimality conditions", VS_VISIBLE },
{ RET_INACCURATE_SOLUTION, "KKT optimality conditions not satisfied to sufficient accuracy", VS_VISIBLE },
{ TERMINAL_LIST_ELEMENT, "", VS_HIDDEN } /* IMPORTANT: Terminal list element! */
};
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* M e s s a g e H a n d l i n g
*/
MessageHandling::MessageHandling( ) : errorVisibility( VS_VISIBLE ),
warningVisibility( VS_VISIBLE ),
infoVisibility( VS_VISIBLE ),
outputFile( myStdout ),
errorCount( 0 )
{
}
/*
* M e s s a g e H a n d l i n g
*/
MessageHandling::MessageHandling( myFILE* _outputFile ) :
errorVisibility( VS_VISIBLE ),
warningVisibility( VS_VISIBLE ),
infoVisibility( VS_VISIBLE ),
outputFile( _outputFile ),
errorCount( 0 )
{
}
/*
* M e s s a g e H a n d l i n g
*/
MessageHandling::MessageHandling( VisibilityStatus _errorVisibility,
VisibilityStatus _warningVisibility,
VisibilityStatus _infoVisibility
) :
errorVisibility( _errorVisibility ),
warningVisibility( _warningVisibility ),
infoVisibility( _infoVisibility ),
outputFile( myStderr ),
errorCount( 0 )
{
}
/*
* M e s s a g e H a n d l i n g
*/
MessageHandling::MessageHandling( myFILE* _outputFile,
VisibilityStatus _errorVisibility,
VisibilityStatus _warningVisibility,
VisibilityStatus _infoVisibility
) :
errorVisibility( _errorVisibility ),
warningVisibility( _warningVisibility ),
infoVisibility( _infoVisibility ),
outputFile( _outputFile ),
errorCount( 0 )
{
}
/*
* M e s s a g e H a n d l i n g
*/
MessageHandling::MessageHandling( const MessageHandling& rhs ) :
errorVisibility( rhs.errorVisibility ),
warningVisibility( rhs.warningVisibility ),
infoVisibility( rhs.infoVisibility ),
outputFile( rhs.outputFile ),
errorCount( rhs.errorCount )
{
}
/*
* ~ M e s s a g e H a n d l i n g
*/
MessageHandling::~MessageHandling( )
{
#ifdef PC_DEBUG
if ( outputFile != 0 )
fclose( outputFile );
#endif
}
/*
* o p e r a t o r =
*/
MessageHandling& MessageHandling::operator=( const MessageHandling& rhs )
{
if ( this != &rhs )
{
errorVisibility = rhs.errorVisibility;
warningVisibility = rhs.warningVisibility;
infoVisibility = rhs.infoVisibility;
outputFile = rhs.outputFile;
errorCount = rhs.errorCount;
}
return *this;
}
/*
* t h r o w E r r o r
*/
returnValue MessageHandling::throwError(
returnValue Enumber,
const char* additionaltext,
const char* functionname,
const char* filename,
const unsigned long linenumber,
VisibilityStatus localVisibilityStatus
)
{
/* consistency check */
if ( Enumber <= SUCCESSFUL_RETURN )
return throwError( RET_ERROR_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
/* Call to common throwMessage function if error shall be displayed. */
if ( errorVisibility == VS_VISIBLE )
return throwMessage( Enumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"ERROR" );
else
return Enumber;
}
/*
* t h r o w W a r n i n g
*/
returnValue MessageHandling::throwWarning(
returnValue Wnumber,
const char* additionaltext,
const char* functionname,
const char* filename,
const unsigned long linenumber,
VisibilityStatus localVisibilityStatus
)
{
/* consistency check */
if ( Wnumber <= SUCCESSFUL_RETURN )
return throwError( RET_WARNING_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
/* Call to common throwMessage function if warning shall be displayed. */
if ( warningVisibility == VS_VISIBLE )
return throwMessage( Wnumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"WARNING" );
else
return Wnumber;
}
/*
* t h r o w I n f o
*/
returnValue MessageHandling::throwInfo(
returnValue Inumber,
const char* additionaltext,
const char* functionname,
const char* filename,
const unsigned long linenumber,
VisibilityStatus localVisibilityStatus
)
{
/* consistency check */
if ( Inumber < SUCCESSFUL_RETURN )
return throwError( RET_INFO_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
/* Call to common throwMessage function if info shall be displayed. */
if ( infoVisibility == VS_VISIBLE )
return throwMessage( Inumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"INFO" );
else
return Inumber;
}
/*
* r e s e t
*/
returnValue MessageHandling::reset( )
{
setErrorVisibilityStatus( VS_VISIBLE );
setWarningVisibilityStatus( VS_VISIBLE );
setInfoVisibilityStatus( VS_VISIBLE );
setOutputFile( myStderr );
setErrorCount( 0 );
return SUCCESSFUL_RETURN;
}
/*
* l i s t A l l M e s s a g e s
*/
returnValue MessageHandling::listAllMessages( )
{
#ifdef PC_DEBUG
int keypos = 0;
char myPrintfString[160];
/* Run through whole returnValueList and print each item. */
while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
{
sprintf( myPrintfString," %d - %s \n",keypos,returnValueList[keypos].data );
myPrintf( myPrintfString );
++keypos;
}
#endif
return SUCCESSFUL_RETURN;
}
/*****************************************************************************
* P R O T E C T E D *
*****************************************************************************/
#ifdef PC_DEBUG /* Re-define throwMessage function for embedded code! */
/*
* t h r o w M e s s a g e
*/
returnValue MessageHandling::throwMessage(
returnValue RETnumber,
const char* additionaltext,
const char* functionname,
const char* filename,
const unsigned long linenumber,
VisibilityStatus localVisibilityStatus,
const char* RETstring
)
{
int i;
int keypos = 0;
char myPrintfString[160];
/* 1) Determine number of whitespace for output. */
char whitespaces[41];
int numberOfWhitespaces = (errorCount-1)*2;
if ( numberOfWhitespaces < 0 )
numberOfWhitespaces = 0;
if ( numberOfWhitespaces > 40 )
numberOfWhitespaces = 40;
for( i=0; i<numberOfWhitespaces; ++i )
whitespaces[i] = ' ';
whitespaces[numberOfWhitespaces] = '\0';
/* 2) Find error/warning/info in list. */
while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
{
if ( returnValueList[keypos].key == RETnumber )
break;
else
++keypos;
}
if ( returnValueList[keypos].key == TERMINAL_LIST_ELEMENT )
{
throwError( RET_EWI_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
return RETnumber;
}
/* 3) Print error/warning/info. */
if ( ( returnValueList[keypos].globalVisibilityStatus == VS_VISIBLE ) && ( localVisibilityStatus == VS_VISIBLE ) )
{
if ( errorCount > 0 )
{
sprintf( myPrintfString,"%s->", whitespaces );
myPrintf( myPrintfString );
}
if ( additionaltext == 0 )
{
sprintf( myPrintfString,"%s (%s, %s:%d): \t%s\n",
RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data
);
myPrintf( myPrintfString );
}
else
{
sprintf( myPrintfString,"%s (%s, %s:%d): \t%s %s\n",
RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data,additionaltext
);
myPrintf( myPrintfString );
}
/* take care of proper indention for subsequent error messages */
if ( RETstring[0] == 'E' )
{
++errorCount;
}
else
{
if ( errorCount > 0 )
myPrintf( "\n" );
errorCount = 0;
}
}
return RETnumber;
}
#else /* = PC_DEBUG not defined */
/*
* t h r o w M e s s a g e
*/
returnValue MessageHandling::throwMessage(
returnValue RETnumber,
const char* additionaltext,
const char* functionname,
const char* filename,
const unsigned long linenumber,
VisibilityStatus localVisibilityStatus,
const char* RETstring
)
{
/* DUMMY CODE FOR PRETENDING USE OF ARGUMENTS
* FOR SUPPRESSING COMPILER WARNINGS! */
int i = 0;
if ( additionaltext == 0 ) i++;
if ( functionname == 0 ) i++;
if ( filename == 0 ) i++;
if ( linenumber == 0 ) i++;
if ( localVisibilityStatus == VS_VISIBLE ) i++;
if ( RETstring == 0 ) i++;
/* END OF DUMMY CODE */
return RETnumber;
}
#endif /* PC_DEBUG */
/*****************************************************************************
* G L O B A L M E S S A G E H A N D L E R *
*****************************************************************************/
/** Global message handler for all qpOASES modules.*/
MessageHandling globalMessageHandler( myStderr,VS_VISIBLE,VS_VISIBLE,VS_VISIBLE );
/*
* g e t G l o b a l M e s s a g e H a n d l e r
*/
MessageHandling* getGlobalMessageHandler( )
{
return &globalMessageHandler;
}
const char* MessageHandling::getErrorString(int error)
{
return returnValueList[ error ].data;
}
/*
* end of file
*/

137
phonelibs/qpoases/SRC/MessageHandling.ipp 100644
View File

@ -0,0 +1,137 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/MessageHandling.ipp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of inlined member functions of the MessageHandling class.
*/
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* g e t E r r o r V i s i b i l i t y S t a t u s
*/
inline VisibilityStatus MessageHandling::getErrorVisibilityStatus( ) const
{
return errorVisibility;
}
/*
* g e t W a r n i n g V i s i b i l i t y S t a t u s
*/
inline VisibilityStatus MessageHandling::getWarningVisibilityStatus( ) const
{
return warningVisibility;
}
/*
* g e t I n f o V i s i b i l i t y S t a t u s
*/
inline VisibilityStatus MessageHandling::getInfoVisibilityStatus( ) const
{
return infoVisibility;
}
/*
* g e t O u t p u t F i l e
*/
inline myFILE* MessageHandling::getOutputFile( ) const
{
return outputFile;
}
/*
* g e t E r r o r C o u n t
*/
inline int MessageHandling::getErrorCount( ) const
{
return errorCount;
}
/*
* s e t E r r o r V i s i b i l i t y S t a t u s
*/
inline void MessageHandling::setErrorVisibilityStatus( VisibilityStatus _errorVisibility )
{
errorVisibility = _errorVisibility;
}
/*
* s e t W a r n i n g V i s i b i l i t y S t a t u s
*/
inline void MessageHandling::setWarningVisibilityStatus( VisibilityStatus _warningVisibility )
{
warningVisibility = _warningVisibility;
}
/*
* s e t I n f o V i s i b i l i t y S t a t u s
*/
inline void MessageHandling::setInfoVisibilityStatus( VisibilityStatus _infoVisibility )
{
infoVisibility = _infoVisibility;
}
/*
* s e t O u t p u t F i l e
*/
inline void MessageHandling::setOutputFile( myFILE* _outputFile )
{
outputFile = _outputFile;
}
/*
* s e t E r r o r C o u n t
*/
inline returnValue MessageHandling::setErrorCount( int _errorCount )
{
if ( _errorCount >= 0 )
{
errorCount = _errorCount;
return SUCCESSFUL_RETURN;
}
else
return RET_INVALID_ARGUMENTS;
}
/*
* end of file
*/

3867
phonelibs/qpoases/SRC/QProblem.cpp 100644
View File

File diff suppressed because it is too large Load Diff

299
phonelibs/qpoases/SRC/QProblem.ipp 100644
View File

@ -0,0 +1,299 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/QProblem.ipp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of inlined member functions of the QProblem class which
* is able to use the newly developed online active set strategy for
* parametric quadratic programming.
*/
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* g e t A
*/
inline returnValue QProblem::getA( real_t* const _A ) const
{
int i;
for ( i=0; i<getNV( )*getNC( ); ++i )
_A[i] = A[i];
return SUCCESSFUL_RETURN;
}
/*
* g e t A
*/
inline returnValue QProblem::getA( int number, real_t* const row ) const
{
int nV = getNV( );
if ( ( number >= 0 ) && ( number < getNC( ) ) )
{
for ( int i=0; i<nV; ++i )
row[i] = A[number*NVMAX + i];
return SUCCESSFUL_RETURN;
}
else
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
/*
* g e t L B A
*/
inline returnValue QProblem::getLBA( real_t* const _lbA ) const
{
int i;
for ( i=0; i<getNC( ); ++i )
_lbA[i] = lbA[i];
return SUCCESSFUL_RETURN;
}
/*
* g e t L B A
*/
inline returnValue QProblem::getLBA( int number, real_t& value ) const
{
if ( ( number >= 0 ) && ( number < getNC( ) ) )
{
value = lbA[number];
return SUCCESSFUL_RETURN;
}
else
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
/*
* g e t U B A
*/
inline returnValue QProblem::getUBA( real_t* const _ubA ) const
{
int i;
for ( i=0; i<getNC( ); ++i )
_ubA[i] = ubA[i];
return SUCCESSFUL_RETURN;
}
/*
* g e t U B A
*/
inline returnValue QProblem::getUBA( int number, real_t& value ) const
{
if ( ( number >= 0 ) && ( number < getNC( ) ) )
{
value = ubA[number];
return SUCCESSFUL_RETURN;
}
else
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
/*
* g e t C o n s t r a i n t s
*/
inline returnValue QProblem::getConstraints( Constraints* const _constraints ) const
{
*_constraints = constraints;
return SUCCESSFUL_RETURN;
}
/*
* g e t N C
*/
inline int QProblem::getNC( ) const
{
return constraints.getNC( );
}
/*
* g e t N E C
*/
inline int QProblem::getNEC( ) const
{
return constraints.getNEC( );
}
/*
* g e t N A C
*/
inline int QProblem::getNAC( )
{
return constraints.getNAC( );
}
/*
* g e t N I A C
*/
inline int QProblem::getNIAC( )
{
return constraints.getNIAC( );
}
/*****************************************************************************
* P R O T E C T E D *
*****************************************************************************/
/*
* s e t A
*/
inline returnValue QProblem::setA( const real_t* const A_new )
{
int i, j;
int nV = getNV( );
int nC = getNC( );
/* Set constraint matrix AND update member AX. */
for( j=0; j<nC; ++j )
{
Ax[j] = 0.0;
for( i=0; i<nV; ++i )
{
A[j*NVMAX + i] = A_new[j*nV + i];
Ax[j] += A[j*NVMAX + i] * x[i];
}
}
return SUCCESSFUL_RETURN;
}
/*
* s e t A
*/
inline returnValue QProblem::setA( int number, const real_t* const row )
{
int i;
int nV = getNV( );
/* Set constraint matrix AND update member AX. */
if ( ( number >= 0 ) && ( number < getNC( ) ) )
{
Ax[number] = 0.0;
for( i=0; i<nV; ++i )
{
A[number*NVMAX + i] = row[i];
Ax[number] += A[number*NVMAX + i] * x[i];
}
return SUCCESSFUL_RETURN;
}
else
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
/*
* s e t L B A
*/
inline returnValue QProblem::setLBA( const real_t* const lbA_new )
{
int i;
int nC = getNC();
for( i=0; i<nC; ++i )
lbA[i] = lbA_new[i];
return SUCCESSFUL_RETURN;
}
/*
* s e t L B A
*/
inline returnValue QProblem::setLBA( int number, real_t value )
{
if ( ( number >= 0 ) && ( number < getNC( ) ) )
{
lbA[number] = value;
return SUCCESSFUL_RETURN;
}
else
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
/*
* s e t U B A
*/
inline returnValue QProblem::setUBA( const real_t* const ubA_new )
{
int i;
int nC = getNC();
for( i=0; i<nC; ++i )
ubA[i] = ubA_new[i];
return SUCCESSFUL_RETURN;
}
/*
* s e t U B A
*/
inline returnValue QProblem::setUBA( int number, real_t value )
{
if ( ( number >= 0 ) && ( number < getNC( ) ) )
{
ubA[number] = value;
return SUCCESSFUL_RETURN;
}
else
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
/*
* end of file
*/

2151
phonelibs/qpoases/SRC/QProblemB.cpp 100644
View File

File diff suppressed because it is too large Load Diff

425
phonelibs/qpoases/SRC/QProblemB.ipp 100644
View File

@ -0,0 +1,425 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/QProblemB.ipp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of inlined member functions of the QProblemB class which
* is able to use the newly developed online active set strategy for
* parametric quadratic programming.
*/
#include <math.h>
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* g e t H
*/
inline returnValue QProblemB::getH( real_t* const _H ) const
{
int i;
for ( i=0; i<getNV( )*getNV( ); ++i )
_H[i] = H[i];
return SUCCESSFUL_RETURN;
}
/*
* g e t G
*/
inline returnValue QProblemB::getG( real_t* const _g ) const
{
int i;
for ( i=0; i<getNV( ); ++i )
_g[i] = g[i];
return SUCCESSFUL_RETURN;
}
/*
* g e t L B
*/
inline returnValue QProblemB::getLB( real_t* const _lb ) const
{
int i;
for ( i=0; i<getNV( ); ++i )
_lb[i] = lb[i];
return SUCCESSFUL_RETURN;
}
/*
* g e t L B
*/
inline returnValue QProblemB::getLB( int number, real_t& value ) const
{
if ( ( number >= 0 ) && ( number < getNV( ) ) )
{
value = lb[number];
return SUCCESSFUL_RETURN;
}
else
{
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
}
/*
* g e t U B
*/
inline returnValue QProblemB::getUB( real_t* const _ub ) const
{
int i;
for ( i=0; i<getNV( ); ++i )
_ub[i] = ub[i];
return SUCCESSFUL_RETURN;
}
/*
* g e t U B
*/
inline returnValue QProblemB::getUB( int number, real_t& value ) const
{
if ( ( number >= 0 ) && ( number < getNV( ) ) )
{
value = ub[number];
return SUCCESSFUL_RETURN;
}
else
{
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
}
/*
* g e t B o u n d s
*/
inline returnValue QProblemB::getBounds( Bounds* const _bounds ) const
{
*_bounds = bounds;
return SUCCESSFUL_RETURN;
}
/*
* g e t N V
*/
inline int QProblemB::getNV( ) const
{
return bounds.getNV( );
}
/*
* g e t N F R
*/
inline int QProblemB::getNFR( )
{
return bounds.getNFR( );
}
/*
* g e t N F X
*/
inline int QProblemB::getNFX( )
{
return bounds.getNFX( );
}
/*
* g e t N F V
*/
inline int QProblemB::getNFV( ) const
{
return bounds.getNFV( );
}
/*
* g e t S t a t u s
*/
inline QProblemStatus QProblemB::getStatus( ) const
{
return status;
}
/*
* i s I n i t i a l i s e d
*/
inline BooleanType QProblemB::isInitialised( ) const
{
if ( status == QPS_NOTINITIALISED )
return BT_FALSE;
else
return BT_TRUE;
}
/*
* i s S o l v e d
*/
inline BooleanType QProblemB::isSolved( ) const
{
if ( status == QPS_SOLVED )
return BT_TRUE;
else
return BT_FALSE;
}
/*
* i s I n f e a s i b l e
*/
inline BooleanType QProblemB::isInfeasible( ) const
{
return infeasible;
}
/*
* i s U n b o u n d e d
*/
inline BooleanType QProblemB::isUnbounded( ) const
{
return unbounded;
}
/*
* g e t P r i n t L e v e l
*/
inline PrintLevel QProblemB::getPrintLevel( ) const
{
return printlevel;
}
/*
* g e t H e s s i a n T y p e
*/
inline HessianType QProblemB::getHessianType( ) const
{
return hessianType;
}
/*
* s e t H e s s i a n T y p e
*/
inline returnValue QProblemB::setHessianType( HessianType _hessianType )
{
hessianType = _hessianType;
return SUCCESSFUL_RETURN;
}
/*****************************************************************************
* P R O T E C T E D *
*****************************************************************************/
/*
* s e t H
*/
inline returnValue QProblemB::setH( const real_t* const H_new )
{
int i, j;
int nV = getNV();
for( i=0; i<nV; ++i )
for( j=0; j<nV; ++j )
H[i*NVMAX + j] = H_new[i*nV + j];
return SUCCESSFUL_RETURN;
}
/*
* s e t G
*/
inline returnValue QProblemB::setG( const real_t* const g_new )
{
int i;
int nV = getNV();
for( i=0; i<nV; ++i )
g[i] = g_new[i];
return SUCCESSFUL_RETURN;
}
/*
* s e t L B
*/
inline returnValue QProblemB::setLB( const real_t* const lb_new )
{
int i;
int nV = getNV();
for( i=0; i<nV; ++i )
lb[i] = lb_new[i];
return SUCCESSFUL_RETURN;
}
/*
* s e t L B
*/
inline returnValue QProblemB::setLB( int number, real_t value )
{
if ( ( number >= 0 ) && ( number < getNV( ) ) )
{
lb[number] = value;
return SUCCESSFUL_RETURN;
}
else
{
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
}
/*
* s e t U B
*/
inline returnValue QProblemB::setUB( const real_t* const ub_new )
{
int i;
int nV = getNV();
for( i=0; i<nV; ++i )
ub[i] = ub_new[i];
return SUCCESSFUL_RETURN;
}
/*
* s e t U B
*/
inline returnValue QProblemB::setUB( int number, real_t value )
{
if ( ( number >= 0 ) && ( number < getNV( ) ) )
{
ub[number] = value;
return SUCCESSFUL_RETURN;
}
else
{
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
}
/*
* c o m p u t e G i v e n s
*/
inline void QProblemB::computeGivens( real_t xold, real_t yold, real_t& xnew, real_t& ynew,
real_t& c, real_t& s
) const
{
if ( getAbs( yold ) <= ZERO )
{
c = 1.0;
s = 0.0;
xnew = xold;
ynew = yold;
}
else
{
real_t t, mu;
mu = getAbs( xold );
if ( getAbs( yold ) > mu )
mu = getAbs( yold );
t = mu * sqrt( (xold/mu)*(xold/mu) + (yold/mu)*(yold/mu) );
if ( xold < 0.0 )
t = -t;
c = xold/t;
s = yold/t;
xnew = t;
ynew = 0.0;
}
return;
}
/*
* a p p l y G i v e n s
*/
inline void QProblemB::applyGivens( real_t c, real_t s, real_t xold, real_t yold,
real_t& xnew, real_t& ynew
) const
{
/* Usual Givens plane rotation requiring four multiplications. */
xnew = c*xold + s*yold;
ynew = -s*xold + c*yold;
// double nu = s/(1.0+c);
//
// xnew = xold*c + yold*s;
// ynew = (xnew+xold)*nu - yold;
return;
}
/*
* end of file
*/

200
phonelibs/qpoases/SRC/SubjectTo.cpp 100644
View File

@ -0,0 +1,200 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/SubjectTo.cpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of the SubjectTo class designed to manage working sets of
* constraints and bounds within a QProblem.
*/
#include <SubjectTo.hpp>
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* S u b j e c t T o
*/
SubjectTo::SubjectTo( ) : noLower( BT_TRUE ),
noUpper( BT_TRUE ),
size( 0 )
{
int i;
for( i=0; i<size; ++i )
{
type[i] = ST_UNKNOWN;
status[i] = ST_UNDEFINED;
}
}
/*
* S u b j e c t T o
*/
SubjectTo::SubjectTo( const SubjectTo& rhs ) : noLower( rhs.noLower ),
noUpper( rhs.noUpper ),
size( rhs.size )
{
int i;
for( i=0; i<size; ++i )
{
type[i] = rhs.type[i];
status[i] = rhs.status[i];
}
}
/*
* ~ S u b j e c t T o
*/
SubjectTo::~SubjectTo( )
{
}
/*
* o p e r a t o r =
*/
SubjectTo& SubjectTo::operator=( const SubjectTo& rhs )
{
int i;
if ( this != &rhs )
{
size = rhs.size;
for( i=0; i<size; ++i )
{
type[i] = rhs.type[i];
status[i] = rhs.status[i];
}
noLower = rhs.noLower;
noUpper = rhs.noUpper;
}
return *this;
}
/*
* i n i t
*/
returnValue SubjectTo::init( int n )
{
int i;
size = n;
noLower = BT_TRUE;
noUpper = BT_TRUE;
for( i=0; i<size; ++i )
{
type[i] = ST_UNKNOWN;
status[i] = ST_UNDEFINED;
}
return SUCCESSFUL_RETURN;
}
/*****************************************************************************
* P R O T E C T E D *
*****************************************************************************/
/*
* a d d I n d e x
*/
returnValue SubjectTo::addIndex( Indexlist* const indexlist,
int newnumber, SubjectToStatus newstatus
)
{
/* consistency check */
if ( status[newnumber] == newstatus )
return THROWERROR( RET_INDEX_ALREADY_OF_DESIRED_STATUS );
status[newnumber] = newstatus;
if ( indexlist->addNumber( newnumber ) == RET_INDEXLIST_EXCEEDS_MAX_LENGTH )
return THROWERROR( RET_ADDINDEX_FAILED );
return SUCCESSFUL_RETURN;
}
/*
* r e m o v e I n d e x
*/
returnValue SubjectTo::removeIndex( Indexlist* const indexlist,
int removenumber
)
{
status[removenumber] = ST_UNDEFINED;
if ( indexlist->removeNumber( removenumber ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_UNKNOWN_BUG );
return SUCCESSFUL_RETURN;
}
/*
* s w a p I n d e x
*/
returnValue SubjectTo::swapIndex( Indexlist* const indexlist,
int number1, int number2
)
{
/* consistency checks */
if ( status[number1] != status[number2] )
return THROWERROR( RET_SWAPINDEX_FAILED );
if ( number1 == number2 )
{
THROWWARNING( RET_NOTHING_TO_DO );
return SUCCESSFUL_RETURN;
}
if ( indexlist->swapNumbers( number1,number2 ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SWAPINDEX_FAILED );
return SUCCESSFUL_RETURN;
}
/*
* end of file
*/

132
phonelibs/qpoases/SRC/SubjectTo.ipp 100644
View File

@ -0,0 +1,132 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/SubjectTo.ipp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of the inlined member functions of the SubjectTo class
* designed to manage working sets of constraints and bounds within a QProblem.
*/
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* g e t T y p e
*/
inline SubjectToType SubjectTo::getType( int i ) const
{
if ( ( i >= 0 ) && ( i < size ) )
return type[i];
else
return ST_UNKNOWN;
}
/*
* g e t S t a t u s
*/
inline SubjectToStatus SubjectTo::getStatus( int i ) const
{
if ( ( i >= 0 ) && ( i < size ) )
return status[i];
else
return ST_UNDEFINED;
}
/*
* s e t T y p e
*/
inline returnValue SubjectTo::setType( int i, SubjectToType value )
{
if ( ( i >= 0 ) && ( i < size ) )
{
type[i] = value;
return SUCCESSFUL_RETURN;
}
else
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
/*
* s e t S t a t u s
*/
inline returnValue SubjectTo::setStatus( int i, SubjectToStatus value )
{
if ( ( i >= 0 ) && ( i < size ) )
{
status[i] = value;
return SUCCESSFUL_RETURN;
}
else
return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
}
/*
* s e t N o L o w e r
*/
inline void SubjectTo::setNoLower( BooleanType _status )
{
noLower = _status;
}
/*
* s e t N o U p p e r
*/
inline void SubjectTo::setNoUpper( BooleanType _status )
{
noUpper = _status;
}
/*
* i s N o L o w e r
*/
inline BooleanType SubjectTo::isNoLower( ) const
{
return noLower;
}
/*
* i s N o L o w e r
*/
inline BooleanType SubjectTo::isNoUpper( ) const
{
return noUpper;
}
/*
* end of file
*/

471
phonelibs/qpoases/SRC/Utils.cpp 100644
View File

@ -0,0 +1,471 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/Utils.cpp
* \author Hans Joachim Ferreau, Eckhard Arnold
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of some inlined utilities for working with the different QProblem
* classes.
*/
#include <math.h>
#if defined(__WIN32__) || defined(WIN32)
#include <windows.h>
#elif defined(LINUX)
#include <sys/stat.h>
#include <sys/time.h>
#endif
#ifdef __MATLAB__
#include <mex.h>
#endif
#include <Utils.hpp>
#ifdef PC_DEBUG /* Define print functions only for debugging! */
/*
* p r i n t
*/
returnValue print( const real_t* const v, int n )
{
int i;
char myPrintfString[160];
/* Print a vector. */
myPrintf( "[\t" );
for( i=0; i<n; ++i )
{
sprintf( myPrintfString," %.16e\t", v[i] );
myPrintf( myPrintfString );
}
myPrintf( "]\n" );
return SUCCESSFUL_RETURN;
}
/*
* p r i n t
*/
returnValue print( const real_t* const v, int n,
const int* const V_idx
)
{
int i;
char myPrintfString[160];
/* Print a permuted vector. */
myPrintf( "[\t" );
for( i=0; i<n; ++i )
{
sprintf( myPrintfString," %.16e\t", v[ V_idx[i] ] );
myPrintf( myPrintfString );
}
myPrintf( "]\n" );
return SUCCESSFUL_RETURN;
}
/*
* p r i n t
*/
returnValue print( const real_t* const v, int n,
const char* name
)
{
char myPrintfString[160];
/* Print vector name ... */
sprintf( myPrintfString,"%s = ", name );
myPrintf( myPrintfString );
/* ... and the vector itself. */
return print( v, n );
}
/*
* p r i n t
*/
returnValue print( const real_t* const M, int nrow, int ncol )
{
int i;
/* Print a matrix as a collection of row vectors. */
for( i=0; i<nrow; ++i )
print( &(M[i*ncol]), ncol );
myPrintf( "\n" );
return SUCCESSFUL_RETURN;
}
/*
* p r i n t
*/
returnValue print( const real_t* const M, int nrow, int ncol,
const int* const ROW_idx, const int* const COL_idx
)
{
int i;
/* Print a permuted matrix as a collection of permuted row vectors. */
for( i=0; i<nrow; ++i )
print( &( M[ ROW_idx[i]*ncol ] ), ncol, COL_idx );
myPrintf( "\n" );
return SUCCESSFUL_RETURN;
}
/*
* p r i n t
*/
returnValue print( const real_t* const M, int nrow, int ncol,
const char* name
)
{
char myPrintfString[160];
/* Print matrix name ... */
sprintf( myPrintfString,"%s = ", name );
myPrintf( myPrintfString );
/* ... and the matrix itself. */
return print( M, nrow, ncol );
}
/*
* p r i n t
*/
returnValue print( const int* const index, int n )
{
int i;
char myPrintfString[160];
/* Print a indexlist. */
myPrintf( "[\t" );
for( i=0; i<n; ++i )
{
sprintf( myPrintfString," %d\t", index[i] );
myPrintf( myPrintfString );
}
myPrintf( "]\n" );
return SUCCESSFUL_RETURN;
}
/*
* p r i n t
*/
returnValue print( const int* const index, int n,
const char* name
)
{
char myPrintfString[160];
/* Print indexlist name ... */
sprintf( myPrintfString,"%s = ", name );
myPrintf( myPrintfString );
/* ... and the indexlist itself. */
return print( index, n );
}
/*
* m y P r i n t f
*/
returnValue myPrintf( const char* s )
{
#ifdef __MATLAB__
mexPrintf( s );
#else
myFILE* outputfile = getGlobalMessageHandler( )->getOutputFile( );
if ( outputfile == 0 )
return THROWERROR( RET_NO_GLOBAL_MESSAGE_OUTPUTFILE );
fprintf( outputfile, "%s", s );
#endif
return SUCCESSFUL_RETURN;
}
/*
* p r i n t C o p y r i g h t N o t i c e
*/
returnValue printCopyrightNotice( )
{
return myPrintf( "\nqpOASES -- An Implementation of the Online Active Set Strategy.\nCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\n\nqpOASES is distributed under the terms of the \nGNU Lesser General Public License 2.1 in the hope that it will be \nuseful, but WITHOUT ANY WARRANTY; without even the implied warranty \nof MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. \nSee the GNU Lesser General Public License for more details.\n\n" );
}
/*
* r e a d F r o m F i l e
*/
returnValue readFromFile( real_t* data, int nrow, int ncol,
const char* datafilename
)
{
int i, j;
float float_data;
myFILE* datafile;
/* 1) Open file. */
if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
{
char errstr[80];
sprintf( errstr,"(%s)",datafilename );
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
}
/* 2) Read data from file. */
for( i=0; i<nrow; ++i )
{
for( j=0; j<ncol; ++j )
{
if ( fscanf( datafile, "%f ", &float_data ) == 0 )
{
fclose( datafile );
char errstr[80];
sprintf( errstr,"(%s)",datafilename );
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
}
data[i*ncol + j] = ( (real_t) float_data );
}
}
/* 3) Close file. */
fclose( datafile );
return SUCCESSFUL_RETURN;
}
/*
* r e a d F r o m F i l e
*/
returnValue readFromFile( real_t* data, int n,
const char* datafilename
)
{
return readFromFile( data, n, 1, datafilename );
}
/*
* r e a d F r o m F i l e
*/
returnValue readFromFile( int* data, int n,
const char* datafilename
)
{
int i;
myFILE* datafile;
/* 1) Open file. */
if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
{
char errstr[80];
sprintf( errstr,"(%s)",datafilename );
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
}
/* 2) Read data from file. */
for( i=0; i<n; ++i )
{
if ( fscanf( datafile, "%d\n", &(data[i]) ) == 0 )
{
fclose( datafile );
char errstr[80];
sprintf( errstr,"(%s)",datafilename );
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
}
}
/* 3) Close file. */
fclose( datafile );
return SUCCESSFUL_RETURN;
}
/*
* w r i t e I n t o F i l e
*/
returnValue writeIntoFile( const real_t* const data, int nrow, int ncol,
const char* datafilename, BooleanType append
)
{
int i, j;
myFILE* datafile;
/* 1) Open file. */
if ( append == BT_TRUE )
{
/* append data */
if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
{
char errstr[80];
sprintf( errstr,"(%s)",datafilename );
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
}
}
else
{
/* do not append data */
if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
{
char errstr[80];
sprintf( errstr,"(%s)",datafilename );
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
}
}
/* 2) Write data into file. */
for( i=0; i<nrow; ++i )
{
for( j=0; j<ncol; ++j )
fprintf( datafile, "%.16e ", data[i*ncol+j] );
fprintf( datafile, "\n" );
}
/* 3) Close file. */
fclose( datafile );
return SUCCESSFUL_RETURN;
}
/*
* w r i t e I n t o F i l e
*/
returnValue writeIntoFile( const real_t* const data, int n,
const char* datafilename, BooleanType append
)
{
return writeIntoFile( data,1,n,datafilename,append );
}
/*
* w r i t e I n t o F i l e
*/
returnValue writeIntoFile( const int* const data, int n,
const char* datafilename, BooleanType append
)
{
int i;
myFILE* datafile;
/* 1) Open file. */
if ( append == BT_TRUE )
{
/* append data */
if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
{
char errstr[80];
sprintf( errstr,"(%s)",datafilename );
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
}
}
else
{
/* do not append data */
if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
{
char errstr[80];
sprintf( errstr,"(%s)",datafilename );
return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
}
}
/* 2) Write data into file. */
for( i=0; i<n; ++i )
fprintf( datafile, "%d\n", data[i] );
/* 3) Close file. */
fclose( datafile );
return SUCCESSFUL_RETURN;
}
#endif /* PC_DEBUG */
/*
* g e t C P U t i m e
*/
real_t getCPUtime( )
{
real_t current_time = -1.0;
#if defined(__WIN32__) || defined(WIN32)
LARGE_INTEGER counter, frequency;
QueryPerformanceFrequency(&frequency);
QueryPerformanceCounter(&counter);
current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);
#elif defined(LINUX)
struct timeval theclock;
gettimeofday( &theclock,0 );
current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;
#endif
return current_time;
}
/*
* g e t N o r m
*/
real_t getNorm( const real_t* const v, int n )
{
int i;
real_t norm = 0.0;
for( i=0; i<n; ++i )
norm += v[i]*v[i];
return sqrt( norm );
}
/*
* end of file
*/

51
phonelibs/qpoases/SRC/Utils.ipp 100644
View File

@ -0,0 +1,51 @@
/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/Utils.ipp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of some inlined utilities for working with the different QProblem
* classes.
*/
/*
* g e t A b s
*/
inline real_t getAbs( real_t x )
{
if ( x < 0.0 )
return -x;
else
return x;
}
/*
* end of file
*/

87
phonelibs/qpoases/VERSIONS.txt 100644
View File

@ -0,0 +1,87 @@
##
## qpOASES -- An Implementation of the Online Active Set Strategy.
## Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
##
## qpOASES is free software; you can redistribute it and/or
## modify it under the terms of the GNU Lesser General Public
## License as published by the Free Software Foundation; either
## version 2.1 of the License, or (at your option) any later version.
##
## qpOASES is distributed in the hope that it will be useful,
## but WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
## Lesser General Public License for more details.
##
## You should have received a copy of the GNU Lesser General Public
## License along with qpOASES; if not, write to the Free Software
## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
##
VERSION HISTORY
===============
1.3embedded (last updated on 30th April 2009):
-----------------------------------------------------------------------
+ Re-programming of internal memory management to avoid dynamic memory allocations
+ Most #ifdef directives removed
+ Almost all type definitions gathered within INCLUDE/Types.hpp
+ Irrelevant functionality removed (like the SQProblem class, functionality
for loading data from files or the SCILAB interface)
+ Replacement of all doubles by real_t
+ Introduction of define "PC_DEBUG" for switching off all print functions
+ stdio.h was made optional, string.h is no longer needed
+ relative paths removed from #include directives
+ made auxiliary objects locally static within solveInitialQP()
+ Matlab interface fixed for single precision
+ New return value -2 from Legacy wrapper added to Matlab/Simulink interfaces
+ KKT optimality check moved into QProblem(B) class, SolutionAnalysis class removed
1.3 (released on 2nd June 2008, last updated on 19th June 2008):
-----------------------------------------------------------------------
+ Implementation of "initialised homotopy" concept
+ Addition of the SolutionAnalysis class
+ Utility functions for solving test problems in OQP format added
+ Flexibility of Matlab(R) interface enhanced
+ Major source code cleanup
(Attention: a few class names and calling interfaces have changed!)
1.2 (released on 9th October 2007):
-----------------------------------------------------------------------
+ Special treatment of diagonal Hessians
+ Improved infeasibility detection
+ Further improved Matlab(R) interface
+ Extended Simulink(R) interface
+ scilab interface added
+ Code cleanup and several bugfixes
1.1 (released on 8th July 2007):
--------------------------------
+ Implementation of the QProblemB class
+ Basic implementation of the SQProblem class
+ Improved Matlab(R) interface
+ Enabling/Disabling of constraints introduced
+ Several bugfixes
1.0 (released on 17th April 2007):
----------------------------------
Initial release.
##
## end of file
##

2
pyextra

@ -1 +1 @@
Subproject commit e0738376db27d208603d7e63dd465e003ca06325
Subproject commit 4eda4dd765c2bc719da9064774de6b2c14c322d1

5
run_docker_tests.sh
View File

@ -2,6 +2,7 @@
set -e
docker build -t tmppilot -f Dockerfile.openpilot .
docker run --rm \
-v "$(pwd)"/selfdrive/test/plant/out:/tmp/openpilot/selfdrive/test/plant/out \
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/plant && ./runtest.sh'
-v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out \
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'

9
selfdrive/boardd/Makefile
View File

@ -4,6 +4,7 @@ CXX = clang++
ARCH := $(shell uname -m)
OS := $(shell uname -o)
BASEDIR = ../..
PHONELIBS = ../../phonelibs
WARN_FLAGS = -Werror=implicit-function-declaration \
@ -12,8 +13,8 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=return-type \
-Werror=format-extra-args
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
CFLAGS = -std=gnu11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
@ -37,6 +38,8 @@ ZMQ_LIBS = -L$(HOME)/one/external/zmq/lib/ \
CEREAL_LIBS = -L$(HOME)/one/external/capnp/lib/ \
-l:libcapnp.a -l:libcapnp_c.a -l:libkj.a
EXTRA_LIBS = -lusb-1.0 -lpthread
CXXFLAGS += -I/usr/include/libusb-1.0
CFLAGS += -I/usr/include/libusb-1.0
endif
.PHONY: all
@ -46,6 +49,8 @@ include ../common/cereal.mk
OBJS = boardd.o \
../common/swaglog.o \
../common/params.o \
../common/util.o \
$(PHONELIBS)/json/src/json.o \
$(CEREAL_OBJS)

132
selfdrive/boardd/boardd.cc
View File

@ -19,7 +19,9 @@
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "cereal/gen/cpp/car.capnp.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
@ -38,20 +40,55 @@ bool loopback_can = false;
#define TIMEOUT 0
#define SAFETY_NOOUTPUT 0x0000
#define SAFETY_HONDA 0x0001
#define SAFETY_ALLOUTPUT 0x1337
pthread_t safety_setter_thread_handle = -1;
void *safety_setter_thread(void *s) {
char *value;
size_t value_sz = 0;
LOGW("waiting for params to set safety model");
while (1) {
const int result = read_db_value(NULL, "CarParams", &value, &value_sz);
if (value_sz > 0) break;
usleep(100*1000);
}
LOGW("got %d bytes CarParams", value_sz);
// format for board, make copy due to alignment issues, will be freed on out of scope
auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), value, value_sz);
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
int safety_model = car_params.getSafetyModel();
LOGW("setting safety model: %d", safety_model);
pthread_mutex_lock(&usb_lock);
// set in the mutex to avoid race
safety_setter_thread_handle = -1;
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
return NULL;
}
// must be called before threads or with mutex
bool usb_connect() {
int err;
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
if (dev_handle == NULL) { return false; }
if (dev_handle == NULL) { goto fail; }
err = libusb_set_configuration(dev_handle, 1);
if (err != 0) { return false; }
if (err != 0) { goto fail; }
err = libusb_claim_interface(dev_handle, 0);
if (err != 0) { return false; }
if (err != 0) { goto fail; }
if (loopback_can) {
libusb_control_transfer(dev_handle, 0xc0, 0xe5, 1, 0, NULL, 0, TIMEOUT);
@ -60,30 +97,23 @@ bool usb_connect() {
// power off ESP
libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT);
// forward CAN1 to CAN3...soon
//libusb_control_transfer(dev_handle, 0xc0, 0xdd, 1, 2, NULL, 0, TIMEOUT);
// power on charging (may trigger a reconnection, should be okay)
#ifndef __x86_64__
libusb_control_transfer(dev_handle, 0xc0, 0xe6, 1, 0, NULL, 0, TIMEOUT);
#else
LOGW("not enabling charging on x86_64");
#endif
// set UART modes for Honda Accord
/*for (int uart = 2; uart <= 3; uart++) {
// 9600 baud
libusb_control_transfer(dev_handle, 0x40, 0xe1, uart, 9600, NULL, 0, TIMEOUT);
// even parity
libusb_control_transfer(dev_handle, 0x40, 0xe2, uart, 1, NULL, 0, TIMEOUT);
// callback 1
libusb_control_transfer(dev_handle, 0x40, 0xe3, uart, 1, NULL, 0, TIMEOUT);
// no output is the default
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
if (safety_setter_thread_handle == -1) {
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
}
// TODO: Boardd should be able to set the baud rate
int baud = 500000;
libusb_control_transfer(dev_handle, 0x40, 0xde, 0, 0,
(unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN1
libusb_control_transfer(dev_handle, 0x40, 0xde, 1, 0,
(unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN2*/
// TODO: Boardd should be able to be told which safety model to use
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_HONDA, 0, NULL, 0, TIMEOUT);
return true;
fail:
return false;
}
void usb_retry_connect() {
@ -93,7 +123,7 @@ void usb_retry_connect() {
}
void handle_usb_issue(int err, const char func[]) {
LOGE("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func);
LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func);
if (err == -4) {
LOGE("lost connection");
usb_retry_connect();
@ -113,7 +143,7 @@ void can_recv(void *s) {
do {
err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT);
if (err != 0) { handle_usb_issue(err, __func__); }
if (err == -8) { LOGE("overflow got 0x%x", recv); };
if (err == -8) { LOGE_100("overflow got 0x%x", recv); };
// timeout is okay to exit, recv still happened
if (err == -7) { break; }
@ -261,6 +291,46 @@ void can_send(void *s) {
// **** threads ****
void *thermal_thread(void *crap) {
int err;
LOGD("start thermal thread");
// thermal = 8005
void *context = zmq_ctx_new();
void *subscriber = zmq_socket(context, ZMQ_SUB);
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
zmq_connect(subscriber, "tcp://127.0.0.1:8005");
// run as fast as messages come in
while (!do_exit) {
// recv from thermal
zmq_msg_t msg;
zmq_msg_init(&msg);
err = zmq_msg_recv(&msg, subscriber, 0);
assert(err >= 0);
// format for board, make copy due to alignment issues, will be freed on out of scope
// copied from send thread...
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
uint16_t target_fan_speed = event.getThermal().getFanSpeed();
//LOGW("setting fan speed %d", target_fan_speed);
pthread_mutex_lock(&usb_lock);
libusb_control_transfer(dev_handle, 0xc0, 0xd3, target_fan_speed, 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
}
// turn the fan off when we exit
libusb_control_transfer(dev_handle, 0xc0, 0xd3, 0, 0, NULL, 0, TIMEOUT);
return NULL;
}
void *can_send_thread(void *crap) {
LOGD("start send thread");
@ -364,8 +434,16 @@ int main() {
can_recv_thread, NULL);
assert(err == 0);
pthread_t thermal_thread_handle;
err = pthread_create(&thermal_thread_handle, NULL,
thermal_thread, NULL);
assert(err == 0);
// join threads
err = pthread_join(thermal_thread_handle, NULL);
assert(err == 0);
err = pthread_join(can_recv_thread_handle, NULL);
assert(err == 0);

5
selfdrive/boardd/boardd.py
View File

@ -20,6 +20,8 @@ except Exception:
SAFETY_NOOUTPUT = 0
SAFETY_HONDA = 1
SAFETY_TOYOTA = 2
SAFETY_TOYOTA_NOLIMITS = 0x1336
SAFETY_ALLOUTPUT = 0x1337
# *** serialization functions ***
@ -33,7 +35,7 @@ def can_list_to_can_capnp(can_msgs, msgtype='can'):
cc = dat.can[i]
cc.address = can_msg[0]
cc.busTime = can_msg[1]
cc.dat = can_msg[2]
cc.dat = str(can_msg[2])
cc.src = can_msg[3]
return dat
@ -111,6 +113,7 @@ def can_init():
def boardd_mock_loop():
context = zmq.Context()
can_init()
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
logcan = messaging.sub_sock(context, service_list['can'].port)

5
selfdrive/can/Makefile
View File

@ -1,6 +1,7 @@
CC = clang
CXX = clang++
BASEDIR = ../..
PHONELIBS := ../../phonelibs
UNAME_S := $(shell uname -s)
@ -53,7 +54,7 @@ libdbc.so:: ../../cereal/gen/cpp/log.capnp.h
../../cereal/gen/cpp/log.capnp.h:
cd ../../cereal && make
libdbc.so:: parser.cc $(DBC_CCS)
libdbc.so:: dbc.cc parser.cc packer.cc $(DBC_CCS)
$(CXX) -fPIC -shared -o '$@' $^ \
-I. \
-I../.. \
@ -63,7 +64,7 @@ libdbc.so:: parser.cc $(DBC_CCS)
$(CEREAL_CXXFLAGS) \
$(CEREAL_LIBS)
dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc
dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc process_dbc.py dbc_template.cc
PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py '$<' '$@'
.PHONY: clean

13
selfdrive/can/parser_common.h → selfdrive/can/common.h
View File

@ -1,13 +1,20 @@
#ifndef PARSER_COMMON_H
#define PARSER_COMMON_H
#ifndef SELFDRIVE_CAN_COMMON_H
#define SELFDRIVE_CAN_COMMON_H
#include <cstddef>
#include <cstdint>
#include <string>
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
struct SignalPackValue {
const char* name;
double value;
};
struct SignalParseOptions {
uint32_t address;
const char* name;
@ -54,6 +61,8 @@ struct DBC {
const Msg *msgs;
};
const DBC* dbc_lookup(const std::string& dbc_name);
void dbc_register(const DBC* dbc);
#define dbc_init(dbc) \

26
selfdrive/can/dbc.cc 100644
View File

@ -0,0 +1,26 @@
#include <string>
#include <vector>
#include "common.h"
namespace {
std::vector<const DBC*>& get_dbcs() {
static std::vector<const DBC*> vec;
return vec;
}
}
const DBC* dbc_lookup(const std::string& dbc_name) {
for (const auto& dbci : get_dbcs()) {
if (dbc_name == dbci->name) {
return dbci;
}
}
return NULL;
}
void dbc_register(const DBC* dbc) {
get_dbcs().push_back(dbc);
}

2
selfdrive/can/dbc_template.cc
View File

@ -1,6 +1,6 @@
#include <cstdint>
#include "parser_common.h"
#include "common.h"
namespace {

51
selfdrive/can/libdbc_py.py 100644
View File

@ -0,0 +1,51 @@
import os
import subprocess
from cffi import FFI
can_dir = os.path.dirname(os.path.abspath(__file__))
libdbc_fn = os.path.join(can_dir, "libdbc.so")
subprocess.check_output(["make"], cwd=can_dir)
ffi = FFI()
ffi.cdef("""
typedef struct SignalParseOptions {
uint32_t address;
const char* name;
double default_value;
} SignalParseOptions;
typedef struct MessageParseOptions {
uint32_t address;
int check_frequency;
} MessageParseOptions;
typedef struct SignalValue {
uint32_t address;
uint16_t ts;
const char* name;
double value;
} SignalValue;
void* can_init(int bus, const char* dbc_name,
size_t num_message_options, const MessageParseOptions* message_options,
size_t num_signal_options, const SignalParseOptions* signal_options);
void can_update(void* can, uint64_t sec, bool wait);
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
typedef struct SignalPackValue {
const char* name;
double value;
} SignalPackValue;
void* canpack_init(const char* dbc_name);
uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals);
""")
libdbc = ffi.dlopen(libdbc_fn)

81
selfdrive/can/packer.cc 100644
View File

@ -0,0 +1,81 @@
#include <cassert>
#include <string>
#include <vector>
#include <utility>
#include <algorithm>
#include <map>
#include "common.h"
#define WARN printf
namespace {
class CANPacker {
public:
CANPacker(const std::string& dbc_name) {
dbc = dbc_lookup(dbc_name);
assert(dbc);
for (int i=0; i<dbc->num_msgs; i++) {
const Msg* msg = &dbc->msgs[i];
for (int j=0; j<msg->num_sigs; j++) {
const Signal* sig = &msg->sigs[j];
signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig;
}
}
}
uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals) {
uint64_t ret = 0;
for (const auto& sigval : signals) {
std::string name = std::string(sigval.name);
double value = sigval.value;
auto sig_it = signal_lookup.find(make_pair(address, name));
if (sig_it == signal_lookup.end()) {
WARN("undefined signal %s", name.c_str());
continue;
}
auto sig = sig_it->second;
int64_t ival = (int64_t)((value - sig.offset) / sig.factor);
if (ival < 0) {
WARN("signed pack unsupported right now");
continue;
}
uint64_t mask = ((1ULL << sig.b2)-1) << sig.bo;
uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << sig.bo;
ret &= ~mask;
ret |= dat;
}
return ret;
}
private:
const DBC *dbc = NULL;
std::map<std::pair<uint32_t, std::string>, Signal> signal_lookup;
};
}
extern "C" {
void* canpack_init(const char* dbc_name) {
CANPacker *ret = new CANPacker(std::string(dbc_name));
return (void*)ret;
}
uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals) {
CANPacker *cp = (CANPacker*)inst;
return cp->pack(address, std::vector<SignalPackValue>(vals, vals+num_vals));
}
}

37
selfdrive/can/packer.py 100644
View File

@ -0,0 +1,37 @@
import struct
from selfdrive.can.libdbc_py import libdbc, ffi
class CANPacker(object):
def __init__(self, dbc_name):
self.packer = libdbc.canpack_init(dbc_name)
self.sig_names = {}
def pack(self, addr, values):
# values: [(signal_name, signal_value)]
values_thing = []
for name, value in values:
if name not in self.sig_names:
self.sig_names[name] = ffi.new("char[]", name)
values_thing.append({
'name': self.sig_names[name],
'value': value
})
values_c = ffi.new("SignalPackValue[]", values_thing)
return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c)
def pack_bytes(self, addr, values):
return struct.pack(">Q", self.pack(addr, values))
if __name__ == "__main__":
cp = CANPacker("honda_civic_touring_2016_can")
s = cp.pack_bytes(0x30c, [
("PCM_SPEED", 123),
("PCM_GAS", 10),
])
print s.encode("hex")

32
selfdrive/can/parser.cc
View File

@ -18,7 +18,7 @@
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "parser_common.h"
#include "common.h"
#define DEBUG(...)
// #define DEBUG printf
@ -40,8 +40,6 @@ uint64_t read_u64_be(const uint8_t* v) {
| (uint64_t)v[7]);
}
std::vector<const DBC*> g_dbc;
bool honda_checksum(int address, uint64_t d, int l) {
int target = (d >> l) & 0xF;
@ -77,9 +75,9 @@ struct MessageState {
for (int i=0; i < parse_sigs.size(); i++) {
auto& sig = parse_sigs[i];
int64_t tmp = (dat >> sig.bo) & ((1 << sig.b2)-1);
int64_t tmp = (dat >> sig.bo) & ((1ULL << sig.b2)-1);
if (sig.is_signed) {
tmp -= (tmp >> (sig.b2-1)) ? (1<<sig.b2) : 0; //signed
tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed
}
DEBUG("parse %X %s -> %ld\n", address, sig.name, tmp);
@ -123,6 +121,7 @@ struct MessageState {
};
class CANParser {
public:
CANParser(int abus, const std::string& dbc_name,
@ -135,15 +134,10 @@ class CANParser {
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
zmq_connect(subscriber, "tcp://127.0.0.1:8006");
for (auto dbci : g_dbc) {
if (dbci->name == dbc_name) {
dbc = dbci;
break;
}
}
dbc = dbc_lookup(dbc_name);
assert(dbc);
for (auto &op : options) {
for (const auto& op : options) {
MessageState state = {
.address = op.address,
// .check_frequency = op.check_frequency,
@ -178,7 +172,7 @@ class CANParser {
}
// track requested signals for this message
for (auto &sigop : sigoptions) {
for (const auto& sigop : sigoptions) {
if (sigop.address != op.address) continue;
for (int i=0; i<msg->num_sigs; i++) {
@ -230,8 +224,8 @@ class CANParser {
void UpdateValid(uint64_t sec) {
can_valid = true;
for (auto &kv : message_states) {
auto &state = kv.second;
for (const auto& kv : message_states) {
const auto& state = kv.second;
if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
if (state.seen > 0) {
INFO("%X TIMEOUT\n", state.address);
@ -279,8 +273,8 @@ class CANParser {
std::vector<SignalValue> query(uint64_t sec) {
std::vector<SignalValue> ret;
for (auto &kv : message_states) {
auto &state = kv.second;
for (const auto& kv : message_states) {
const auto& state = kv.second;
if (sec != 0 && state.seen != sec) continue;
for (int i=0; i<state.parse_sigs.size(); i++) {
@ -311,10 +305,6 @@ class CANParser {
}
void dbc_register(const DBC* dbc) {
g_dbc.push_back(dbc);
}
extern "C" {
void* can_init(int bus, const char* dbc_name,

40
selfdrive/can/parser.py
View File

@ -1,46 +1,8 @@
import os
import time
import subprocess
from collections import defaultdict
from cffi import FFI
can_dir = os.path.dirname(os.path.abspath(__file__))
libdbc_fn = os.path.join(can_dir, "libdbc.so")
subprocess.check_output(["make"], cwd=can_dir)
ffi = FFI()
ffi.cdef("""
typedef struct SignalParseOptions {
uint32_t address;
const char* name;
double default_value;
} SignalParseOptions;
typedef struct MessageParseOptions {
uint32_t address;
int check_frequency;
} MessageParseOptions;
typedef struct SignalValue {
uint32_t address;
uint16_t ts;
const char* name;
double value;
} SignalValue;
void* can_init(int bus, const char* dbc_name,
size_t num_message_options, const MessageParseOptions* message_options,
size_t num_signal_options, const SignalParseOptions* signal_options);
void can_update(void* can, uint64_t sec, bool wait);
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
""")
libdbc = ffi.dlopen(libdbc_fn)
from selfdrive.can.libdbc_py import libdbc, ffi
class CANParser(object):
def __init__(self, dbc_name, signals, checks=[], bus=0):

6
selfdrive/car/__init__.py
View File

@ -2,6 +2,11 @@ from common.fingerprints import fingerprint
from .honda.interface import CarInterface as HondaInterface
try:
from .toyota.interface import CarInterface as ToyotaInterface
except ImportError:
ToyotaInterface = None
try:
from .simulator.interface import CarInterface as SimInterface
except ImportError:
@ -17,6 +22,7 @@ interfaces = {
"ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface,
"HONDA ACCORD 2016 TOURING": HondaInterface,
"HONDA CR-V 2016 TOURING": HondaInterface,
"TOYOTA PRIUS 2017": ToyotaInterface,
"simulator": SimInterface,
"simulator2": Sim2Interface

86
selfdrive/car/honda/carcontroller.py
View File

@ -1,5 +1,5 @@
from collections import namedtuple
import os
import common.numpy_fast as np
from common.numpy_fast import clip, interp
from common.realtime import sec_since_boot
@ -10,25 +10,25 @@ from selfdrive.controls.lib.drive_helpers import rate_limit
from . import hondacan
def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
# hyst params... TODO: move these to VehicleParams
brake_hyst_on = 0.055 if civic else 0.1 # to activate brakes exceed this value
brake_hyst_off = 0.005 # to deactivate brakes below this value
brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value
#*** histeresys logic to avoid brake blinking. go above 0.1 to trigger
if (final_brake < brake_hyst_on and not braking) or final_brake < brake_hyst_off:
final_brake = 0.
braking = final_brake > 0.
if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
brake = 0.
braking = brake > 0.
# for small brake oscillations within brake_hyst_gap, don't change the brake command
if final_brake == 0.:
if brake == 0.:
brake_steady = 0.
elif final_brake > brake_steady + brake_hyst_gap:
brake_steady = final_brake - brake_hyst_gap
elif final_brake < brake_steady - brake_hyst_gap:
brake_steady = final_brake + brake_hyst_gap
final_brake = brake_steady
elif brake > brake_steady + brake_hyst_gap:
brake_steady = brake - brake_hyst_gap
elif brake < brake_steady - brake_hyst_gap:
brake_steady = brake + brake_hyst_gap
brake = brake_steady
if not civic:
brake_on_offset_v = [.25, .15] # min brake command on brake activation. below this no decel is perceived
@ -36,10 +36,10 @@ def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
# offset the brake command for threshold in the brake system. no brake torque perceived below it
brake_on_offset = interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
brake_offset = brake_on_offset - brake_hyst_on
if final_brake > 0.0:
final_brake += brake_offset
if brake > 0.0:
brake += brake_offset
return final_brake, braking, brake_steady
return brake, braking, brake_steady
class AH:
#[alert_idx, value]
@ -78,12 +78,9 @@ class CarController(object):
def __init__(self):
self.braking = False
self.brake_steady = 0.
self.final_brake_last = 0.
self.brake_last = 0.
# redundant safety check with the board
self.controls_allowed = False
def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_steer, \
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
@ -94,20 +91,16 @@ class CarController(object):
return
# *** apply brake hysteresis ***
final_brake, self.braking, self.brake_steady = actuator_hystereses(final_brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
# *** no output if not enabled ***
if not enabled:
final_gas = 0.
final_brake = 0.
final_steer = 0.
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
if CS.pcm_acc_status:
pcm_cancel_cmd = True
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
final_brake = rate_limit(final_brake, self.final_brake_last, -2., 1./100)
self.final_brake_last = final_brake
brake = rate_limit(brake, self.brake_last, -2., 1./100)
self.brake_last = brake
# vehicle hud display, wait for one update from 10Hz 0x304 msg
#TODO: use enum!!
@ -143,7 +136,8 @@ class CarController(object):
GAS_MAX = 1004
BRAKE_MAX = 1024/4
if CS.civic:
STEER_MAX = 0x1000
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
elif CS.crv:
STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value
else:
@ -151,50 +145,22 @@ class CarController(object):
GAS_OFFSET = 328
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = int(clip(final_gas*GAS_MAX, 0, GAS_MAX-1))
apply_brake = int(clip(final_brake*BRAKE_MAX, 0, BRAKE_MAX-1))
apply_steer = int(clip(-final_steer*STEER_MAX, -STEER_MAX, STEER_MAX))
apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1))
apply_brake = int(clip(brake * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
# no gas if you are hitting the brake or the user is
if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed):
print "CANCELLING GAS", apply_brake
apply_gas = 0
# no computer brake if the gas is being pressed
if CS.car_gas > 0 and apply_brake != 0:
print "CANCELLING BRAKE"
apply_brake = 0
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
if CS.steer_not_allowed:
print "STEER ALERT, TORQUE INHIBITED"
apply_steer = 0
# *** entry into controls state ***
if (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) and \
CS.cruise_buttons == 0 and not self.controls_allowed:
print "CONTROLS ARE LIVE"
self.controls_allowed = True
# *** exit from controls state on cancel, gas, or brake ***
if (CS.cruise_buttons == CruiseButtons.CANCEL or CS.brake_pressed or
CS.user_gas_pressed or (CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed:
print "CONTROLS ARE DEAD"
self.controls_allowed = False
# *** controls fail on steer error, brake error, or invalid can ***
if CS.steer_error:
print "STEER ERROR"
self.controls_allowed = False
if CS.brake_error:
print "BRAKE ERROR"
self.controls_allowed = False
if not CS.can_valid and self.controls_allowed: # 200 ms
print "CAN INVALID"
self.controls_allowed = False
# Send CAN commands.
can_sends = []

143
selfdrive/car/honda/carstate.py
View File

@ -1,18 +1,54 @@
import os
import time
from cereal import car
import common.numpy_fast as np
from common.realtime import sec_since_boot
import selfdrive.messaging as messaging
from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.honda.can_parser import CANParser
from selfdrive.can.parser import CANParser as CANParserC
NEW_CAN = os.getenv("OLD_CAN") is None
def parse_gear_shifter(can_gear_shifter, is_acura):
def get_can_parser(CP):
# this function generates lists for signal, messages and initial values
if can_gear_shifter == 0x1:
return "park"
elif can_gear_shifter == 0x2:
return "reverse"
if is_acura:
if can_gear_shifter == 0x3:
return "neutral"
elif can_gear_shifter == 0x4:
return "drive"
elif can_gear_shifter == 0xa:
return "sport"
else:
if can_gear_shifter == 0x4:
return "neutral"
elif can_gear_shifter == 0x8:
return "drive"
elif can_gear_shifter == 0x10:
return "sport"
elif can_gear_shifter == 0x20:
return "low"
return "unknown"
_K0 = -0.3
_K1 = -0.01879
_K2 = 0.01013
def calc_cruise_offset(offset, speed):
# euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
# constraints to solve for _K0, _K1, _K2 are:
# - speed = 0m/s, out = -0.3
# - speed = 34m/s, offset = 20, out = -0.25
# - speed = 34m/s, offset = -2.5, out = -1.8
return min(_K0 + _K1 * speed + _K2 * speed * offset, 0.)
def get_can_signals(CP):
# this function generates lists for signal, messages and initial values
if CP.carFingerprint == "HONDA CIVIC 2016 TOURING":
dbc_f = 'honda_civic_touring_2016_can.dbc'
signals = [
@ -50,8 +86,9 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x296, 0),
("LEFT_BLINKER", 0x326, 0),
("RIGHT_BLINKER", 0x326, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
("CRUISE_SPEED_OFFSET", 0x37c, 0),
("EPB_STATE", 0x1c2, 0),
("BRAKE_HOLD_ACTIVE", 0x1A4, 0),
]
checks = [
# address, frequency
@ -65,6 +102,7 @@ def get_can_parser(CP):
(0x1d0, 50),
(0x305, 10),
(0x324, 10),
(0x37c, 10),
(0x405, 3),
]
@ -104,8 +142,7 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
("CRUISE_SPEED_OFFSET", 0x37c, 0)
]
checks = [
(0x156, 100),
@ -118,6 +155,7 @@ def get_can_parser(CP):
(0x1d0, 50),
(0x305, 10),
(0x324, 10),
(0x37c, 10),
(0x405, 3),
]
elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING":
@ -157,8 +195,6 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
]
checks = [
(0x156, 100),
@ -209,8 +245,6 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
]
checks = [
(0x156, 100),
@ -230,22 +264,23 @@ def get_can_parser(CP):
signals.append(("INTERCEPTOR_GAS", 0x201, 0))
checks.append((0x201, 50))
if NEW_CAN:
return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 0)
else:
return CANParser(dbc_f, signals, checks)
return dbc_f, signals, checks
def get_can_parser(CP):
dbc_f, signals, checks = get_can_signals(CP)
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0)
class CarState(object):
def __init__(self, CP, logcan):
self.acura = False
self.civic = False
self.accord = False
self.crv = False
if CP.carFingerprint == "HONDA CIVIC 2016 TOURING":
self.civic = True
elif CP.carFingerprint == "ACURA ILX 2016 ACURAWATCH PLUS":
self.civic = False
self.acura = True
elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING":
# fake civic
self.accord = True
elif CP.carFingerprint == "HONDA CR-V 2016 TOURING":
self.crv = True
@ -274,10 +309,7 @@ class CarState(object):
def update(self, can_pub_main=None):
cp = self.cp
if NEW_CAN:
cp.update(int(sec_since_boot() * 1e9), False)
else:
cp.update_can(can_pub_main)
cp.update(int(sec_since_boot() * 1e9), False)
# copy can_valid
self.can_valid = cp.can_valid
@ -294,8 +326,6 @@ class CarState(object):
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.rpm = cp.vl[0x17C]['ENGINE_RPM']
# ******************* parse out can *******************
self.door_all_closed = not any([cp.vl[0x405]['DOOR_OPEN_FL'], cp.vl[0x405]['DOOR_OPEN_FR'],
cp.vl[0x405]['DOOR_OPEN_RL'], cp.vl[0x405]['DOOR_OPEN_RR']])
@ -316,9 +346,11 @@ class CarState(object):
self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2']
self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel = (
cp.vl[0x1D0]['WHEEL_SPEED_FL'] + cp.vl[0x1D0]['WHEEL_SPEED_FR'] +
cp.vl[0x1D0]['WHEEL_SPEED_RL'] + cp.vl[0x1D0]['WHEEL_SPEED_RR']) / 4.
self.v_wheel_fl = cp.vl[0x1D0]['WHEEL_SPEED_FL']
self.v_wheel_fr = cp.vl[0x1D0]['WHEEL_SPEED_FR']
self.v_wheel_rl = cp.vl[0x1D0]['WHEEL_SPEED_RL']
self.v_wheel_rr = cp.vl[0x1D0]['WHEEL_SPEED_RR']
self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
# blend in transmission speed at low speed, since it has more low speed accuracy
self.v_weight = np.interp(self.v_wheel, v_weight_bp, v_weight_v)
self.v_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
@ -328,49 +360,59 @@ class CarState(object):
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
#print self.user_gas, self.user_gas_pressed
if self.civic:
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x14A]['STEER_ANGLE']
self.gear = 0 # TODO: civic has CVT... needs rev engineering
self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x326]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x326]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x326]['RIGHT_BLINKER']
self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego)
self.park_brake = cp.vl[0x1c2]['EPB_STATE'] != 0
self.brake_hold = cp.vl[0x1A4]['BRAKE_HOLD_ACTIVE']
elif self.accord:
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
self.gear = 0 # TODO: accord has CVT... needs rev engineering
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x1A6]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
self.cruise_speed_offset = -0.3
self.park_brake = 0 # TODO
self.brake_hold = 0 # TODO
elif self.crv:
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
self.gear = cp.vl[0x191]['GEAR']
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x1A6]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
else:
self.gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
self.cruise_speed_offset = -0.3
self.park_brake = 0 # TODO
self.brake_hold = 0 # TODO
elif self.acura:
can_gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
self.gear = cp.vl[0x1A3]['GEAR']
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x1A6]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,4] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego)
self.park_brake = 0 # TODO
self.brake_hold = 0
self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.acura)
if self.accord:
# on the accord, this doesn't seem to include cruise control
@ -398,4 +440,25 @@ class CarState(object):
self.pcm_acc_status = cp.vl[0x17C]['ACC_STATUS']
self.pedal_gas = cp.vl[0x17C]['PEDAL_GAS']
self.hud_lead = cp.vl[0x30C]['HUD_LEAD']
self.counter_pcm = cp.vl[0x324]['COUNTER']
# carstate standalone tester
if __name__ == '__main__':
import zmq
import time
from selfdrive.services import service_list
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port)
class CarParams(object):
def __init__(self):
self.carFingerprint = "HONDA CIVIC 2016 TOURING"
self.enableGas = 0
self.enableCruise = 0
CP = CarParams()
CS = CarState(CP, logcan)
while 1:
CS.update()
time.sleep(0.01)

231
selfdrive/car/honda/interface.py
View File

@ -1,19 +1,65 @@
#!/usr/bin/env python
import os
import time
import common.numpy_fast as np
import numpy as np
from common.numpy_fast import clip
from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
from .carstate import CarState
from .carcontroller import CarController, AH
from selfdrive.boardd.boardd import can_capnp_to_can_list
from cereal import car
from selfdrive.services import service_list
import selfdrive.messaging as messaging
NEW_CAN = os.getenv("OLD_CAN") is None
def get_compute_gb():
# generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0]
# where -1.0 is max brake and 1.0 is max gas
# see debug/dump_accel_from_fiber.py to see how those parameters were generated
w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657],
[ 1.03691769, 0.78210306, -0.41343188]])
b0 = np.array([ 0.01536703, -0.14335321, -0.26932889])
w2 = np.array([[-0.59124422, 0.42899439, 0.38660881],
[ 0.79973811, 0.13178682, 0.08550351],
[-0.15651935, -0.44360259, 0.76910877]])
b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ])
w4 = np.array([[-0.31521443],
[-0.38626176],
[ 0.52667892]])
b4 = np.array([-0.02922216])
def compute_output(dat, w0, b0, w2, b2, w4, b4):
m0 = np.dot(dat, w0) + b0
m0 = leakyrelu(m0, 0.1)
m2 = np.dot(m0, w2) + b2
m2 = leakyrelu(m2, 0.1)
m4 = np.dot(m2, w4) + b4
return m4
def leakyrelu(x, alpha):
return np.maximum(x, alpha * x)
def _compute_gb(accel, speed):
#linearly extrap below v1 using v1 and v2 data
v1 = 5.
v2 = 10.
dat = np.array([accel, speed])
if speed > 5.:
m4 = compute_output(dat, w0, b0, w2, b2, w4, b4)
else:
dat[1] = v1
m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4)
dat[1] = v2
m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4)
m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
return float(m4)
return _compute_gb
# Car button codes
class CruiseButtons:
@ -22,6 +68,7 @@ class CruiseButtons:
CANCEL = 2
MAIN = 1
#car chimes: enumeration from dbc file. Chimes are for alerts and warnings
class CM:
MUTE = 0
@ -30,6 +77,7 @@ class CM:
REPEATED = 1
CONTINUOUS = 2
#car beepss: enumeration from dbc file. Beeps are for activ and deactiv
class BP:
MUTE = 0
@ -37,12 +85,17 @@ class BP:
TRIPLE = 2
REPEATED = 1
class CarInterface(object):
def __init__(self, CP, logcan, sendcan=None):
self.logcan = logcan
self.CP = CP
self.frame = 0
self.last_enable_pressed = 0
self.last_enable_sent = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
# *** init the major players ***
@ -54,7 +107,8 @@ class CarInterface(object):
self.CC = CarController()
if self.CS.accord:
self.accord_msg = []
# self.accord_msg = []
raise NotImplementedError
@staticmethod
def get_params(candidate, fingerprint):
@ -68,6 +122,8 @@ class CarInterface(object):
ret.radarName = "nidec"
ret.carFingerprint = candidate
ret.safetyModel = car.CarParams.SafetyModels.honda
ret.enableSteer = True
ret.enableBrake = True
@ -75,7 +131,7 @@ class CarInterface(object):
ret.enableGas = 0x201 in fingerprint
ret.enableCruise = not ret.enableGas
# FIXME: hardcoding honda civic 2016 touring wight so it can be used to
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
m_civic = 2923./2.205 + std_cargo
l_civic = 2.70
@ -86,27 +142,32 @@ class CarInterface(object):
cR_civic = 90000
if candidate == "HONDA CIVIC 2016 TOURING":
stop_and_go = True
ret.m = m_civic
ret.l = l_civic
ret.aF = aF_civic
ret.sR = 13.0
ret.steerKp, ret.steerKi = 0.8, 0.24
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
stop_and_go = False
ret.m = 3095./2.205 + std_cargo
ret.l = 2.67
ret.aF = ret.l * 0.37
ret.sR = 15.3
# Acura at comma has modified steering FW, so different tuning for the Neo in that car
# FIXME: using dongleId isn't great, better to identify the car than the Neo
is_fw_modified = os.getenv("DONGLE_ID") == 'cb38263377b873ee'
is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee']
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
elif candidate == "HONDA ACCORD 2016 TOURING":
stop_and_go = False
ret.m = 3580./2.205 + std_cargo
ret.l = 2.74
ret.aF = ret.l * 0.38
ret.sR = 15.3
ret.steerKp, ret.steerKi = 0.8, 0.24
elif candidate == "HONDA CR-V 2016 TOURING":
stop_and_go = False
ret.m = 3572./2.205 + std_cargo
ret.l = 2.62
ret.aF = ret.l * 0.41
@ -114,48 +175,54 @@ class CarInterface(object):
ret.steerKp, ret.steerKi = 0.8, 0.24
else:
raise ValueError("unsupported car %s" % candidate)
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
# conflict with PCM acc
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGas) else 25.5 * CV.MPH_TO_MS
ret.aR = ret.l - ret.aF
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_civic**2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position... all cars will have approximately similar dyn behaviors
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.cF = cF_civic * ret.m / m_civic * (ret.aR / ret.l) / (aR_civic / l_civic)
ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_civic)
# no rear steering, at least on the listed cars above
ret.chi = 0.
# no max steer limit VS speed
ret.steerMaxBP = [0.] # m/s
ret.steerMaxV = [1.] # max steer allowed
ret.gasMaxBP = [0.] # m/s
ret.gasMaxV = [0.6] if ret.enableGas else [0.] # max gas allowed
ret.brakeMaxBP = [5., 20.] # m/s
ret.brakeMaxV = [1., 0.8] # max brake allowed
return ret
compute_gb = staticmethod(get_compute_gb())
# returns a car.CarState
def update(self):
def update(self, c):
# ******************* do can recv *******************
can_pub_main = []
canMonoTimes = []
if NEW_CAN:
self.CS.update(can_pub_main)
else:
for a in messaging.drain_sock(self.logcan):
canMonoTimes.append(a.logMonoTime)
can_pub_main.extend(can_capnp_to_can_list(a.can, [0,0x80]))
if self.CS.accord:
self.accord_msg.extend(can_capnp_to_can_list(a.can, [9]))
self.accord_msg = self.accord_msg[-1:]
self.CS.update(can_pub_main)
self.CS.update(can_pub_main)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.wheelSpeeds.fl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FL']
ret.wheelSpeeds.fr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FR']
ret.wheelSpeeds.rl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RL']
ret.wheelSpeeds.rr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RR']
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal
ret.gas = self.CS.car_gas / 256.0
@ -172,28 +239,17 @@ class CarInterface(object):
# TODO: units
ret.steeringAngle = self.CS.angle_steers
if self.CS.accord:
# TODO: move this into the CAN parser
ret.steeringTorque = 0
if len(self.accord_msg) > 0:
aa = map(lambda x: ord(x)&0x7f, self.accord_msg[0][2])
if len(aa) != 5 or (-(aa[0]+aa[1]+aa[2]+aa[3]))&0x7f != aa[4]:
print "ACCORD MSG BAD LEN OR CHECKSUM!"
# TODO: throw an error here?
else:
st = ((aa[0]&0xF) << 5) + (aa[1]&0x1F)
if st >= 256:
st = -(512-st)
ret.steeringTorque = st
ret.steeringPressed = abs(ret.steeringTorque) > 20
else:
ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
ret.steeringPressed = self.CS.steer_override
# gear shifter lever
ret.gearShifter = self.CS.gear_shifter
ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
# TODO: button presses
buttonEvents = []
@ -244,38 +300,94 @@ class CarInterface(object):
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# errors
# events
# TODO: I don't like the way capnp does enums
# These strings aren't checked at compile time
errors = []
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
errors.append('commIssue')
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if self.CS.steer_error:
errors.append('steerUnavailable')
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
elif self.CS.steer_not_allowed:
errors.append('steerTempUnavailable')
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if self.CS.brake_error:
errors.append('brakeUnavailable')
if not self.CS.gear_shifter_valid:
errors.append('wrongGear')
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not ret.gearShifter == 'drive':
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.door_all_closed:
errors.append('doorOpen')
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.seatbelt:
errors.append('seatbeltNotLatched')
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
errors.append('espDisabled')
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
errors.append('wrongCarMode')
if self.CS.gear_shifter == 2:
errors.append('reverseGear')
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.brake_hold:
events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.park_brake:
events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
ret.errors = errors
if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
#if (ret.brakePressed and ret.vEgo < 0.001) or ret.gasPressed:
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
# it can happen that car cruise disables while comma system is enabled: need to
# keep braking if needed or if the speed is very low
# TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
if self.CP.enableCruise and not ret.cruiseState.enabled and \
(c.actuators.brake <= 0. or ret.vEgo < 0.3):
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
cur_time = sec_since_boot()
enable_pressed = False
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
print "enabled pressed at", cur_time
self.last_enable_pressed = cur_time
enable_pressed = True
# do disable on button down
if b.type == "cancel" and b.pressed:
events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
if self.CP.enableCruise:
# KEEP THIS EVENT LAST! send enable event if button is pressed and there are
# NO_ENTRY events, so controlsd will display alerts. Also not send enable events
# too close in time, so a no_entry will not be followed by another one.
# TODO: button press should be the only thing that triggers enble
if ((cur_time - self.last_enable_pressed) < 0.2 and
(cur_time - self.last_enable_sent) > 0.2 and
ret.cruiseState.enabled) or \
(enable_pressed and get_events(events, [ET.NO_ENTRY])):
events.append(create_event('buttonEnable', [ET.ENABLE]))
self.last_enable_sent = cur_time
elif enable_pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
ret.events = events
ret.canMonoTimes = canMonoTimes
# update previous brake/gas pressed
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified
#print ret
return ret.as_reader()
@ -309,10 +421,10 @@ class CarInterface(object):
"chimeRepeated": (BP.MUTE, CM.REPEATED),
"chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)]
pcm_accel = int(np.clip(c.cruiseControl.accelOverride,0,1)*0xc6)
pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6)
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \
c.gas, c.brake, c.steeringTorque, \
c.actuators, \
c.cruiseControl.speedOverride, \
c.cruiseControl.override, \
c.cruiseControl.cancel, \
@ -324,4 +436,3 @@ class CarInterface(object):
snd_chime = snd_chime)
self.frame += 1
return not (c.enabled and not self.CC.controls_allowed)

8
selfdrive/car/honda/can_parser.py → selfdrive/car/honda/old_can_parser.py
View File

@ -36,15 +36,16 @@ class CANParser(object):
for _, addr, _ in signals:
self.vl[addr] = {}
self.ts[addr] = 0
self.ts[addr] = {}
self.ct[addr] = sec_since_boot()
self.ok[addr] = False
self.ok[addr] = True
self.cn[addr] = 0
self.cn_vl[addr] = 0
self.ck[addr] = False
for name, addr, ival in signals:
self.vl[addr][name] = ival
self.ts[addr][name] = 0
self._msgs = [s[1] for s in signals]
self._sgs = [s[0] for s in signals]
@ -94,7 +95,6 @@ class CANParser(object):
self.ok[msg] = False
# update msg time stamps and counter value
self.ts[msg] = ts
self.ct[msg] = self.sec_since_boot_cached
self.cn[msg] = cn
self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max)
@ -107,12 +107,14 @@ class CANParser(object):
for ii in idxs:
sg = self._sgs[ii]
self.vl[msg][sg] = out[sg]
self.ts[msg][sg] = ts
# for each message, check if it's too long since last time we received it
self._check_dead_msgs()
# assess overall can validity: if there is one relevant message invalid, then set can validity flag to False
self.can_valid = True
if False in self.ok.values():
#print "CAN INVALID!"
self.can_valid = False

20
selfdrive/common/cereal.mk
View File

@ -1,14 +1,27 @@
UNAME_M ?= $(shell uname -m)
ifeq ($(UNAME_M),x86_64)
UNAME_S ?= $(shell uname -s)
CEREAL_CFLAGS = -I$(ONE)/external/capnp/include
ifeq ($(UNAME_S),Darwin)
CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include
CEREAL_LIBS = $(PHONELIBS)/capnp-cpp/mac/lib/libcapnp.a \
$(PHONELIBS)/capnp-cpp/mac/lib/libkj.a \
$(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a
else ifeq ($(UNAME_M),x86_64)
CEREAL_CFLAGS = -I$(BASEDIR)/external/capnp/include
CEREAL_CXXFLAGS = $(CEREAL_CFLAGS)
ifeq ($(CEREAL_LIBS),)
CEREAL_LIBS = -L$(ONE)/external/capnp/lib \
CEREAL_LIBS = -L$(BASEDIR)/external/capnp/lib \
-l:libcapnp.a -l:libkj.a -l:libcapnp_c.a
endif
else
CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include
ifeq ($(CEREAL_LIBS),)
@ -30,4 +43,3 @@ car.capnp.o: ../../cereal/gen/cpp/car.capnp.c++
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) $(CEREAL_CXXFLAGS) \
-c -o '$@' '$<'

20
selfdrive/common/mat.h
View File

@ -65,4 +65,24 @@ static inline vec4 matvecmul(const mat4 a, const vec4 b) {
return ret;
}
// scales the input and output space of a transformation matrix
// that assumes pixel-center origin.
static inline mat3 transform_scale_buffer(const mat3 in, float s) {
// in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
mat3 transform_out = {{
1.0f/s, 0.0f, 0.5f,
0.0f, 1.0f/s, 0.5f,
0.0f, 0.0f, 1.0f,
}};
mat3 transform_in = {{
s, 0.0f, -0.5f*s,
0.0f, s, -0.5f*s,
0.0f, 0.0f, 1.0f,
}};
return matmul3(transform_in, matmul3(in, transform_out));
}
#endif

25
selfdrive/common/params.c → selfdrive/common/params.cc
View File

@ -11,6 +11,18 @@
#include <stdlib.h>
#include <stdio.h>
namespace {
template <typename T>
T* null_coalesce(T* a, T* b) {
return a != NULL ? a : b;
}
static const char* default_params_path = null_coalesce(
const_cast<const char*>(getenv("PARAMS_PATH")), "/data/params");
} // namespace
int write_db_value(const char* params_path, const char* key, const char* value,
size_t value_size) {
int lock_fd = -1;
@ -18,6 +30,11 @@ int write_db_value(const char* params_path, const char* key, const char* value,
int result;
char tmp_path[1024];
char path[1024];
ssize_t bytes_written;
if (params_path == NULL) {
params_path = default_params_path;
}
// Write value to temp.
result =
@ -27,7 +44,7 @@ int write_db_value(const char* params_path, const char* key, const char* value,
}
tmp_fd = mkstemp(tmp_path);
const ssize_t bytes_written = write(tmp_fd, value, value_size);
bytes_written = write(tmp_fd, value, value_size);
if (bytes_written != value_size) {
result = -20;
goto cleanup;
@ -79,6 +96,10 @@ int read_db_value(const char* params_path, const char* key, char** value,
int result;
char path[1024];
if (params_path == NULL) {
params_path = default_params_path;
}
result = snprintf(path, sizeof(path), "%s/.lock", params_path);
if (result < 0) {
goto cleanup;
@ -99,7 +120,7 @@ int read_db_value(const char* params_path, const char* key, char** value,
// Read value.
// TODO(mgraczyk): If there is a lot of contention, we can release the lock
// after opening the file, before reading.
*value = read_file(path, value_sz);
*value = static_cast<char*>(read_file(path, value_sz));
if (*value == NULL) {
result = -22;
goto cleanup;

4
selfdrive/common/params.h
View File

@ -7,14 +7,12 @@
extern "C" {
#endif
#define PARAMS_PATH "/data/params"
int write_db_value(const char* params_path, const char* key, const char* value,
size_t value_size);
// Reads a value from the params database.
// Inputs:
// params_path: The path of the database, eg /sdcard/params.
// params_path: The path of the database, or NULL to use the default.
// key: The key to read.
// value: A pointer where a newly allocated string containing the db value will
// be written.

36
selfdrive/common/swaglog.h
View File

@ -1,6 +1,8 @@
#ifndef SWAGLOG_H
#define SWAGLOG_H
#include "common/timing.h"
#define CLOUDLOG_DEBUG 10
#define CLOUDLOG_INFO 20
#define CLOUDLOG_WARNING 30
@ -24,9 +26,43 @@ void cloudlog_bind(const char* k, const char* v);
__func__, \
fmt, ## __VA_ARGS__)
#define cloudlog_rl(burst, millis, lvl, fmt, ...) \
{ \
static uint64_t __begin = 0; \
static int __printed = 0; \
static int __missed = 0; \
\
int __burst = (burst); \
int __millis = (millis); \
uint64_t __ts = nanos_since_boot(); \
\
if (!__begin) __begin = __ts; \
\
if (__begin + __millis*1000000ULL < __ts) { \
if (__missed) { \
cloudlog(CLOUDLOG_WARNING, "cloudlog: %d messages supressed", __missed); \
} \
__begin = 0; \
__printed = 0; \
__missed = 0; \
} \
\
if (__printed < __burst) { \
cloudlog(lvl, fmt, ## __VA_ARGS__); \
__printed++; \
} else { \
__missed++; \
} \
}
#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#define LOGD_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#endif

15
selfdrive/common/timing.h
View File

@ -5,7 +5,7 @@
#include <time.h>
#ifdef __APPLE__
#define CLOCK_BOOTTIME CLOCK_REALTIME
#define CLOCK_BOOTTIME CLOCK_MONOTONIC
#endif
static inline uint64_t nanos_since_boot() {
@ -38,4 +38,17 @@ static inline double seconds_since_epoch() {
return (double)t.tv_sec + t.tv_nsec * 1e-9;
}
// you probably should use nanos_since_boot instead
static inline uint64_t nanos_monotonic() {
struct timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
static inline uint64_t nanos_monotonic_raw() {
struct timespec t;
clock_gettime(CLOCK_MONOTONIC_RAW, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
#endif

11
selfdrive/common/util.c
View File

@ -3,6 +3,10 @@
#include <string.h>
#include <assert.h>
#ifdef __linux__
#include <sys/prctl.h>
#endif
void* read_file(const char* path, size_t* out_len) {
FILE* f = fopen(path, "r");
if (!f) {
@ -28,3 +32,10 @@ void* read_file(const char* path, size_t* out_len) {
return buf;
}
void set_thread_name(const char* name) {
#ifdef __linux__
// pthread_setname_np is dumb (fails instead of truncates)
prctl(PR_SET_NAME, (unsigned long)name, 0, 0, 0);
#endif
}

14
selfdrive/common/util.h
View File

@ -1,6 +1,9 @@
#ifndef COMMON_UTIL_H
#define COMMON_UTIL_H
#ifndef __cplusplus
#define min(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
@ -11,6 +14,8 @@
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
#endif
#define clamp(a,b,c) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
@ -19,11 +24,20 @@
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
#ifdef __cplusplus
extern "C" {
#endif
// Reads a file into a newly allocated buffer.
//
// Returns NULL on failure, otherwise the NULL-terminated file contents.
// The result must be freed by the caller.
void* read_file(const char* path, size_t* out_len);
void set_thread_name(const char* name);
#ifdef __cplusplus
}
#endif
#endif

4
selfdrive/common/utilpp.h
View File

@ -9,6 +9,10 @@
#include <sstream>
#include <fstream>
#ifdef __x86_64
#include <linux/limits.h>
#endif
namespace util {
inline bool starts_with(std::string s, std::string prefix) {

2
selfdrive/common/version.h
View File

@ -1 +1 @@
#define OPENPILOT_VERSION "0.3.6.1"
#define OPENPILOT_VERSION "0.3.7"

9
selfdrive/common/visionipc.c
View File

@ -1,6 +1,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <unistd.h>
#include <assert.h>
#include <errno.h>
@ -126,7 +127,7 @@ int vipc_send(int fd, const VisionPacket *p2) {
return sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL);
}
void visionbufs_load(VisionBuf *bufs, const VisionStreamBufs *stream_bufs,
void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
int num_fds, const int* fds) {
for (int i=0; i<num_fds; i++) {
if (bufs[i].addr) {
@ -180,10 +181,10 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi
s->bufs_info = rp.d.stream_bufs;
s->num_bufs = rp.num_fds;
s->bufs = calloc(s->num_bufs, sizeof(VisionBuf));
s->bufs = calloc(s->num_bufs, sizeof(VIPCBuf));
assert(s->bufs);
visionbufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
vipc_bufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
if (out_bufs_info) {
*out_bufs_info = s->bufs_info;
@ -192,7 +193,7 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi
return 0;
}
VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra) {
VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra) {
int err;
VisionPacket rp;

16
selfdrive/common/visionipc.h
View File

@ -47,9 +47,9 @@ typedef struct VisionStreamBufs {
} buf_info;
} VisionStreamBufs;
typedef struct VisionBufExtra {
typedef struct VIPCBufExtra {
uint32_t frame_id; // only for yuv
} VisionBufExtra;
} VIPCBufExtra;
typedef union VisionPacketData {
struct {
@ -60,7 +60,7 @@ typedef union VisionPacketData {
struct {
VisionStreamType type;
int idx;
VisionBufExtra extra;
VIPCBufExtra extra;
} stream_acq;
struct {
VisionStreamType type;
@ -79,12 +79,12 @@ int vipc_connect(void);
int vipc_recv(int fd, VisionPacket *out_p);
int vipc_send(int fd, const VisionPacket *p);
typedef struct VisionBuf {
typedef struct VIPCBuf {
int fd;
size_t len;
void* addr;
} VisionBuf;
void visionbufs_load(VisionBuf *bufs, const VisionStreamBufs *stream_bufs,
} VIPCBuf;
void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
int num_fds, const int* fds);
@ -94,11 +94,11 @@ typedef struct VisionStream {
int last_idx;
int num_bufs;
VisionStreamBufs bufs_info;
VisionBuf *bufs;
VIPCBuf *bufs;
} VisionStream;
int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info);
VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra);
VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra);
void visionstream_destroy(VisionStream *s);
#ifdef __cplusplus

782
selfdrive/controls/controlsd.py
View File

@ -2,24 +2,24 @@
import os
import json
from copy import copy
import zmq
from cereal import car, log
from common.numpy_fast import clip
from common.fingerprints import fingerprint
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.profiler import Profiler
from common.params import Params
import selfdrive.messaging as messaging
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.car import get_car
from selfdrive.controls.lib.planner import Planner
from selfdrive.controls.lib.drive_helpers import learn_angle_offset
from selfdrive.controls.lib.longcontrol import LongControl
from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \
get_events, \
create_event, \
EventTypes as ET
from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
from selfdrive.controls.lib.latcontrol import LatControl
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -34,427 +34,472 @@ AWARENESS_TIME = 360. # 6 minutes limit without user touching steering whee
AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before start decelerating the car
AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
# class Cal
class Calibration:
UNCALIBRATED = 0
CALIBRATED = 1
INVALID = 2
# to be used
class State():
class State:
DISABLED = 0
ENABLED = 1
SOFT_DISABLE = 2
PRE_ENABLED = 2
SOFT_DISABLING = 3
class Controls(object):
def __init__(self, gctx, rate=100):
self.rate = rate
# *** log ***
context = zmq.Context()
# True when actuators are controlled
def isActive(state):
return state in [State.ENABLED, State.SOFT_DISABLING]
# pub
self.live100 = messaging.pub_sock(context, service_list['live100'].port)
self.carstate = messaging.pub_sock(context, service_list['carState'].port)
self.carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
# sub
self.thermal = messaging.sub_sock(context, service_list['thermal'].port)
self.health = messaging.sub_sock(context, service_list['health'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
self.cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
self.CC = car.CarControl.new_message()
self.CI, self.CP = get_car(logcan, sendcan)
self.PL = Planner(self.CP)
self.AM = AlertManager()
self.LoC = LongControl()
self.LaC = LatControl()
self.VM = VehicleModel(self.CP)
# write CarParams
params = Params()
params.put("CarParams", self.CP.to_bytes())
# fake plan
self.plan_ts = 0
self.plan = log.Plan.new_message()
self.plan.lateralValid = False
self.plan.longitudinalValid = False
# controls enabled state
self.enabled = False
self.last_enable_request = 0
# learned angle offset
self.angle_offset = 0
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
self.angle_offset = calibration_params["angle_offset"]
except (ValueError, KeyError):
pass
# rear view camera state
self.rear_view_toggle = False
self.rear_view_allowed = (params.get("IsRearViewMirror") == "1")
self.v_cruise_kph = 255
# 0.0 - 1.0
self.awareness_status = 1.0
self.soft_disable_timer = None
self.overtemp = False
self.free_space = 1.0
self.cal_status = Calibration.UNCALIBRATED
self.cal_perc = 0
self.rk = Ratekeeper(self.rate, print_delay_threshold=2./1000)
def data_sample(self):
self.prof = Profiler()
self.cur_time = sec_since_boot()
# first read can and compute car states
self.CS = self.CI.update()
self.prof.checkpoint("CarInterface")
# True if system is engaged
def isEnabled(state):
return (isActive(state) or state == State.PRE_ENABLED)
# *** thermal checking logic ***
# thermal data, checked every second
td = messaging.recv_sock(self.thermal)
if td is not None:
# Check temperature.
self.overtemp = any(
t > 950
for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
# under 15% of space free
self.free_space = td.thermal.freeSpace
# read calibration status
cal = messaging.recv_sock(self.cal)
if cal is not None:
self.cal_status = cal.liveCalibration.calStatus
self.cal_perc = cal.liveCalibration.calPerc
def data_sample(CI, CC, thermal, health, cal, cal_status, cal_perc):
def state_transition(self):
pass # for now
# *** read can and compute car states ***
CS = CI.update(CC)
events = list(CS.events)
def state_control(self):
# did it request to enable?
enable_request, enable_condition = False, False
# *** thermal checking logic ***
# thermal data, checked every second
td = messaging.recv_sock(thermal)
if td is not None:
# Check temperature
overtemp = any(
t > 950
for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
# reset awareness status on steering
if self.CS.steeringPressed or not self.enabled:
self.awareness_status = 1.0
elif self.enabled:
# gives the user 6 minutes
self.awareness_status -= 1.0/(self.rate * AWARENESS_TIME)
if self.awareness_status <= 0.:
self.AM.add("driverDistracted", self.enabled)
elif self.awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
self.awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
self.AM.add("preDriverDistracted", self.enabled)
# under 15% of space free
if td.thermal.freeSpace < 0.15:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# handle button presses
for b in self.CS.buttonEvents:
print b
# *** read calibration status ***
cal = messaging.recv_sock(cal)
if cal is not None:
cal_status = cal.liveCalibration.calStatus
cal_perc = cal.liveCalibration.calPerc
# button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed and self.rear_view_allowed:
self.rear_view_toggle = True
else:
self.rear_view_toggle = False
if b.type == "altButton1" and b.pressed:
self.rear_view_toggle = not self.rear_view_toggle
if not self.CP.enableCruise and self.enabled and not b.pressed:
if b.type == "accelCruise":
self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
elif b.type == "decelCruise":
self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
self.v_cruise_kph = clip(self.v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
if not self.enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed:
enable_request = True
# do disable on button down
if b.type == "cancel" and b.pressed:
self.AM.add("disable", self.enabled)
self.prof.checkpoint("Buttons")
# *** health checking logic ***
hh = messaging.recv_sock(self.health)
if hh is not None:
# if the board isn't allowing controls but somehow we are enabled!
# TODO: this should be in state transition with a function follower logic
if not hh.health.controlsAllowed and self.enabled:
self.AM.add("controlsMismatch", self.enabled)
# disable if the pedals are pressed while engaged, this is a user disable
if self.enabled:
if self.CS.gasPressed or self.CS.brakePressed or not self.CS.cruiseState.available:
self.AM.add("disable", self.enabled)
# it can happen that car cruise disables while comma system is enabled: need to
# keep braking if needed or if the speed is very low
# TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
if self.CP.enableCruise and not self.CS.cruiseState.enabled and \
(self.CC.brake <= 0. or self.CS.vEgo < 0.3):
self.AM.add("cruiseDisabled", self.enabled)
if enable_request:
# check for pressed pedals
if self.CS.gasPressed or self.CS.brakePressed:
self.AM.add("pedalPressed", self.enabled)
enable_request = False
else:
print "enabled pressed at", self.cur_time
self.last_enable_request = self.cur_time
# don't engage with less than 15% free
if self.free_space < 0.15:
self.AM.add("outOfSpace", self.enabled)
enable_request = False
if self.CP.enableCruise:
enable_condition = ((self.cur_time - self.last_enable_request) < 0.2) and self.CS.cruiseState.enabled
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(create_event('calibrationInProgress', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
else:
enable_condition = enable_request
events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CP.enableCruise and self.CS.cruiseState.enabled:
self.v_cruise_kph = self.CS.cruiseState.speed * CV.MS_TO_KPH
# *** health checking logic ***
hh = messaging.recv_sock(health)
if hh is not None:
controls_allowed = hh.health.controlsAllowed
if not controls_allowed:
events.append(create_event('controlsMismatch', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
self.prof.checkpoint("AdaptiveCruise")
return CS, events, cal_status, cal_perc
# *** what's the plan ***
plan_packet = self.PL.update(self.CS, self.LoC)
self.plan = plan_packet.plan
self.plan_ts = plan_packet.logMonoTime
# if user is not responsive to awareness alerts, then start a smooth deceleration
if self.awareness_status < -0.:
self.plan.aTargetMax = min(self.plan.aTargetMax, AWARENESS_DECEL)
self.plan.aTargetMin = min(self.plan.aTargetMin, self.plan.aTargetMax)
def calc_plan(CS, events, PL, LoC):
# plan runs always, independently of the state
plan_packet = PL.update(CS, LoC)
plan = plan_packet.plan
plan_ts = plan_packet.logMonoTime
if enable_request or enable_condition or self.enabled:
# add all alerts from car
for alert in self.CS.errors:
self.AM.add(alert, self.enabled)
# add events from planner
events += list(plan.events)
if not self.plan.longitudinalValid:
self.AM.add("radarCommIssue", self.enabled)
# disable if lead isn't close when system is active and brake is pressed to avoid
# unexpected vehicle accelerations
if CS.brakePressed and plan.vTarget >= STARTING_TARGET_SPEED:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.cal_status != Calibration.CALIBRATED:
if self.cal_status == Calibration.UNCALIBRATED:
self.AM.add("calibrationInProgress", self.enabled, str(self.cal_perc) + '%')
else:
self.AM.add("calibrationInvalid", self.enabled)
return plan, plan_ts
if not self.plan.lateralValid:
# If the model is not broadcasting, assume that it is because
# the user has uploaded insufficient data for calibration.
# Other cases that would trigger this are rare and unactionable by the user.
self.AM.add("dataNeeded", self.enabled)
if self.overtemp:
self.AM.add("overheat", self.enabled)
def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, cal_perc, AM):
# compute conditional state transitions and execute actions on state transitions
enabled = isEnabled(state)
# *** angle offset learning ***
if self.rk.frame % 5 == 2 and self.plan.lateralValid:
# *** run this at 20hz again ***
self.angle_offset = learn_angle_offset(self.enabled, self.CS.vEgo, self.angle_offset,
self.PL.PP.c_poly, self.PL.PP.c_prob, self.LaC.y_des,
self.CS.steeringPressed)
# handle button presses. TODO: this should be in state_control, but a decelCruise press
# would have the effect of both enabling and changing speed is checked after the state transition
for b in CS.buttonEvents:
if not CP.enableCruise and enabled and not b.pressed:
if b.type == "accelCruise":
v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
elif b.type == "decelCruise":
v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
# *** gas/brake PID loop ***
final_gas, final_brake = self.LoC.update(self.enabled, self.CS.vEgo, self.v_cruise_kph,
self.plan.vTarget,
[self.plan.aTargetMin, self.plan.aTargetMax],
self.plan.jerkFactor, self.CP)
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
soft_disable_timer = max(0, soft_disable_timer - 1)
# *** steering PID loop ***
final_steer, sat_flag = self.LaC.update(self.enabled, self.CS.vEgo, self.CS.steeringAngle,
self.CS.steeringPressed, self.plan.dPoly, self.angle_offset, self.VM)
self.prof.checkpoint("PID")
# ***** handle alerts ****
# send FCW alert if triggered by planner
if self.plan.fcw:
self.AM.add("fcw", self.enabled)
# ***** handle state transitions *****
# send a "steering required alert" if saturation count has reached the limit
if sat_flag:
self.AM.add("steerSaturated", self.enabled)
if self.enabled and self.AM.alertShouldDisable():
print "DISABLING IMMEDIATELY ON ALERT"
self.enabled = False
if self.enabled and self.AM.alertShouldSoftDisable():
if self.soft_disable_timer is None:
self.soft_disable_timer = 3 * self.rate
elif self.soft_disable_timer == 0:
print "SOFT DISABLING ON ALERT"
self.enabled = False
# DISABLED
if state == State.DISABLED:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
for e in get_events(events, [ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
for e in get_events(events, [ET.NO_ENTRY]):
txt = str(cal_perc) + '%' if e == 'calibrationInProgress' else ''
AM.add(str(e) + "NoEntry", enabled, txt)
else:
self.soft_disable_timer -= 1
else:
self.soft_disable_timer = None
if get_events(events, [ET.PRE_ENABLE]):
state = State.PRE_ENABLED
else:
state = State.ENABLED
AM.add("enable", enabled)
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active
v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
if enable_condition and not self.enabled and not self.AM.alertPresent():
print "*** enabling controls"
# ENABLED
elif state == State.ENABLED:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
AM.add("disable", enabled)
# beep for enabling
self.AM.add("enable", self.enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.DISABLED
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
# enable both lateral and longitudinal controls
self.enabled = True
elif get_events(events, [ET.SOFT_DISABLE]):
state = State.SOFT_DISABLING
soft_disable_timer = 300 # 3s TODO: use rate
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled)
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active
self.v_cruise_kph = int(round(max(self.CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
# SOFT DISABLING
elif state == State.SOFT_DISABLING:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
AM.add("disable", enabled)
# 6 minutes driver you're on
self.awareness_status = 1.0
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.DISABLED
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
# reset the PID loops
self.LaC.reset()
# start long control at actual speed
self.LoC.reset(v_pid = self.CS.vEgo)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
state = State.ENABLED
# *** push the alerts to current ***
# TODO: remove output, store them inside AM class instead
self.alert_text_1, self.alert_text_2, self.visual_alert, self.audible_alert = self.AM.process_alerts(self.cur_time)
# ***** control the car *****
self.CC.enabled = self.enabled
elif soft_disable_timer <= 0:
state = State.DISABLED
# TODO: PRE ENABLING
elif state == State.PRE_ENABLED:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
AM.add("disable", enabled)
self.CC.gas = float(final_gas)
self.CC.brake = float(final_brake)
self.CC.steeringTorque = float(final_steer)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.DISABLED
for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(e, enabled)
self.CC.cruiseControl.override = True
# always cancel if we have an interceptor
self.CC.cruiseControl.cancel = bool(not self.CP.enableCruise or
(not self.enabled and self.CS.cruiseState.enabled))
elif not get_events(events, [ET.PRE_ENABLE]):
state = State.ENABLED
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(final_brake*3., 0.0, 1.0))
self.CC.cruiseControl.speedOverride = float(max(0.0, ((self.LoC.v_pid - .5) * brake_discount)) if self.CP.enableCruise else 0.0)
return state, soft_disable_timer, v_cruise_kph
#CC.cruiseControl.accelOverride = float(AC.a_pcm)
# TODO: parametrize 0.714 in interface?
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
# unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
self.CC.cruiseControl.accelOverride = float(max(0.714, self.plan.aTargetMax/A_ACC_MAX))
self.CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
self.CC.hudControl.speedVisible = self.enabled
self.CC.hudControl.lanesVisible = self.enabled
self.CC.hudControl.leadVisible = self.plan.hasLead
self.CC.hudControl.visualAlert = self.visual_alert
self.CC.hudControl.audibleAlert = self.audible_alert
def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status,
PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle):
# Given the state, this function returns the actuators
# TODO: remove it from here and put it in state_transition
# this alert will apply next controls cycle
if not self.CI.apply(self.CC):
self.AM.add("controlsFailed", self.enabled)
# reset actuators to zero
actuators = car.CarControl.Actuators.new_message()
def data_send(self):
# broadcast carControl first
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = copy(self.CC)
self.carcontrol.send(cc_send.to_bytes())
enabled = isEnabled(state)
active = isActive(state)
self.prof.checkpoint("CarControl")
for b in CS.buttonEvents:
# any button event resets awarenesss_status
awareness_status = 1.
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.carState = copy(self.CS)
self.carstate.send(cs_send.to_bytes())
# ***** publish state to logger *****
# button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed and rear_view_allowed:
rear_view_toggle = True
else:
rear_view_toggle = False
# publish controls state at 100Hz
if b.type == "altButton1" and b.pressed:
rear_view_toggle = not rear_view_toggle
# send FCW alert if triggered by planner
if plan.fcw:
AM.add("fcw", enabled)
# ***** state specific actions *****
# DISABLED
if state in [State.PRE_ENABLED, State.DISABLED]:
awareness_status = 1.
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
# ENABLED or SOFT_DISABLING
elif state in [State.ENABLED, State.SOFT_DISABLING]:
if CS.steeringPressed:
# reset awareness status on steering
awareness_status = 1.0
# 6 minutes driver you're on
awareness_status -= 0.01/(AWARENESS_TIME)
if awareness_status <= 0.:
AM.add("driverDistracted", enabled)
elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
AM.add("preDriverDistracted", enabled)
# parse warnings from car specific interface
for e in get_events(events, [ET.WARNING]):
AM.add(e, enabled)
# if user is not responsive to awareness alerts, then start a smooth deceleration
if awareness_status < -0.:
plan.aTargetMax = min(plan.aTargetMax, AWARENESS_DECEL)
plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax)
# *** angle offset learning ***
if rk.frame % 5 == 2 and plan.lateralValid:
# *** run this at 20hz again ***
angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
PL.PP.c_poly, PL.PP.c_prob, LaC.y_des,
CS.steeringPressed)
# *** gas/brake PID loop ***
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed,
v_cruise_kph, plan.vTarget,
[plan.aTargetMin, plan.aTargetMax],
plan.jerkFactor, CP)
# *** steering PID loop ***
actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle,
CS.steeringPressed, plan.dPoly, angle_offset, VM, PL)
# send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag:
AM.add("steerSaturated", enabled)
if CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# *** process alerts ***
AM.process_alerts(sec_since_boot())
return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle
def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status,
LaC, LoC, angle_offset):
# ***** control the car *****
CC = car.CarControl.new_message()
CC.enabled = isEnabled(state)
CC.actuators = actuators
CC.cruiseControl.override = True
# always cancel if we have an interceptor
CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
# TODO: parametrize 0.714 in interface?
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
# unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX))
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state)
CC.hudControl.leadVisible = plan.hasLead
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert
# send car controls over can
CI.apply(CC)
# ***** publish state to logger *****
# publish controls state at 100Hz
dat = messaging.new_message()
dat.init('live100')
# show rear view camera on phone if in reverse gear or when button is pressed
dat.live100.rearViewCam = ('reverseGear' in [e.name for e in events] and rear_view_allowed) or rear_view_toggle
dat.live100.alertText1 = AM.alert_text_1
dat.live100.alertText2 = AM.alert_text_2
dat.live100.awarenessStatus = max(awareness_status, 0.0) if isEnabled(state) else 0.0
# what packets were used to process
dat.live100.canMonoTimes = list(CS.canMonoTimes)
dat.live100.planMonoTime = plan_ts
# if controls is enabled
dat.live100.enabled = isEnabled(state)
# car state
dat.live100.vEgo = CS.vEgo
dat.live100.angleSteers = CS.steeringAngle
dat.live100.steerOverride = CS.steeringPressed
# longitudinal control state
dat.live100.vPid = float(LoC.v_pid)
dat.live100.vCruise = float(v_cruise_kph)
dat.live100.upAccelCmd = float(LoC.pid.p)
dat.live100.uiAccelCmd = float(LoC.pid.i)
# lateral control state
dat.live100.yDes = float(LaC.y_des)
dat.live100.angleSteersDes = float(LaC.angle_steers_des)
dat.live100.upSteer = float(LaC.pid.p)
dat.live100.uiSteer = float(LaC.pid.i)
# processed radar state, should add a_pcm?
dat.live100.vTargetLead = float(plan.vTarget)
dat.live100.aTargetMin = float(plan.aTargetMin)
dat.live100.aTargetMax = float(plan.aTargetMax)
dat.live100.jerkFactor = float(plan.jerkFactor)
# log learned angle offset
dat.live100.angleOffset = float(angle_offset)
# lag
dat.live100.cumLagMs = -rk.remaining*1000.
live100.send(dat.to_bytes())
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
# TODO: override CS.events with all the cumulated events
cs_send.carState = copy(CS)
carstate.send(cs_send.to_bytes())
# broadcast carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = copy(CC)
carcontrol.send(cc_send.to_bytes())
#print [i.name for i in events]
# publish mpc state at 20Hz
if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated:
dat = messaging.new_message()
dat.init('live100')
dat.init('liveMpc')
dat.liveMpc.x = list(LaC.mpc_solution[0].x)
dat.liveMpc.y = list(LaC.mpc_solution[0].y)
dat.liveMpc.psi = list(LaC.mpc_solution[0].psi)
dat.liveMpc.delta = list(LaC.mpc_solution[0].delta)
livempc.send(dat.to_bytes())
# show rear view camera on phone if in reverse gear or when button is pressed
dat.live100.rearViewCam = ('reverseGear' in self.CS.errors and self.rear_view_allowed) or self.rear_view_toggle
dat.live100.alertText1 = self.alert_text_1
dat.live100.alertText2 = self.alert_text_2
dat.live100.awarenessStatus = max(self.awareness_status, 0.0) if self.enabled else 0.0
# what packets were used to process
dat.live100.canMonoTimes = list(self.CS.canMonoTimes)
dat.live100.planMonoTime = self.plan_ts
# if controls is enabled
dat.live100.enabled = self.enabled
# car state
dat.live100.vEgo = self.CS.vEgo
dat.live100.angleSteers = self.CS.steeringAngle
dat.live100.steerOverride = self.CS.steeringPressed
# longitudinal control state
dat.live100.vPid = float(self.LoC.v_pid)
dat.live100.vCruise = float(self.v_cruise_kph)
dat.live100.upAccelCmd = float(self.LoC.Up_accel_cmd)
dat.live100.uiAccelCmd = float(self.LoC.Ui_accel_cmd)
# lateral control state
dat.live100.yDes = float(self.LaC.y_des)
dat.live100.angleSteersDes = float(self.LaC.angle_steers_des)
dat.live100.upSteer = float(self.LaC.Up_steer)
dat.live100.uiSteer = float(self.LaC.Ui_steer)
# processed radar state, should add a_pcm?
dat.live100.vTargetLead = float(self.plan.vTarget)
dat.live100.aTargetMin = float(self.plan.aTargetMin)
dat.live100.aTargetMax = float(self.plan.aTargetMax)
dat.live100.jerkFactor = float(self.plan.jerkFactor)
# log learned angle offset
dat.live100.angleOffset = float(self.angle_offset)
# lag
dat.live100.cumLagMs = -self.rk.remaining*1000.
self.live100.send(dat.to_bytes())
self.prof.checkpoint("Live100")
def wait(self):
# *** run loop at fixed rate ***
if self.rk.keep_time():
self.prof.display()
return CC
def controlsd_thread(gctx, rate=100):
# start the loop
set_realtime_priority(2)
CTRLS = Controls(gctx, rate)
context = zmq.Context()
# pub
live100 = messaging.pub_sock(context, service_list['live100'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
# sub
thermal = messaging.sub_sock(context, service_list['thermal'].port)
health = messaging.sub_sock(context, service_list['health'].port)
cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CI, CP = get_car(logcan, sendcan)
PL = Planner(CP)
LoC = LongControl(CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(VM)
AM = AlertManager()
# write CarParams
params = Params()
params.put("CarParams", CP.to_bytes())
state = State.DISABLED
soft_disable_timer = 0
v_cruise_kph = 255
cal_perc = 0
cal_status = Calibration.UNCALIBRATED
rear_view_toggle = False
rear_view_allowed = params.get("IsRearViewMirror") == "1"
# 0.0 - 1.0
awareness_status = 0.
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
# learned angle offset
angle_offset = 0.
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
angle_offset = calibration_params["angle_offset"]
except (ValueError, KeyError):
pass
prof = Profiler()
while 1:
CTRLS.data_sample()
CTRLS.state_transition()
CTRLS.state_control()
CTRLS.data_send()
CTRLS.wait()
prof.reset() # avoid memory leak
# sample data and compute car events
CS, events, cal_status, cal_perc = data_sample(CI, CC, thermal, health, cal, cal_status, cal_perc)
prof.checkpoint("Sample")
# define plan
plan, plan_ts = calc_plan(CS, events, PL, LoC)
prof.checkpoint("Plan")
# update control state
state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, cal_perc, AM)
prof.checkpoint("State transition")
# compute actuators
actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph,
AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset,
rear_view_allowed, rear_view_toggle)
prof.checkpoint("State Control")
# publish data
CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
rear_view_toggle, awareness_status, LaC, LoC, angle_offset)
prof.checkpoint("Sent")
# *** run loop at fixed rate ***
if rk.keep_time():
prof.display()
def main(gctx=None):
@ -462,4 +507,3 @@ def main(gctx=None):
if __name__ == "__main__":
main()

12
selfdrive/controls/lib/adaptivecruise.py
View File

@ -109,7 +109,7 @@ def calc_critical_decel(d_lead, v_rel, d_offset, v_offset):
# maximum acceleration adjustment
_A_CORR_BY_SPEED_V = [0.4, 0.4, 0]
# speeds
_A_CORR_BY_SPEED_BP = [0., 5., 20.]
_A_CORR_BY_SPEED_BP = [0., 2., 10.]
# max acceleration allowed in acc, which happens in restart
A_ACC_MAX = max(_A_CORR_BY_SPEED_V) + max(_A_CRUISE_MAX_V)
@ -283,20 +283,12 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP):
class AdaptiveCruise(object):
def __init__(self):
self.last_cal = 0.
self.l1, self.l2 = None, None
self.dead = True
def update(self, cur_time, v_ego, angle_steers, v_pid, CP, l20):
def update(self, v_ego, angle_steers, v_pid, CP, l20):
if l20 is not None:
self.l1 = l20.live20.leadOne
self.l2 = l20.live20.leadTwo
# TODO: no longer has anything to do with calibration
self.last_cal = cur_time
self.dead = False
elif cur_time - self.last_cal > 0.5:
self.dead = True
self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \
compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP)
self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE

392
selfdrive/controls/lib/alertmanager.py
View File

@ -2,22 +2,31 @@ from cereal import car
from selfdrive.swaglog import cloudlog
import copy
class ET:
ENABLE = 0
NO_ENTRY = 1
WARNING = 2
USER_DISABLE = 3
SOFT_DISABLE = 4
IMMEDIATE_DISABLE = 5
class alert(object):
def __init__(self, alert_text_1, alert_text_2, alert_type, visual_alert, audible_alert, duration_sound, duration_hud_alert, duration_text):
# Priority
class PT:
HIGH = 3
MID = 2
LOW = 1
class Alert(object):
def __init__(self,
alert_text_1,
alert_text_2,
alert_priority,
visual_alert,
audible_alert,
duration_sound,
duration_hud_alert,
duration_text):
self.alert_text_1 = alert_text_1
self.alert_text_2 = alert_text_2
self.alert_type = alert_type
self.alert_priority = alert_priority
self.visual_alert = visual_alert if visual_alert is not None else "none"
self.audible_alert = audible_alert if audible_alert is not None else "none"
self.duration_sound = duration_sound
self.duration_hud_alert = duration_hud_alert
self.duration_text = duration_text
@ -28,45 +37,297 @@ class alert(object):
tst.hudControl.audibleAlert = self.audible_alert
def __str__(self):
return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_type) + " " + str(self.visual_alert) + " " + str(self.audible_alert)
return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_priority) + " " + str(
self.visual_alert) + " " + str(self.audible_alert)
def __gt__(self, alert2):
return self.alert_type > alert2.alert_type
return self.alert_priority > alert2.alert_priority
class AlertManager(object):
alerts = {
"enable": alert("", "", ET.ENABLE, None, "beepSingle", .2, 0., 0.),
"disable": alert("", "", ET.USER_DISABLE, None, "beepSingle", .2, 0., 0.),
"pedalPressed": alert("Comma Unavailable", "Pedal Pressed", ET.NO_ENTRY, "brakePressed", "chimeDouble", .4, 2., 3.),
"preDriverDistracted": alert("Take Control ", "User Distracted", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.),
"driverDistracted": alert("Take Control to Regain Speed", "User Distracted", ET.WARNING, "steerRequired", "chimeRepeated", .5, .5, .5),
"steerSaturated": alert("Take Control", "Turn Exceeds Limit", ET.WARNING, "steerRequired", "chimeSingle", 1., 2., 3.),
"overheat": alert("Take Control Immediately", "System Overheated", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsMismatch": alert("Take Control Immediately", "Controls Mismatch", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"radarCommIssue": alert("Take Control Immediately", "Radar Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInvalid": alert("Take Control Immediately", "Calibration Invalid: Reposition Neo and Recalibrate", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInProgress": alert("Take Control Immediately", "Calibration in Progress: ", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"modelCommIssue": alert("Take Control Immediately", "Model Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsFailed": alert("Take Control Immediately", "Controls Failed", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"fcw": alert("", "", ET.WARNING, None, None, .1, .1, .1),
# car errors
"commIssue": alert("Take Control Immediately","CAN Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerUnavailable": alert("Take Control Immediately","Steer Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerTempUnavailable": alert("Take Control", "Steer Temporarily Unavailable", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.),
"brakeUnavailable": alert("Take Control Immediately","Brake Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"gasUnavailable": alert("Take Control Immediately","Gas Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"wrongGear": alert("Take Control Immediately","Gear not D", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"reverseGear": alert("Take Control Immediately","Car in Reverse", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"doorOpen": alert("Take Control Immediately","Door Open", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"seatbeltNotLatched": alert("Take Control Immediately","Seatbelt Unlatched", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"espDisabled": alert("Take Control Immediately","ESP Off", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"cruiseDisabled": alert("Take Control Immediately","Cruise Is Off", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"wrongCarMode": alert("Comma Unavailable","Main Switch Off", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
"outOfSpace": alert("Comma Unavailable","Out of Space", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
"dataNeeded": alert("Comma Unavailable","Data needed for calibration. Upload drive, try again", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
"ethicalDilemma": alert("Take Control Immediately","Ethical Dilemma Detected", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"startup": alert("Always Keep Hands on Wheel","Be Ready to Take Over Any Time", ET.NO_ENTRY, None, None, 0., 0., 15.),
# Miscellaneous alerts
"enable": Alert(
"",
"",
PT.MID, None, "beepSingle", .2, 0., 0.),
"disable": Alert(
"",
"",
PT.MID, None, "beepSingle", .2, 0., 0.),
"fcw": Alert(
"",
"",
PT.LOW, None, None, .1, .1, .1),
"steerSaturated": Alert(
"Take Control",
"Turn Exceeds Limit",
PT.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
"steerTempUnavailable": Alert(
"Take Control",
"Steer Temporarily Unavailable",
PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
"preDriverDistracted": Alert(
"Take Control ",
"User Distracted",
PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
"driverDistracted": Alert(
"Take Control to Regain Speed",
"User Distracted",
PT.LOW, "steerRequired", "chimeRepeated", .5, .5, .5),
"startup": Alert(
"Always Keep Hands on Wheel",
"Be Ready to Take Over Any Time",
PT.LOW, None, None, 0., 0., 15.),
"ethicalDilemma": Alert(
"Take Control Immediately",
"Ethical Dilemma Detected",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerTempUnavailableNoEntry": Alert(
"Comma Unavailable",
"Steer Temporary Unavailable",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
# Non-entry only alerts
"wrongCarModeNoEntry": Alert(
"Comma Unavailable",
"Main Switch Off",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
"dataNeededNoEntry": Alert(
"Comma Unavailable",
"Data needed for calibration. Upload drive, try again",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
"outOfSpaceNoEntry": Alert(
"Comma Unavailable",
"Out of Space",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
"pedalPressedNoEntry": Alert(
"Comma Unavailable",
"Pedal Pressed",
PT.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
"speedTooLowNoEntry": Alert(
"Comma Unavailable",
"Speed Too Low",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"brakeHoldNoEntry": Alert(
"Comma Unavailable",
"Brake Hold Active",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"parkBrakeNoEntry": Alert(
"Comma Unavailable",
"Park Brake Engaged",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
# Cancellation alerts causing disabling
"overheat": Alert(
"Take Control Immediately",
"System Overheated",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"wrongGear": Alert(
"Take Control Immediately",
"Gear not D",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInvalid": Alert(
"Take Control Immediately",
"Calibration Invalid: Reposition Neo and Recalibrate",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInProgress": Alert(
"Take Control Immediately",
"Calibration in Progress",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"doorOpen": Alert(
"Take Control Immediately",
"Door Open",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"seatbeltNotLatched": Alert(
"Take Control Immediately",
"Seatbelt Unlatched",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"espDisabled": Alert(
"Take Control Immediately",
"ESP Off",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"radarCommIssue": Alert(
"Take Control Immediately",
"Radar Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"radarFault": Alert(
"Take Control Immediately",
"Radar Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"modelCommIssue": Alert(
"Take Control Immediately",
"Model Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsFailed": Alert(
"Take Control Immediately",
"Controls Failed",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsMismatch": Alert(
"Take Control Immediately",
"Controls Mismatch",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"commIssue": Alert(
"Take Control Immediately",
"CAN Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerUnavailable": Alert(
"Take Control Immediately",
"Steer Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"brakeUnavailable": Alert(
"Take Control Immediately",
"Brake Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"gasUnavailable": Alert(
"Take Control Immediately",
"Gas Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"reverseGear": Alert(
"Take Control Immediately",
"Reverse Gear",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"cruiseDisabled": Alert(
"Take Control Immediately",
"Cruise Is Off",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
# not loud cancellations (user is in control)
"noTarget": Alert(
"Comma Canceled",
"No Close Lead",
PT.HIGH, None, "chimeDouble", .4, 2., 3.),
# Cancellation alerts causing non-entry
"overheatNoEntry": Alert(
"Comma Unavailable",
"System Overheated",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"wrongGearNoEntry": Alert(
"Comma Unavailable",
"Gear not D",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"calibrationInvalidNoEntry": Alert(
"Comma Unavailable",
"Calibration Invalid: Reposition Neo and Recalibrate",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"calibrationInProgressNoEntry": Alert(
"Comma Unavailable",
"Calibration in Progress: ",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"doorOpenNoEntry": Alert(
"Comma Unavailable",
"Door Open",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"seatbeltNotLatchedNoEntry": Alert(
"Comma Unavailable",
"Seatbelt Unlatched",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"espDisabledNoEntry": Alert(
"Comma Unavailable",
"ESP Off",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"radarCommIssueNoEntry": Alert(
"Comma Unavailable",
"Radar Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"radarFaultNoEntry": Alert(
"Comma Unavailable",
"Radar Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"modelCommIssueNoEntry": Alert(
"Comma Unavailable",
"Model Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"controlsFailedNoEntry": Alert(
"Comma Unavailable",
"Controls Failed",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"controlsMismatchNoEntry": Alert(
"Comma Unavailable",
"Controls Mismatch",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"commIssueNoEntry": Alert(
"Comma Unavailable",
"CAN Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"steerUnavailableNoEntry": Alert(
"Comma Unavailable",
"Steer Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"brakeUnavailableNoEntry": Alert(
"Comma Unavailable",
"Brake Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"gasUnavailableNoEntry": Alert(
"Comma Unavailable",
"Gas Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"reverseGearNoEntry": Alert(
"Comma Unavailable",
"Reverse Gear",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"cruiseDisabledNoEntry": Alert(
"Comma Unavailable",
"Cruise is Off",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"noTargetNoEntry": Alert(
"Comma Unavailable",
"No Close Lead",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
}
def __init__(self):
self.activealerts = []
self.current_alert = None
@ -75,31 +336,16 @@ class AlertManager(object):
def alertPresent(self):
return len(self.activealerts) > 0
def alertShouldSoftDisable(self):
return len(self.activealerts) > 0 and any(a.alert_type == ET.SOFT_DISABLE for a in self.activealerts)
def alertShouldDisable(self):
return len(self.activealerts) > 0 and any(a.alert_type in [ET.IMMEDIATE_DISABLE, ET.USER_DISABLE] for a in self.activealerts)
def add(self, alert_type, enabled=True, extra_text=''):
alert_type = str(alert_type)
this_alert = copy.copy(self.alerts[alert_type])
this_alert.alert_text_2 += extra_text
# downgrade the alert if we aren't enabled, except if it's FCW, which remains the same
# TODO: remove this 'if' by adding more alerts
if not enabled and this_alert.alert_type in [ET.WARNING, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE] \
and this_alert != self.alerts['fcw']:
this_alert = alert("Comma Unavailable" if this_alert.alert_text_1 != "" else "", this_alert.alert_text_2, ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.)
# ignore no entries if we are enabled
if enabled and this_alert.alert_type in [ET.ENABLE, ET.NO_ENTRY]:
return
# if new alert is higher priority, log it
if self.current_alert is None or this_alert > self.current_alert:
cloudlog.event('alert_add',
alert_type=alert_type,
enabled=enabled)
alert_type=alert_type,
enabled=enabled)
self.activealerts.append(this_alert)
self.activealerts.sort()
@ -109,29 +355,29 @@ class AlertManager(object):
self.alert_start_time = cur_time
self.current_alert = self.activealerts[0]
print self.current_alert
alert_text_1 = ""
alert_text_2 = ""
visual_alert = "none"
audible_alert = "none"
# start with assuming no alerts
self.alert_text_1 = ""
self.alert_text_2 = ""
self.visual_alert = "none"
self.audible_alert = "none"
if self.current_alert is not None:
# ewwwww
if self.alert_start_time + self.current_alert.duration_sound > cur_time:
audible_alert = self.current_alert.audible_alert
self.audible_alert = self.current_alert.audible_alert
if self.alert_start_time + self.current_alert.duration_hud_alert > cur_time:
visual_alert = self.current_alert.visual_alert
self.visual_alert = self.current_alert.visual_alert
if self.alert_start_time + self.current_alert.duration_text > cur_time:
alert_text_1 = self.current_alert.alert_text_1
alert_text_2 = self.current_alert.alert_text_2
self.alert_text_1 = self.current_alert.alert_text_1
self.alert_text_2 = self.current_alert.alert_text_2
# disable current alert
if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert, self.current_alert.duration_text) < cur_time:
if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert,
self.current_alert.duration_text) < cur_time:
self.current_alert = None
# reset
self.activealerts = []
return alert_text_1, alert_text_2, visual_alert, audible_alert

30
selfdrive/controls/lib/drive_helpers.py
View File

@ -1,8 +1,38 @@
from common.numpy_fast import clip
from cereal import car
class EventTypes:
ENABLE = 'enable'
PRE_ENABLE = 'preEnable'
NO_ENTRY = 'noEntry'
WARNING = 'warning'
USER_DISABLE = 'userDisable'
SOFT_DISABLE = 'softDisable'
IMMEDIATE_DISABLE = 'immediateDisable'
def create_event(name, types):
event = car.CarEvent.new_message()
event.name = name
for t in types:
setattr(event, t, True)
return event
def get_events(events, types):
out = []
for e in events:
for t in types:
if getattr(e, t):
out.append(e.name)
return out
def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_des, steer_override):
# simple integral controller that learns how much steering offset to put to have the car going straight
# while being in the middle of the lane

173
selfdrive/controls/lib/latcontrol.py
View File

@ -1,133 +1,80 @@
import math
import numpy as np
from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from common.numpy_fast import clip, interp
from selfdrive.config import Conversions as CV
_K_CURV_V = [1., 0.6]
_K_CURV_BP = [0., 0.002]
def calc_d_lookahead(v_ego, d_poly):
#*** this function computes how far too look for lateral control
# howfar we look ahead is function of speed and how much curvy is the path
offset_lookahead = 1.
k_lookahead = 7.
# integrate abs value of second derivative of poly to get a measure of path curvature
pts_len = 50. # m
if len(d_poly)>0:
pts = np.polyval([6*d_poly[0], 2*d_poly[1]], np.arange(0, pts_len))
else:
pts = 0.
curv = np.sum(np.abs(pts))/pts_len
# 100ms is a rule of thumb estimation of lag from image processing to actuator command
ACTUATORS_DELAY = 0.1
k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
# sqrt on speed is needed to keep, for a given curvature, the y_des
# proportional to speed. Indeed, y_des is prop to d_lookahead^2
# 36m at 25m/s
d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
return d_lookahead
def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio):
states[0].x = v_ego * ACTUATORS_DELAY
states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * ACTUATORS_DELAY
return states
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
#*** this function returns the lateral offset given the steering angle, speed and the lookahead distance
sa = (angle_steers - angle_offset) * CV.DEG_TO_RAD
curvature = VM.calc_curvature(sa, v_ego)
# clip is to avoid arcsin NaNs due to too sharp turns
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999))/2.)
return y_actual, curvature
def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
# inverse of the above function
curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
steer_des = VM.get_steer_from_curvature(curvature, v_ego) * CV.RAD_TO_DEG + angle_offset
return steer_des, curvature
def get_steer_max(CP, v_ego):
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
def pid_lateral_control(v_ego, sa_actual, sa_des, Ui_steer, steer_max,
steer_override, sat_count, enabled, Kp, Ki, rate):
sat_count_rate = 1./rate
sat_count_limit = 0.8 # after 0.8s of continuous saturation, an alert will be sent
error_steer = sa_des - sa_actual
Ui_unwind_speed = 0.3/rate #.3 per second
Up_steer = error_steer*Kp
Ui_steer_new = Ui_steer + error_steer*Ki * 1./rate
output_steer_new = Ui_steer_new + Up_steer
# Anti-wind up for integrator: do not integrate if we are against the steer limits
if (
(error_steer >= 0. and (output_steer_new < steer_max or Ui_steer < 0)) or
(error_steer <= 0. and
(output_steer_new > -steer_max or Ui_steer > 0))) and not steer_override:
#update integrator
Ui_steer = Ui_steer_new
# unwind integrator if driver is maneuvering the steering wheel
elif steer_override:
Ui_steer -= Ui_unwind_speed * np.sign(Ui_steer)
# still, intergral term should not be bigger then limits
Ui_steer = clip(Ui_steer, -steer_max, steer_max)
output_steer = Up_steer + Ui_steer
# don't run steer control if at very low speed
if v_ego < 0.3 or not enabled:
output_steer = 0.
Ui_steer = 0.
# useful to know if control is against the limit
lateral_control_sat = False
if abs(output_steer) > steer_max:
lateral_control_sat = True
output_steer = clip(output_steer, -steer_max, steer_max)
# if lateral control is saturated for a certain period of time, send an alert for taking control of the car
# wind
if lateral_control_sat and not steer_override and v_ego > 10 and abs(error_steer) > 0.1:
sat_count += sat_count_rate
# unwind
else:
sat_count -= sat_count_rate
sat_flag = False
if sat_count >= sat_count_limit:
sat_flag = True
sat_count = clip(sat_count, 0, 1)
return output_steer, Up_steer, Ui_steer, lateral_control_sat, sat_count, sat_flag
class LatControl(object):
def __init__(self):
self.Up_steer = 0.
self.sat_count = 0
self.y_des = 0.0
self.lateral_control_sat = False
self.Ui_steer = 0.
self.reset()
def __init__(self, VM):
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, pos_limit=1.0)
self.setup_mpc()
self.y_des = -1 # Legacy
def setup_mpc(self):
self.libmpc = libmpc_py.libmpc
self.libmpc.init()
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
self.mpc_updated = False
self.cur_state[0].x = 0.0
self.cur_state[0].y = 0.0
self.cur_state[0].psi = 0.0
self.cur_state[0].delta = 0.0
self.last_mpc_ts = 0.0
self.angle_steers_des = 0
def reset(self):
self.Ui_steer = 0.
self.pid.reset()
def update(self, enabled, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM):
rate = 100
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
self.mpc_updated = False
if self.last_mpc_ts + 0.001 < PL.last_md_ts:
self.last_mpc_ts = PL.last_md_ts
steer_max = 1.0
curvature_factor = VM.curvature_factor(v_ego)
# how far we look ahead is function of speed and desired path
d_lookahead = calc_d_lookahead(v_ego, d_poly)
l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly))
r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly))
p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
# desired lookahead offset
self.y_des = np.polyval(d_poly, d_lookahead)
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.sR)
# calculate actual offset at the lookahead point
self.angle_steers_des, _ = calc_desired_steer_angle(v_ego, self.y_des,
d_lookahead, VM, angle_offset)
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
l_poly, r_poly, p_poly,
PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego, PL.PP.lane_width)
output_steer, self.Up_steer, self.Ui_steer, self.lateral_control_sat, self.sat_count, sat_flag = pid_lateral_control(
v_ego, angle_steers, self.angle_steers_des, self.Ui_steer, steer_max,
steer_override, self.sat_count, enabled, VM.CP.steerKp, VM.CP.steerKi, rate)
delta_desired = self.mpc_solution[0].delta[1]
self.cur_state[0].delta = delta_desired
final_steer = clip(output_steer, -steer_max, steer_max)
return final_steer, sat_flag
self.angle_steers_des = math.degrees(delta_desired * VM.CP.sR) + angle_offset
self.mpc_updated = True
if v_ego < 0.3 or not active:
output_steer = 0.0
self.pid.reset()
else:
steer_max = get_steer_max(VM.CP, v_ego)
self.pid.pos_limit = steer_max
self.pid.neg_limit = -steer_max
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override)
self.sat_flag = self.pid.saturated
return output_steer

89
selfdrive/controls/lib/latcontrol_helpers.py 100644
View File

@ -0,0 +1,89 @@
import numpy as np
import math
from common.numpy_fast import interp
_K_CURV_V = [1., 0.6]
_K_CURV_BP = [0., 0.002]
# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
_LANE_WIDTH_V = [3., 3.8]
# break points of speed
_LANE_WIDTH_BP = [0., 31.]
def calc_d_lookahead(v_ego, d_poly):
# this function computes how far too look for lateral control
# howfar we look ahead is function of speed and how much curvy is the path
offset_lookahead = 1.
k_lookahead = 7.
# integrate abs value of second derivative of poly to get a measure of path curvature
pts_len = 50. # m
if len(d_poly) > 0:
pts = np.polyval([6 * d_poly[0], 2 * d_poly[1]], np.arange(0, pts_len))
else:
pts = 0.
curv = np.sum(np.abs(pts)) / pts_len
k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
# sqrt on speed is needed to keep, for a given curvature, the y_des
# proportional to speed. Indeed, y_des is prop to d_lookahead^2
# 36m at 25m/s
d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
return d_lookahead
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
# this function returns the lateral offset given the steering angle, speed and the lookahead distance
sa = math.radians(angle_steers - angle_offset)
curvature = VM.calc_curvature(sa, v_ego)
# clip is to avoid arcsin NaNs due to too sharp turns
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999)) / 2.)
return y_actual, curvature
def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
# inverse of the above function
curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
steer_des = math.degrees(VM.get_steer_from_curvature(curvature, v_ego)) + angle_offset
return steer_des, curvature
def compute_path_pinv():
deg = 3
x = np.arange(50.0)
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
pinv = np.linalg.pinv(X)
return pinv
def model_polyfit(points, path_pinv):
return np.dot(path_pinv, map(float, points))
def calc_desired_path(l_poly,
r_poly,
p_poly,
l_prob,
r_prob,
p_prob,
speed,
lane_width=None):
# this function computes the poly for the center of the lane, averaging left and right polys
if lane_width is None:
lane_width = interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
# lanes in US are ~3.6m wide
half_lane_poly = np.array([0., 0., 0., lane_width / 2.])
if l_prob + r_prob > 0.01:
c_poly = ((l_poly - half_lane_poly) * l_prob +
(r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob)
c_prob = l_prob + r_prob - l_prob * r_prob
else:
c_poly = np.zeros(4)
c_prob = 0.
p_weight = 1. # predicted path weight relatively to the center of the lane
d_poly = list((c_poly * c_prob + p_poly * p_prob * p_weight) / (c_prob + p_prob * p_weight))
return d_poly, c_poly, c_prob

75
selfdrive/controls/lib/lateral_mpc/Makefile 100644
View File

@ -0,0 +1,75 @@
CC = clang
CXX = clang++
PHONELIBS = ../../../../phonelibs
UNAME_M := $(shell uname -m)
CFLAGS = -O3 -fPIC -I.
CXXFLAGS = -O3 -fPIC -I.
QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC
ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado
ifeq ($(UNAME_M),aarch64)
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
else
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
endif
OBJS = \
$(PHONELIBS)/qpoases/SRC/Bounds.o \
$(PHONELIBS)/qpoases/SRC/Constraints.o \
$(PHONELIBS)/qpoases/SRC/CyclingManager.o \
$(PHONELIBS)/qpoases/SRC/Indexlist.o \
$(PHONELIBS)/qpoases/SRC/MessageHandling.o \
$(PHONELIBS)/qpoases/SRC/QProblem.o \
$(PHONELIBS)/qpoases/SRC/QProblemB.o \
$(PHONELIBS)/qpoases/SRC/SubjectTo.o \
$(PHONELIBS)/qpoases/SRC/Utils.o \
$(PHONELIBS)/qpoases/SRC/EXTRAS/SolutionAnalysis.o \
mpc_export/acado_qpoases_interface.o \
mpc_export/acado_integrator.o \
mpc_export/acado_solver.o \
mpc_export/acado_auxiliary_functions.o \
mpc.o
DEPS := $(OBJS:.o=.d)
.PHONY: all
all: libcommampc.so
libcommampc.so: $(OBJS)
$(CXX) -shared -o '$@' $^ -lm
%.o: %.cpp
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) -MMD \
-I mpc_export/ \
$(QPOASES_FLAGS) \
-c -o '$@' '$<'
%.o: %.c
@echo "[ CC ] $@"
$(CC) $(CFLAGS) -MMD \
-I mpc_export/ \
$(QPOASES_FLAGS) \
-c -o '$@' '$<'
generator: generator.cpp
$(CXX) -Wall -std=c++11 \
generator.cpp \
-o generator \
$(ACADO_FLAGS) \
$(ACADO_LIBS)
.PHONY: generate
generate: generator
./generator
.PHONY: clean
clean:
rm -f libcommampc.so generator $(OBJS) $(DEPS)
-include $(DEPS)

0
selfdrive/controls/lib/lateral_mpc/__init__.py 100644
View File

137
selfdrive/controls/lib/lateral_mpc/generator.cpp 100644
View File

@ -0,0 +1,137 @@
#include <acado_code_generation.hpp>
#define PI 3.1415926536
#define deg2rad(d) (d/180.0*PI)
const int controlHorizon = 50;
const double samplingTime = 0.05; // 20 Hz
using namespace std;
int main( )
{
USING_NAMESPACE_ACADO
DifferentialEquation f;
DifferentialState xx; // x position
DifferentialState yy; // y position
DifferentialState psi; // vehicle heading
DifferentialState delta;
OnlineData curvature_factor;
OnlineData v_ref; // m/s
OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3;
OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3;
OnlineData p_poly_r0, p_poly_r1, p_poly_r2, p_poly_r3;
OnlineData l_prob, r_prob, p_prob;
OnlineData lane_width;
Control t;
// Equations of motion
f << dot(xx) == v_ref * cos(psi);
f << dot(yy) == v_ref * sin(psi);
f << dot(psi) == v_ref * delta * curvature_factor;
f << dot(delta) == t;
auto lr_prob = l_prob + r_prob - l_prob * r_prob;
auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3;
auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3;
auto poly_p = p_poly_r0*(xx*xx*xx) + p_poly_r1*(xx*xx) + p_poly_r2*xx + p_poly_r3;
auto angle_l = atan(3*l_poly_r0*xx*xx + 2*l_poly_r1*xx + l_poly_r2);
auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2);
auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2);
auto c_left_lane = exp(-(poly_l - yy));
auto c_right_lane = exp(poly_r - yy);
auto r_phantom = poly_l - lane_width/2.0;
auto l_phantom = poly_r + lane_width/2.0;
auto path = lr_prob * (l_prob * r_phantom + r_prob * l_phantom) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * poly_p;
auto angle = lr_prob * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * angle_p;
// Running cost
Function h;
// Distance errors
h << path - yy;
h << l_prob * c_left_lane;
h << r_prob * c_right_lane;
// Heading error
h << (v_ref + 1.0 ) * (angle - psi);
// Angular rate error
h << (v_ref + 1.0 ) * t;
DMatrix Q(5,5);
Q(0,0) = 1.0;
Q(1,1) = 1.0;
Q(2,2) = 1.0;
Q(3,3) = 1.0;
Q(4,4) = 1.0;
// Terminal cost
Function hN;
// Distance errors
hN << path - yy;
hN << l_prob * c_left_lane;
hN << r_prob * c_right_lane;
// Heading errors
hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
DMatrix QN(4,4);
QN(0,0) = 1.0;
QN(1,1) = 1.0;
QN(2,2) = 1.0;
QN(3,3) = 1.0;
// Setup Optimal Control Problem
const double tStart = 0.0;
const double tEnd = samplingTime * controlHorizon;
OCP ocp( tStart, tEnd, controlHorizon );
ocp.subjectTo(f);
ocp.minimizeLSQ(Q, h);
ocp.minimizeLSQEndTerm(QN, hN);
ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
ocp.subjectTo( deg2rad(-25) <= delta <= deg2rad(25));
ocp.subjectTo( -0.1 <= t <= 0.1);
ocp.setNOD(18);
OCPexport mpc(ocp);
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
mpc.set( INTEGRATOR_TYPE, INT_RK4 );
mpc.set( NUM_INTEGRATOR_STEPS, 250 );
mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
mpc.set( QP_SOLVER, QP_QPOASES );
mpc.set( HOTSTART_QP, YES );
mpc.set( GENERATE_TEST_FILE, NO);
mpc.set( GENERATE_MAKE_FILE, NO );
mpc.set( GENERATE_MATLAB_INTERFACE, NO );
mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
mpc.printDimensionsQP( );
return EXIT_SUCCESS;
}

30
selfdrive/controls/lib/lateral_mpc/libmpc_py.py 100644
View File

@ -0,0 +1,30 @@
import os
import subprocess
from cffi import FFI
mpc_dir = os.path.dirname(os.path.abspath(__file__))
libmpc_fn = os.path.join(mpc_dir, "libcommampc.so")
subprocess.check_output(["make", "-j4"], cwd=mpc_dir)
ffi = FFI()
ffi.cdef("""
typedef struct {
double x, y, psi, delta, t;
} state_t;
typedef struct {
double x[50];
double y[50];
double psi[50];
double delta[50];
} log_t;
void init();
void run_mpc(state_t * x0, log_t * solution,
double l_poly[4], double r_poly[4], double p_poly[4],
double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width);
""")
libmpc = ffi.dlopen(libmpc_fn)

100
selfdrive/controls/lib/lateral_mpc/mpc.c 100644
View File

@ -0,0 +1,100 @@
#include "acado_common.h"
#include "acado_auxiliary_functions.h"
#include <stdio.h>
#define NX ACADO_NX /* Number of differential state variables. */
#define NXA ACADO_NXA /* Number of algebraic variables. */
#define NU ACADO_NU /* Number of control inputs. */
#define NOD ACADO_NOD /* Number of online data values. */
#define NY ACADO_NY /* Number of measurements/references on nodes 0..N - 1. */
#define NYN ACADO_NYN /* Number of measurements/references on node N. */
#define N ACADO_N /* Number of intervals in the horizon. */
ACADOvariables acadoVariables;
ACADOworkspace acadoWorkspace;
typedef struct {
double x, y, psi, delta, t;
} state_t;
typedef struct {
double x[N];
double y[N];
double psi[N];
double delta[N];
} log_t;
void init(){
acado_initializeSolver();
int i;
/* Initialize the states and controls. */
for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0;
for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.1;
/* Initialize the measurements/reference. */
for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0;
for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0;
/* MPC: initialize the current state feedback. */
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
}
void run_mpc(state_t * x0, log_t * solution,
double l_poly[4], double r_poly[4], double p_poly[4],
double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width){
int i;
for (i = 0; i <= NOD * N; i+= NOD){
acadoVariables.od[i] = curvature_factor;
acadoVariables.od[i+1] = v_ref;
acadoVariables.od[i+2] = l_poly[0];
acadoVariables.od[i+3] = l_poly[1];
acadoVariables.od[i+4] = l_poly[2];
acadoVariables.od[i+5] = l_poly[3];
acadoVariables.od[i+6] = r_poly[0];
acadoVariables.od[i+7] = r_poly[1];
acadoVariables.od[i+8] = r_poly[2];
acadoVariables.od[i+9] = r_poly[3];
acadoVariables.od[i+10] = p_poly[0];
acadoVariables.od[i+11] = p_poly[1];
acadoVariables.od[i+12] = p_poly[2];
acadoVariables.od[i+13] = p_poly[3];
acadoVariables.od[i+14] = l_prob;
acadoVariables.od[i+15] = r_prob;
acadoVariables.od[i+16] = p_prob;
acadoVariables.od[i+17] = lane_width;
}
acadoVariables.x0[0] = x0->x;
acadoVariables.x0[1] = x0->y;
acadoVariables.x0[2] = x0->psi;
acadoVariables.x0[3] = x0->delta;
acado_preparationStep();
acado_feedbackStep( );
acado_shiftStates(2, 0, 0);
acado_shiftControls( 0 );
for (i = 0; i <= N; i++){
solution->x[i] = acadoVariables.x[i*NX];
solution->y[i] = acadoVariables.x[i*NX+1];
solution->psi[i] = acadoVariables.x[i*NX+2];
solution->delta[i] = acadoVariables.x[i*NX+3];
}
return;
}

212
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.c 100644
View File

@ -0,0 +1,212 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
#include "acado_auxiliary_functions.h"
#include <stdio.h>
real_t* acado_getVariablesX( )
{
return acadoVariables.x;
}
real_t* acado_getVariablesU( )
{
return acadoVariables.u;
}
#if ACADO_NY > 0
real_t* acado_getVariablesY( )
{
return acadoVariables.y;
}
#endif
#if ACADO_NYN > 0
real_t* acado_getVariablesYN( )
{
return acadoVariables.yN;
}
#endif
real_t* acado_getVariablesX0( )
{
#if ACADO_INITIAL_VALUE_FIXED
return acadoVariables.x0;
#else
return 0;
#endif
}
/** Print differential variables. */
void acado_printDifferentialVariables( )
{
int i, j;
printf("\nDifferential variables:\n[\n");
for (i = 0; i < ACADO_N + 1; ++i)
{
for (j = 0; j < ACADO_NX; ++j)
printf("\t%e", acadoVariables.x[i * ACADO_NX + j]);
printf("\n");
}
printf("]\n\n");
}
/** Print control variables. */
void acado_printControlVariables( )
{
int i, j;
printf("\nControl variables:\n[\n");
for (i = 0; i < ACADO_N; ++i)
{
for (j = 0; j < ACADO_NU; ++j)
printf("\t%e", acadoVariables.u[i * ACADO_NU + j]);
printf("\n");
}
printf("]\n\n");
}
/** Print ACADO code generation notice. */
void acado_printHeader( )
{
printf(
"\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
"Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n"
"Milan Vukov and Rien Quirynen, KU Leuven.\n"
);
printf(
"Developed within the Optimization in Engineering Center (OPTEC) under\n"
"supervision of Moritz Diehl. All rights reserved.\n\n"
"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
"General Public License 3 in the hope that it will be useful,\n"
"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
"GNU Lesser General Public License for more details.\n\n"
);
}
#if !(defined _DSPACE)
#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
void acado_tic( acado_timer* t )
{
QueryPerformanceFrequency(&t->freq);
QueryPerformanceCounter(&t->tic);
}
real_t acado_toc( acado_timer* t )
{
QueryPerformanceCounter(&t->toc);
return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart);
}
#elif (defined __APPLE__)
void acado_tic( acado_timer* t )
{
/* read current clock cycles */
t->tic = mach_absolute_time();
}
real_t acado_toc( acado_timer* t )
{
uint64_t duration; /* elapsed time in clock cycles*/
t->toc = mach_absolute_time();
duration = t->toc - t->tic;
/*conversion from clock cycles to nanoseconds*/
mach_timebase_info(&(t->tinfo));
duration *= t->tinfo.numer;
duration /= t->tinfo.denom;
return (real_t)duration / 1e9;
}
#else
#if __STDC_VERSION__ >= 199901L
/* C99 mode */
/* read current time */
void acado_tic( acado_timer* t )
{
gettimeofday(&t->tic, 0);
}
/* return time passed since last call to tic on this timer */
real_t acado_toc( acado_timer* t )
{
struct timeval temp;
gettimeofday(&t->toc, 0);
if ((t->toc.tv_usec - t->tic.tv_usec) < 0)
{
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec;
}
else
{
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec;
}
return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6;
}
#else
/* ANSI */
/* read current time */
void acado_tic( acado_timer* t )
{
clock_gettime(CLOCK_MONOTONIC, &t->tic);
}
/* return time passed since last call to tic on this timer */
real_t acado_toc( acado_timer* t )
{
struct timespec temp;
clock_gettime(CLOCK_MONOTONIC, &t->toc);
if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0)
{
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec;
}
else
{
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec;
}
return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9;
}
#endif /* __STDC_VERSION__ >= 199901L */
#endif /* (defined _WIN32 || _WIN64) */
#endif

138
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h 100644
View File

@ -0,0 +1,138 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
#ifndef ACADO_AUXILIARY_FUNCTIONS_H
#define ACADO_AUXILIARY_FUNCTIONS_H
#include "acado_common.h"
#ifndef __MATLAB__
#ifdef __cplusplus
extern "C"
{
#endif /* __cplusplus */
#endif /* __MATLAB__ */
/** Get pointer to the matrix with differential variables. */
real_t* acado_getVariablesX( );
/** Get pointer to the matrix with control variables. */
real_t* acado_getVariablesU( );
#if ACADO_NY > 0
/** Get pointer to the matrix with references/measurements. */
real_t* acado_getVariablesY( );
#endif
#if ACADO_NYN > 0
/** Get pointer to the vector with references/measurement on the last node. */
real_t* acado_getVariablesYN( );
#endif
/** Get pointer to the current state feedback vector. Only applicable for NMPC. */
real_t* acado_getVariablesX0( );
/** Print differential variables. */
void acado_printDifferentialVariables( );
/** Print control variables. */
void acado_printControlVariables( );
/** Print ACADO code generation notice. */
void acado_printHeader( );
/*
* A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for
* providing us with the following timing routines.
*/
#if !(defined _DSPACE)
#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
/* Use Windows QueryPerformanceCounter for timing. */
#include <Windows.h>
/** A structure for keeping internal timer data. */
typedef struct acado_timer_
{
LARGE_INTEGER tic;
LARGE_INTEGER toc;
LARGE_INTEGER freq;
} acado_timer;
#elif (defined __APPLE__)
#include "unistd.h"
#include <mach/mach_time.h>
/** A structure for keeping internal timer data. */
typedef struct acado_timer_
{
uint64_t tic;
uint64_t toc;
mach_timebase_info_data_t tinfo;
} acado_timer;
#else
/* Use POSIX clock_gettime() for timing on non-Windows machines. */
#include <time.h>
#if __STDC_VERSION__ >= 199901L
/* C99 mode of operation. */
#include <sys/stat.h>
#include <sys/time.h>
typedef struct acado_timer_
{
struct timeval tic;
struct timeval toc;
} acado_timer;
#else
/* ANSI C */
/** A structure for keeping internal timer data. */
typedef struct acado_timer_
{
struct timespec tic;
struct timespec toc;
} acado_timer;
#endif /* __STDC_VERSION__ >= 199901L */
#endif /* (defined _WIN32 || defined _WIN64) */
/** A function for measurement of the current time. */
void acado_tic( acado_timer* t );
/** A function which returns the elapsed time. */
real_t acado_toc( acado_timer* t );
#endif
#ifndef __MATLAB__
#ifdef __cplusplus
} /* extern "C" */
#endif /* __cplusplus */
#endif /* __MATLAB__ */
#endif /* ACADO_AUXILIARY_FUNCTIONS_H */

349
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h 100644
View File

@ -0,0 +1,349 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
#ifndef ACADO_COMMON_H
#define ACADO_COMMON_H
#include <math.h>
#include <string.h>
#ifndef __MATLAB__
#ifdef __cplusplus
extern "C"
{
#endif /* __cplusplus */
#endif /* __MATLAB__ */
/** \defgroup ACADO ACADO CGT generated module. */
/** @{ */
/** qpOASES QP solver indicator. */
#define ACADO_QPOASES 0
#define ACADO_QPOASES3 1
/** FORCES QP solver indicator.*/
#define ACADO_FORCES 2
/** qpDUNES QP solver indicator.*/
#define ACADO_QPDUNES 3
/** HPMPC QP solver indicator. */
#define ACADO_HPMPC 4
#define ACADO_GENERIC 5
/** Indicator for determining the QP solver used by the ACADO solver code. */
#define ACADO_QP_SOLVER ACADO_QPOASES
#include "acado_qpoases_interface.hpp"
/*
* Common definitions
*/
/** User defined block based condensing. */
#define ACADO_BLOCK_CONDENSING 0
/** Compute covariance matrix of the last state estimate. */
#define ACADO_COMPUTE_COVARIANCE_MATRIX 0
/** Flag indicating whether constraint values are hard-coded or not. */
#define ACADO_HARDCODED_CONSTRAINT_VALUES 1
/** Indicator for fixed initial state. */
#define ACADO_INITIAL_STATE_FIXED 1
/** Number of control/estimation intervals. */
#define ACADO_N 50
/** Number of online data values. */
#define ACADO_NOD 18
/** Number of path constraints. */
#define ACADO_NPAC 0
/** Number of control variables. */
#define ACADO_NU 1
/** Number of differential variables. */
#define ACADO_NX 4
/** Number of algebraic variables. */
#define ACADO_NXA 0
/** Number of differential derivative variables. */
#define ACADO_NXD 0
/** Number of references/measurements per node on the first N nodes. */
#define ACADO_NY 5
/** Number of references/measurements on the last (N + 1)st node. */
#define ACADO_NYN 4
/** Total number of QP optimization variables. */
#define ACADO_QP_NV 54
/** Number of integration steps per shooting interval. */
#define ACADO_RK_NIS 5
/** Number of Runge-Kutta stages per integration step. */
#define ACADO_RK_NSTAGES 4
/** Providing interface for arrival cost. */
#define ACADO_USE_ARRIVAL_COST 0
/** Indicator for usage of non-hard-coded linear terms in the objective. */
#define ACADO_USE_LINEAR_TERMS 0
/** Indicator for type of fixed weighting matrices. */
#define ACADO_WEIGHTING_MATRICES_TYPE 0
/*
* Globally used structure definitions
*/
/** The structure containing the user data.
*
* Via this structure the user "communicates" with the solver code.
*/
typedef struct ACADOvariables_
{
int dummy;
/** Matrix of size: 51 x 4 (row major format)
*
* Matrix containing 51 differential variable vectors.
*/
real_t x[ 204 ];
/** Column vector of size: 50
*
* Matrix containing 50 control variable vectors.
*/
real_t u[ 50 ];
/** Matrix of size: 51 x 18 (row major format)
*
* Matrix containing 51 online data vectors.
*/
real_t od[ 918 ];
/** Column vector of size: 250
*
* Matrix containing 50 reference/measurement vectors of size 5 for first 50 nodes.
*/
real_t y[ 250 ];
/** Column vector of size: 4
*
* Reference/measurement vector for the 51. node.
*/
real_t yN[ 4 ];
/** Column vector of size: 4
*
* Current state feedback vector.
*/
real_t x0[ 4 ];
} ACADOvariables;
/** Private workspace used by the auto-generated code.
*
* Data members of this structure are private to the solver.
* In other words, the user code should not modify values of this
* structure.
*/
typedef struct ACADOworkspace_
{
/** Column vector of size: 14 */
real_t rhs_aux[ 14 ];
real_t rk_ttt;
/** Row vector of size: 43 */
real_t rk_xxx[ 43 ];
/** Matrix of size: 4 x 24 (row major format) */
real_t rk_kkk[ 96 ];
/** Row vector of size: 43 */
real_t state[ 43 ];
/** Column vector of size: 200 */
real_t d[ 200 ];
/** Column vector of size: 250 */
real_t Dy[ 250 ];
/** Column vector of size: 4 */
real_t DyN[ 4 ];
/** Matrix of size: 200 x 4 (row major format) */
real_t evGx[ 800 ];
/** Column vector of size: 200 */
real_t evGu[ 200 ];
/** Column vector of size: 19 */
real_t objAuxVar[ 19 ];
/** Row vector of size: 23 */
real_t objValueIn[ 23 ];
/** Row vector of size: 30 */
real_t objValueOut[ 30 ];
/** Matrix of size: 200 x 4 (row major format) */
real_t Q1[ 800 ];
/** Matrix of size: 200 x 5 (row major format) */
real_t Q2[ 1000 ];
/** Column vector of size: 50 */
real_t R1[ 50 ];
/** Matrix of size: 50 x 5 (row major format) */
real_t R2[ 250 ];
/** Matrix of size: 4 x 4 (row major format) */
real_t QN1[ 16 ];
/** Matrix of size: 4 x 4 (row major format) */
real_t QN2[ 16 ];
/** Column vector of size: 4 */
real_t Dx0[ 4 ];
/** Matrix of size: 4 x 4 (row major format) */
real_t T[ 16 ];
/** Column vector of size: 5100 */
real_t E[ 5100 ];
/** Column vector of size: 5100 */
real_t QE[ 5100 ];
/** Matrix of size: 200 x 4 (row major format) */
real_t QGx[ 800 ];
/** Column vector of size: 200 */
real_t Qd[ 200 ];
/** Column vector of size: 204 */
real_t QDy[ 204 ];
/** Matrix of size: 50 x 4 (row major format) */
real_t H10[ 200 ];
/** Matrix of size: 54 x 54 (row major format) */
real_t H[ 2916 ];
/** Matrix of size: 100 x 54 (row major format) */
real_t A[ 5400 ];
/** Column vector of size: 54 */
real_t g[ 54 ];
/** Column vector of size: 54 */
real_t lb[ 54 ];
/** Column vector of size: 54 */
real_t ub[ 54 ];
/** Column vector of size: 100 */
real_t lbA[ 100 ];
/** Column vector of size: 100 */
real_t ubA[ 100 ];
/** Column vector of size: 54 */
real_t x[ 54 ];
/** Column vector of size: 154 */
real_t y[ 154 ];
} ACADOworkspace;
/*
* Forward function declarations.
*/
/** Performs the integration and sensitivity propagation for one shooting interval.
*
* \param rk_eta Working array to pass the input values and return the results.
* \param resetIntegrator The internal memory of the integrator can be reset.
*
* \return Status code of the integrator.
*/
int acado_integrate( real_t* const rk_eta, int resetIntegrator );
/** Export of an ACADO symbolic function.
*
* \param in Input to the exported function.
* \param out Output of the exported function.
*/
void acado_rhs_forw(const real_t* in, real_t* out);
/** Preparation step of the RTI scheme.
*
* \return Status of the integration module. =0: OK, otherwise the error code.
*/
int acado_preparationStep( );
/** Feedback/estimation step of the RTI scheme.
*
* \return Status code of the qpOASES QP solver.
*/
int acado_feedbackStep( );
/** Solver initialization. Must be called once before any other function call.
*
* \return =0: OK, otherwise an error code of a QP solver.
*/
int acado_initializeSolver( );
/** Initialize shooting nodes by a forward simulation starting from the first node.
*/
void acado_initializeNodesByForwardSimulation( );
/** Shift differential variables vector by one interval.
*
* \param strategy Shifting strategy: 1. Initialize node 51 with xEnd. 2. Initialize node 51 by forward simulation.
* \param xEnd Value for the x vector on the last node. If =0 the old value is used.
* \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
*/
void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd );
/** Shift controls vector by one interval.
*
* \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
*/
void acado_shiftControls( real_t* const uEnd );
/** Get the KKT tolerance of the current iterate.
*
* \return The KKT tolerance value.
*/
real_t acado_getKKT( );
/** Calculate the objective value.
*
* \return Value of the objective function.
*/
real_t acado_getObjective( );
/*
* Extern declarations.
*/
extern ACADOworkspace acadoWorkspace;
extern ACADOvariables acadoVariables;
/** @} */
#ifndef __MATLAB__
#ifdef __cplusplus
} /* extern "C" */
#endif /* __cplusplus */
#endif /* __MATLAB__ */
#endif /* ACADO_COMMON_H */

252
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c 100644
View File

@ -0,0 +1,252 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
#include "acado_common.h"
void acado_rhs_forw(const real_t* in, real_t* out)
{
const real_t* xd = in;
const real_t* u = in + 24;
const real_t* od = in + 25;
/* Vector of auxiliary variables; number of elements: 14. */
real_t* a = acadoWorkspace.rhs_aux;
/* Compute intermediate quantities: */
a[0] = (cos(xd[2]));
a[1] = (sin(xd[2]));
a[2] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2])));
a[3] = (xd[12]*a[2]);
a[4] = (xd[13]*a[2]);
a[5] = (xd[14]*a[2]);
a[6] = (xd[15]*a[2]);
a[7] = (cos(xd[2]));
a[8] = (xd[12]*a[7]);
a[9] = (xd[13]*a[7]);
a[10] = (xd[14]*a[7]);
a[11] = (xd[15]*a[7]);
a[12] = (xd[22]*a[2]);
a[13] = (xd[22]*a[7]);
/* Compute outputs: */
out[0] = (od[1]*a[0]);
out[1] = (od[1]*a[1]);
out[2] = ((od[1]*xd[3])*od[0]);
out[3] = u[0];
out[4] = (od[1]*a[3]);
out[5] = (od[1]*a[4]);
out[6] = (od[1]*a[5]);
out[7] = (od[1]*a[6]);
out[8] = (od[1]*a[8]);
out[9] = (od[1]*a[9]);
out[10] = (od[1]*a[10]);
out[11] = (od[1]*a[11]);
out[12] = ((od[1]*xd[16])*od[0]);
out[13] = ((od[1]*xd[17])*od[0]);
out[14] = ((od[1]*xd[18])*od[0]);
out[15] = ((od[1]*xd[19])*od[0]);
out[16] = (real_t)(0.0000000000000000e+00);
out[17] = (real_t)(0.0000000000000000e+00);
out[18] = (real_t)(0.0000000000000000e+00);
out[19] = (real_t)(0.0000000000000000e+00);
out[20] = (od[1]*a[12]);
out[21] = (od[1]*a[13]);
out[22] = ((od[1]*xd[23])*od[0]);
out[23] = (real_t)(1.0000000000000000e+00);
}
/* Fixed step size:0.01 */
int acado_integrate( real_t* const rk_eta, int resetIntegrator )
{
int error;
int run1;
acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
rk_eta[4] = 1.0000000000000000e+00;
rk_eta[5] = 0.0000000000000000e+00;
rk_eta[6] = 0.0000000000000000e+00;
rk_eta[7] = 0.0000000000000000e+00;
rk_eta[8] = 0.0000000000000000e+00;
rk_eta[9] = 1.0000000000000000e+00;
rk_eta[10] = 0.0000000000000000e+00;
rk_eta[11] = 0.0000000000000000e+00;
rk_eta[12] = 0.0000000000000000e+00;
rk_eta[13] = 0.0000000000000000e+00;
rk_eta[14] = 1.0000000000000000e+00;
rk_eta[15] = 0.0000000000000000e+00;
rk_eta[16] = 0.0000000000000000e+00;
rk_eta[17] = 0.0000000000000000e+00;
rk_eta[18] = 0.0000000000000000e+00;
rk_eta[19] = 1.0000000000000000e+00;
rk_eta[20] = 0.0000000000000000e+00;
rk_eta[21] = 0.0000000000000000e+00;
rk_eta[22] = 0.0000000000000000e+00;
rk_eta[23] = 0.0000000000000000e+00;
acadoWorkspace.rk_xxx[24] = rk_eta[24];
acadoWorkspace.rk_xxx[25] = rk_eta[25];
acadoWorkspace.rk_xxx[26] = rk_eta[26];
acadoWorkspace.rk_xxx[27] = rk_eta[27];
acadoWorkspace.rk_xxx[28] = rk_eta[28];
acadoWorkspace.rk_xxx[29] = rk_eta[29];
acadoWorkspace.rk_xxx[30] = rk_eta[30];
acadoWorkspace.rk_xxx[31] = rk_eta[31];
acadoWorkspace.rk_xxx[32] = rk_eta[32];
acadoWorkspace.rk_xxx[33] = rk_eta[33];
acadoWorkspace.rk_xxx[34] = rk_eta[34];
acadoWorkspace.rk_xxx[35] = rk_eta[35];
acadoWorkspace.rk_xxx[36] = rk_eta[36];
acadoWorkspace.rk_xxx[37] = rk_eta[37];
acadoWorkspace.rk_xxx[38] = rk_eta[38];
acadoWorkspace.rk_xxx[39] = rk_eta[39];
acadoWorkspace.rk_xxx[40] = rk_eta[40];
acadoWorkspace.rk_xxx[41] = rk_eta[41];
acadoWorkspace.rk_xxx[42] = rk_eta[42];
for (run1 = 0; run1 < 5; ++run1)
{
acadoWorkspace.rk_xxx[0] = + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[0] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[1] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[2] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[3] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[4] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[5] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[6] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[7] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[8] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[9] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[10] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[11] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[12] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[13] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[14] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[15] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[16] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[17] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[18] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[19] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[20] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[21] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[22] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[23] + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[24] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[25] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[26] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[27] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[28] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[29] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[30] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[31] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[32] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[33] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[34] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[35] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[36] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[37] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[38] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[39] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[40] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[41] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[42] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[43] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[44] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[45] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[46] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[47] + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) );
rk_eta[0] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[0] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[24] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[48] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[72];
rk_eta[1] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[1] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[25] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[49] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[73];
rk_eta[2] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[2] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[26] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[50] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[74];
rk_eta[3] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[3] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[27] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[51] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[75];
rk_eta[4] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[4] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[28] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[52] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[76];
rk_eta[5] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[5] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[29] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[53] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[77];
rk_eta[6] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[6] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[54] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[78];
rk_eta[7] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[7] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[55] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[79];
rk_eta[8] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[8] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[56] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[80];
rk_eta[9] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[9] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[57] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[81];
rk_eta[10] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[10] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[58] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[82];
rk_eta[11] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[11] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[59] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[83];
rk_eta[12] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[12] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[60] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[84];
rk_eta[13] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[13] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[61] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[85];
rk_eta[14] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[14] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[62] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[86];
rk_eta[15] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[15] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[63] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[87];
rk_eta[16] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[16] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[64] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[88];
rk_eta[17] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[17] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[65] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[89];
rk_eta[18] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[18] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[66] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[90];
rk_eta[19] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[19] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[67] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[91];
rk_eta[20] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[20] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[68] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[92];
rk_eta[21] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[21] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[45] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[69] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[93];
rk_eta[22] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[22] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[46] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[70] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[94];
rk_eta[23] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[23] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[47] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[71] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[95];
acadoWorkspace.rk_ttt += 2.0000000000000001e-01;
}
error = 0;
return error;
}

70
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp 100644
View File

@ -0,0 +1,70 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
extern "C"
{
#include "acado_common.h"
}
#include "INCLUDE/QProblem.hpp"
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp"
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
static int acado_nWSR;
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
static SolutionAnalysis acado_sa;
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
int acado_solve( void )
{
acado_nWSR = QPOASES_NWSRMAX;
QProblem qp(54, 100);
returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
qp.getPrimalSolution( acadoWorkspace.x );
qp.getDualSolution( acadoWorkspace.y );
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
if (retVal != SUCCESSFUL_RETURN)
return (int)retVal;
retVal = acado_sa.getHessianInverse( &qp,var );
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
return (int)retVal;
}
int acado_getNWSR( void )
{
return acado_nWSR;
}
const char* acado_getErrorString( int error )
{
return MessageHandling::getErrorString( error );
}

65
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp 100644
View File

@ -0,0 +1,65 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
#ifndef QPOASES_HEADER
#define QPOASES_HEADER
#ifdef PC_DEBUG
#include <stdio.h>
#endif /* PC_DEBUG */
#include <math.h>
#ifdef __cplusplus
#define EXTERNC extern "C"
#else
#define EXTERNC
#endif
/*
* A set of options for qpOASES
*/
/** Maximum number of optimization variables. */
#define QPOASES_NVMAX 54
/** Maximum number of constraints. */
#define QPOASES_NCMAX 100
/** Maximum number of working set recalculations. */
#define QPOASES_NWSRMAX 462
/** Print level for qpOASES. */
#define QPOASES_PRINTLEVEL PL_NONE
/** The value of EPS */
#define QPOASES_EPS 2.221e-16
/** Internally used floating point type */
typedef double real_t;
/*
* Forward function declarations
*/
/** A function that calls the QP solver */
EXTERNC int acado_solve( void );
/** Get the number of active set changes */
EXTERNC int acado_getNWSR( void );
/** Get the error string. */
const char* acado_getErrorString( int error );
#endif /* QPOASES_HEADER */

1981
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c 100644
View File

File diff suppressed because one or more lines are too long

Some files were not shown because too many files have changed in this diff Show More