rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

INS/Guidance: Convert distance to using m instead of cm. This is mainly to be

Browse Source 
able to use NAN to indicate when values aren't valid from INS.
This commit is contained in:
James Cotton 2011-09-11 22:57:11 -05:00
parent a682d5deb6
commit 5b836db54c
8 changed files with 38 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
flight/INS/insgps_helper.c
View File

@ -83,16 +83,16 @@ void ins_outdoor_update()
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = Nav.Pos[0] * 100;
positionActual.East = Nav.Pos[1] * 100;
positionActual.Down = Nav.Pos[2] * 100;
positionActual.North = Nav.Pos[0];
positionActual.East = Nav.Pos[1];
positionActual.Down = Nav.Pos[2];
PositionActualSet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = Nav.Vel[0] * 100;
velocityActual.East = Nav.Vel[1] * 100;
velocityActual.Down = Nav.Vel[2] * 100;
velocityActual.North = Nav.Vel[0];
velocityActual.East = Nav.Vel[1];
velocityActual.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual);
sensors = 0;
@ -234,30 +234,32 @@ void ins_indoor_update()
if(gps_data.updated) {
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = gps_data.NED[0] * 100;
positionActual.East = gps_data.NED[1] * 100;
positionActual.Down = Nav.Pos[2] * 100;
positionActual.North = gps_data.NED[0];
positionActual.East = gps_data.NED[1];
positionActual.Down = Nav.Pos[2];
PositionActualSet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
velocityActual.East = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
velocityActual.Down = Nav.Vel[2] * 100;
velocityActual.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual);
gps_data.updated = false;
} else {
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.Down = Nav.Pos[2] * 100;
positionActual.North = NAN;
positionActual.East = NAN;
positionActual.Down = Nav.Pos[2];
PositionActualSet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = 0;
velocityActual.East = 0;
velocityActual.Down = Nav.Vel[2] * 100;
velocityActual.North = NAN;
velocityActual.East = NAN;
velocityActual.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual);
}

6
flight/Modules/Guidance/guidance.c
View File

@ -192,9 +192,9 @@ static void guidanceTask(void *parameters)
NedAccelData accelData;
NedAccelGet(&accelData);
// Convert from m/s to cm/s
accelData.North = accel_ned[0] * 100;
accelData.East = accel_ned[1] * 100;
accelData.Down = accel_ned[2] * 100;
accelData.North = accel_ned[0];
accelData.East = accel_ned[1];
accelData.Down = accel_ned[2];
NedAccelSet(&accelData);

8
shared/uavobjectdefinition/guidancesettings.xml
View File

@ -2,10 +2,10 @@
<object name="GuidanceSettings" singleinstance="true" settings="true">
<description>Settings for the @ref GuidanceModule</description>
<field name="GuidanceMode" units="" type="enum" elements="1" options="DUAL_LOOP,VELOCITY_CONTROL" defaultvalue="DUAL_LOOP"/>
<field name="HorizontalVelMax" units="cm/s" type="uint16" elements="1" defaultvalue="300"/>
<field name="VerticalVelMax" units="cm/s" type="uint16" elements="1" defaultvalue="150"/>
<field name="HorizontalPosPI" units="(cm/s)/cm" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="0.1,0.001,300"/>
<field name="HorizontalVelPID" units="deg/(cm/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.05,0.002,0,1000"/>
<field name="HorizontalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="300"/>
<field name="VerticalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="150"/>
<field name="HorizontalPosPI" units="(m/s)/m" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="0.1,0.001,300"/>
<field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.05,0.002,0,1000"/>
<field name="VerticalPosPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="0.1,0.001,200"/>
<field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.1,0,0,0"/>
<field name="ThrottleControl" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>

6
shared/uavobjectdefinition/nedaccel.xml
View File

@ -1,9 +1,9 @@
<xml>
<object name="NedAccel" singleinstance="true" settings="false">
<description>The projection of acceleration in the NED reference frame used by @ref Guidance.</description>
<field name="North" units="cm/s^2" type="float" elements="1"/>
<field name="East" units="cm/s^2" type="float" elements="1"/>
<field name="Down" units="cm/s^2" type="float" elements="1"/>
<field name="North" units="m/s^2" type="float" elements="1"/>
<field name="East" units="m/s^2" type="float" elements="1"/>
<field name="Down" units="m/s^2" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="10001"/>

6
shared/uavobjectdefinition/positionactual.xml
View File

@ -1,9 +1,9 @@
<xml>
<object name="PositionActual" singleinstance="true" settings="false">
<description>Contains the current position relative to @ref HomeLocation</description>
<field name="North" units="cm" type="int32" elements="1"/>
<field name="East" units="cm" type="int32" elements="1"/>
<field name="Down" units="cm" type="int32" elements="1"/>
<field name="North" units="m" type="float" elements="1"/>
<field name="East" units="m" type="float" elements="1"/>
<field name="Down" units="m" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

6
shared/uavobjectdefinition/positiondesired.xml
View File

@ -1,9 +1,9 @@
<xml>
<object name="PositionDesired" singleinstance="true" settings="false">
<description>The position the craft is trying t achieve. Can come from GCS or @ref PathPlanner </description>
<field name="North" units="cm" type="int32" elements="1"/>
<field name="East" units="cm" type="int32" elements="1"/>
<field name="Down" units="cm" type="int32" elements="1"/>
<field name="North" units="m" type="float" elements="1"/>
<field name="East" units="m" type="float" elements="1"/>
<field name="Down" units="m" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="onchange" period="0"/>

6
shared/uavobjectdefinition/velocityactual.xml
View File

@ -1,9 +1,9 @@
<xml>
<object name="VelocityActual" singleinstance="true" settings="false">
<description>Updated by @ref AHRSCommsModule and used within @ref GuidanceModule for velocity control</description>
<field name="North" units="cm/s" type="int32" elements="1"/>
<field name="East" units="cm/s" type="int32" elements="1"/>
<field name="Down" units="cm/s" type="int32" elements="1"/>
<field name="North" units="m/s" type="float" elements="1"/>
<field name="East" units="m/s" type="float" elements="1"/>
<field name="Down" units="m/s" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

6
shared/uavobjectdefinition/velocitydesired.xml
View File

@ -1,9 +1,9 @@
<xml>
<object name="VelocityDesired" singleinstance="true" settings="false">
<description>Used within @ref GuidanceModule to communicate between the task computing the desired velocity and the PID loop to achieve it (running at different rates).</description>
<field name="North" units="cm/s" type="int32" elements="1"/>
<field name="East" units="cm/s" type="int32" elements="1"/>
<field name="Down" units="cm/s" type="int32" elements="1"/>
<field name="North" units="m/s" type="float" elements="1"/>
<field name="East" units="m/s" type="float" elements="1"/>
<field name="Down" units="m/s" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>