mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
INS/Guidance: Convert distance to using m instead of cm. This is mainly to be
able to use NAN to indicate when values aren't valid from INS.
This commit is contained in:
parent
a682d5deb6
commit
5b836db54c
@ -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);
|
||||
}
|
||||
|
||||
|
@ -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);
|
||||
|
||||
|
||||
|
@ -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"/>
|
||||
|
@ -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"/>
|
||||
|
@ -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"/>
|
||||
|
@ -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"/>
|
||||
|
@ -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"/>
|
||||
|
@ -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"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user