Skip to content

Laser orientation eth #413

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 4 commits into
base: development
Choose a base branch
from
Open

Laser orientation eth #413

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
5238990
quick debug changes
Jan 19, 2017
9dc14c7
charge test changes 01.19.17
Jan 20, 2017
3c501b4
added pitch/roll/yaw to gs packet
Jan 28, 2017
7a97a1f
replace use of double and sqrt
Jan 28, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
111 changes: 56 additions & 55 deletions ...WARE/COMMON_CODE/MULTICORE/LCCM644__MULTICORE__DS18B20/TEMPERATURE/ds18b20__temperature.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,65 +76,66 @@ Lint16 s16DS18B20_TEMP__Read(Luint16 u16SensorIndex)
* @st_funcMD5 2A3FA2AC34982FD9EA9455F9CB0482D1
* @st_funcID LCCM644R0.FILE.003.FUNC.003
*/
Lint16 s16DS18B20_TEMP__All_Request(Luint16 u16SensorIndex, Luint8 u8Wait)
Lint16 s16DS18B20_TEMP__All_Request(Luint16 u16BusIndex, Luint8 u8Wait)
{
/*lint -e934*/
//Note 934: Taking address of near auto variable [MISRA 2004 Rule 1.2]

Lint16 s16Return;
/*lint -e934*/
//Note 934: Taking address of near auto variable [MISRA 2004 Rule 1.2]

Lint16 s16Return;

//generate a reset on the wire
s16Return = s16DS18B20_1WIRE__Generate_Reset(u16BusIndex);

//issue the skip command, no serial number needed.
s16Return = s16DS18B20_1WIRE__Skip(u16BusIndex);

//start the convert
s16Return = s16DS18B20_1WIRE__WriteByte(u16BusIndex, 0x44U);

//wait for conversion?
if(u8Wait == 1U)
{
//TODO: Use the defines in stead of a random bus index
/*
switch(sDS18B20.sDevice[0].u8Resolution)
{
case 9:
vDS18B20_DELAYS__Delay_mS(94U);
break;
case 10:
vDS18B20_DELAYS__Delay_mS(188U);
break;
case 11:
vDS18B20_DELAYS__Delay_mS(375U);
break;
case 12:
vDS18B20_DELAYS__Delay_mS(750U);
break;
default:
//not sure what to do here?
break;

}//switch(sDS18B20.sDevice[u8DSIndex].u8Resolution)
*/

//now that we have waited, the user can read it

s16Return = 0;

}
else
{
//exit
}

//at this point here, either we are ready to read, or we have an error

return s16Return;

/*lint +e934*/

//generate a reset on the wire
s16Return = s16DS18B20_1WIRE__Generate_Reset(sDS18B20.sDevice[u16SensorIndex].u8ChannelIndex);

//issue the skip command, no serial number needed.
s16Return = s16DS18B20_1WIRE__Skip(sDS18B20.sDevice[u16SensorIndex].u8ChannelIndex);

//start the convert
s16Return = s16DS18B20_1WIRE__WriteByte(sDS18B20.sDevice[u16SensorIndex].u8ChannelIndex, 0x44U);

//wait for conversion?
if(u8Wait == 1U)
{

switch(sDS18B20.sDevice[u16SensorIndex].u8Resolution)
{
case 9:
vDS18B20_DELAYS__Delay_mS(94U);
break;
case 10:
vDS18B20_DELAYS__Delay_mS(188U);
break;
case 11:
vDS18B20_DELAYS__Delay_mS(375U);
break;
case 12:
vDS18B20_DELAYS__Delay_mS(750U);
break;
default:
//not sure what to do here?
break;

}//switch(sDS18B20.sDevice[u8DSIndex].u8Resolution)

//now that we have waited, the user can read it

s16Return = 0;

}
else
{
//exit
}

//at this point here, either we are ready to read, or we have an error

return s16Return;

/*lint +e934*/

}


Lint16 s16DS18B20_TEMP__All_Request_ByChannel(Luint8 u8ChannelIndex, Luint8 u8Wait)
{
/*lint -e934*/
Expand Down
13 changes: 13 additions & 0 deletions FIRMWARE/PROJECT_CODE/LCCM653__RLOOP__POWER_CORE/CHARGER/power_core__charger.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -164,10 +164,15 @@ void vPWRNODE_CHG__Process(void)
//Check that the battery temperatures are all below 40C.
//If this is ever exceeded, stop charging. This is done autonomously,
//but should additionally be checked by operator.
<<<<<<< HEAD
#if 0
if(sPWRNODE.sTemp.u8NewTempAvail == 1U)
{
#endif //0
=======
//if(sPWRNODE.sTemp.u8NewTempAvail == 1U)
//{
>>>>>>> charge test changes 01.19.17
//check the temp ranges
if(sPWRNODE.sTemp.f32HighestTemp < sPWRNODE.sCharger.f32MaxCellTemp)
{
Expand All @@ -182,13 +187,21 @@ void vPWRNODE_CHG__Process(void)

//clear the flag, we are done
sPWRNODE.sTemp.u8NewTempAvail = 0U;
<<<<<<< HEAD
#if 0
}
else
{
//stay in state, no new temp avail yet
}
#endif //0
=======
//}
//else
//{
// //stay in state, no new temp avail yet
//}
>>>>>>> charge test changes 01.19.17
break;

case CHG_STATE__START_BALANCE:
Expand Down
12 changes: 6 additions & 6 deletions ...ODE/LCCM655__RLOOP__FCU_CORE/FLIGHT_CONTROLLER/LASER_ORIENTATION/fcu__laser_orientation.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -293,7 +293,7 @@ Lfloat32 f32FCU_FLIGHTCTL_LASERORIENT__PointToPlaneDistance(Lfloat32 f32Position
return
(
(sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] * f32Position[LASER_ORIENT__X] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] * f32Position[LASER_ORIENT__Y] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C] * f32Position[LASER_ORIENT__Z] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__D]) /
sqrt((double)(sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C]))
f32NUMERICAL_FLOAT__SQRT((Lfloat32)(sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C]))
);
}

Expand All @@ -305,7 +305,7 @@ void vFCU_FLIGHTCTL_LASERORIENT__CalcRoll(void)
Lfloat32 f32vec1x = 1, f32vec1y = 0, f32vec1z = 0;

//Angle between two planes
sFCU.sFlightControl.sOrient.s16Roll = (Lint16)(C_NUMERICAL__PI/2 - f32NUMERICAL_Asine((double)((f32vec1x * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] + f32vec1y * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] + f32vec1z * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C]) / sqrt((double)(sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C])))) * 10000); // TODO: Trig
sFCU.sFlightControl.sOrient.s16Roll = (Lint16)(C_NUMERICAL__PI/2 - f32NUMERICAL_Asine((Lfloat32)((f32vec1x * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] + f32vec1y * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] + f32vec1z * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C]) / f32NUMERICAL_FLOAT__SQRT((Lfloat32)(sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C])))) * 10000); // TODO: Trig
}


Expand All @@ -316,7 +316,7 @@ void vFCU_FLIGHTCTL_LASERORIENT__CalcPitch(void)
Lfloat32 f32vec1x = 0, f32vec1y = 1, f32vec1z = 0;

//Angle between two planes
sFCU.sFlightControl.sOrient.s16Pitch = (Lint16)(C_NUMERICAL__PI/2 - f32NUMERICAL_Asine((double)((f32vec1x * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] + f32vec1y * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] + f32vec1z * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C]) / sqrt((double)(sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C])))) * 10000); // TODO: Trig
sFCU.sFlightControl.sOrient.s16Pitch = (Lint16)(C_NUMERICAL__PI/2 - f32NUMERICAL_Asine((Lfloat32)((f32vec1x * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] + f32vec1y * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] + f32vec1z * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C]) / f32NUMERICAL_FLOAT__SQRT((Lfloat32)(sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__A] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__B] + sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C] * sFCU.sFlightControl.sOrient.f32PlaneCoeffs[LASER_ORIENT__C])))) * 10000); // TODO: Trig
}


Expand All @@ -327,7 +327,7 @@ void vFCU_FLIGHTCTL_LASERORIENT__CalcTwistRoll(void)
Lfloat32 f32vec1x = 1, f32vec1y = 0, f32vec1z = 0;

//Angle between two planes
sFCU.sFlightControl.sOrient.s16TwistRoll = (Lint16)(C_NUMERICAL__PI/2 - f32NUMERICAL_Asine((double)((f32vec1x * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] + f32vec1y * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] + f32vec1z * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C]) / sqrt((double)(sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] + sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] + sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C])))) * 10000); // TODO: Trig
sFCU.sFlightControl.sOrient.s16TwistRoll = (Lint16)(C_NUMERICAL__PI/2 - f32NUMERICAL_Asine((Lfloat32)((f32vec1x * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] + f32vec1y * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] + f32vec1z * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C]) / f32NUMERICAL_FLOAT__SQRT((Lfloat32)(sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] + sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] + sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C])))) * 10000); // TODO: Trig
// the discrepancy in roll measured by two triplets of lasers gives the twisting of the pod structure // TODO: check signs
sFCU.sFlightControl.sOrient.s16TwistRoll -= sFCU.sFlightControl.sOrient.s16Roll;
}
Expand All @@ -340,7 +340,7 @@ void vFCU_FLIGHTCTL_LASERORIENT__CalcTwistPitch(void)
Lfloat32 f32vec1x = 0, f32vec1y = 1, f32vec1z = 0;

//Angle between two planes
sFCU.sFlightControl.sOrient.s16TwistPitch = (Lint16)(C_NUMERICAL__PI/2 - f32NUMERICAL_Asine((double)((f32vec1x * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] + f32vec1y * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] + f32vec1z * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C]) / sqrt((double)(sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] + sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] + sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C])))) * 10000); // TODO: Trig
sFCU.sFlightControl.sOrient.s16TwistPitch = (Lint16)(C_NUMERICAL__PI/2 - f32NUMERICAL_Asine((Lfloat32)((f32vec1x * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] + f32vec1y * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] + f32vec1z * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C]) / f32NUMERICAL_FLOAT__SQRT((Lfloat32)(sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__A] + sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__B] + sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C] * sFCU.sFlightControl.sOrient.f32TwistPlaneCoeffs[LASER_ORIENT__C])))) * 10000); // TODO: Trig
// the discrepancy in pitch measured by two triplets of lasers gives the bending of the pod structure // TODO: check signs
sFCU.sFlightControl.sOrient.s16TwistPitch -= sFCU.sFlightControl.sOrient.s16Pitch;
}
Expand Down Expand Up @@ -406,7 +406,7 @@ void vFCU_FLIGHTCTL_LASERORIENT__CalcYaw_and_Lateral(void)

f32LaserSeparation_X = sFCU.sFlightControl.sOrient.sBeamLasers[0].f32Position[0] - sFCU.sFlightControl.sOrient.sBeamLasers[1].f32Position[0];

sFCU.sFlightControl.sOrient.s16Yaw = (Lint16)(2 * f32NUMERICAL_Atan((f32LaserSeparation_X - sqrt(f32LaserSeparation_X * f32LaserSeparation_X + f32Y * f32Y - f32BeamThickness * f32BeamThickness)) / (f32BeamThickness - f32Y))); //todo: why is there a *10000 in old code?
sFCU.sFlightControl.sOrient.s16Yaw = (Lint16)(2 * f32NUMERICAL_Atan((f32LaserSeparation_X - f32NUMERICAL_FLOAT__SQRT((Lfloat32)f32LaserSeparation_X * f32LaserSeparation_X + f32Y * f32Y - f32BeamThickness * f32BeamThickness)) / (f32BeamThickness - f32Y))); //todo: why is there a *10000 in old code?

sFCU.sFlightControl.sOrient.f32Lateral = (f32Y0 - f32Y1) * f32NUMERICAL_Cosine(sFCU.sFlightControl.sOrient.s16Yaw) / 2;

Expand Down
25 changes: 25 additions & 0 deletions FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__ethernet.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,31 @@ void vFCU_LASEROPTO_ETH__Transmit(E_NET__PACKET_T ePacketType)
pu8Buffer += 4U;

}//for(u8Device = 0U; u8Device < C_FCU__NUM_LASERS_OPTONCDT; u8Device++)

//Lateral Translation
vNUMERICAL_CONVERT__Array_F32(pu8Buffer, sFCU.sFlightControl.sOrient.f32Lateral);
pu8Buffer += 4U;

//Pitch
vNUMERICAL_CONVERT__Array_S16(pu8Buffer, sFCU.sFlightControl.sOrient.s16Pitch);
pu8Buffer += 2U;

//Roll
vNUMERICAL_CONVERT__Array_S16(pu8Buffer, sFCU.sFlightControl.sOrient.s16Roll);
pu8Buffer += 2U;

//Yaw
vNUMERICAL_CONVERT__Array_S16(pu8Buffer, sFCU.sFlightControl.sOrient.s16Yaw);
pu8Buffer += 2U;

//Twist Pitch
vNUMERICAL_CONVERT__Array_S16(pu8Buffer, sFCU.sFlightControl.sOrient.s16TwistPitch);
pu8Buffer += 2U;

//Twist Roll
vNUMERICAL_CONVERT__Array_S16(pu8Buffer, sFCU.sFlightControl.sOrient.s16TwistRoll);
pu8Buffer += 2U;

break;

default:
Expand Down