diff --git a/FIRMWARE/COMMON_CODE/MULTICORE/LCCM644__MULTICORE__DS18B20/TEMPERATURE/ds18b20__temperature.c b/FIRMWARE/COMMON_CODE/MULTICORE/LCCM644__MULTICORE__DS18B20/TEMPERATURE/ds18b20__temperature.c index 880d78f4..5d17b018 100644 --- a/FIRMWARE/COMMON_CODE/MULTICORE/LCCM644__MULTICORE__DS18B20/TEMPERATURE/ds18b20__temperature.c +++ b/FIRMWARE/COMMON_CODE/MULTICORE/LCCM644__MULTICORE__DS18B20/TEMPERATURE/ds18b20__temperature.c @@ -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*/ diff --git a/FIRMWARE/PROJECT_CODE/LCCM653__RLOOP__POWER_CORE/CHARGER/power_core__charger.c b/FIRMWARE/PROJECT_CODE/LCCM653__RLOOP__POWER_CORE/CHARGER/power_core__charger.c index 11803a0c..9a4e8ef5 100644 --- a/FIRMWARE/PROJECT_CODE/LCCM653__RLOOP__POWER_CORE/CHARGER/power_core__charger.c +++ b/FIRMWARE/PROJECT_CODE/LCCM653__RLOOP__POWER_CORE/CHARGER/power_core__charger.c @@ -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) { @@ -182,6 +187,7 @@ void vPWRNODE_CHG__Process(void) //clear the flag, we are done sPWRNODE.sTemp.u8NewTempAvail = 0U; +<<<<<<< HEAD #if 0 } else @@ -189,6 +195,13 @@ void vPWRNODE_CHG__Process(void) //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: diff --git a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/FLIGHT_CONTROLLER/LASER_ORIENTATION/fcu__laser_orientation.c b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/FLIGHT_CONTROLLER/LASER_ORIENTATION/fcu__laser_orientation.c index 5a255685..fbba4a73 100644 --- a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/FLIGHT_CONTROLLER/LASER_ORIENTATION/fcu__laser_orientation.c +++ b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/FLIGHT_CONTROLLER/LASER_ORIENTATION/fcu__laser_orientation.c @@ -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])) ); } @@ -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 } @@ -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 } @@ -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; } @@ -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; } @@ -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; diff --git a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__ethernet.c b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__ethernet.c index d97f6e3e..04676399 100644 --- a/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__ethernet.c +++ b/FIRMWARE/PROJECT_CODE/LCCM655__RLOOP__FCU_CORE/LASER_OPTO/fcu__laser_opto__ethernet.c @@ -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: