Team Fox
/
workinQM_5thJan_azad
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
workinQM_10thDec
by 
Team Fox
TCTM.cpp
- Committer:
- Bragadeesh153
- Date:
- 2016-04-13
- Revision:
- 12:af1d7e18b868
- Parent:
- 11:1fdb94ae6563
- Child:
- 13:fb7facaf308b
File content as of revision 12:af1d7e18b868:
#include "mbed.h"
#include "TCTM.h"
#include "crc.h"
#include "EPS.h"
#include "pin_config.h"
#include "FreescaleIAP.h"
#include "inttypes.h"
#include "iostream"
#include "stdint.h"
#include "cassert"
#include"math.h"
/*define the pins for digital out*/
extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
extern DigitalOut TRXY_SW_EN; //TR XY Switch if any TR_SW error arises then it is same as TR_SW_EN
extern DigitalOut TRZ_SW; //TR Z Switch
extern DigitalOut CDMS_RESET; // CDMS RESET
extern DigitalOut BCN_SW; //Beacon switch
extern uint8_t BCN_TX_STATUS;
extern uint8_t BCN_FEN;
extern BAE_HK_actual actual_data;
extern BAE_HK_min_max bae_HK_minmax;
extern uint32_t BAE_STATUS;
extern float data[6];
extern float moment[3];
extern uint8_t ACS_STATE;
extern DigitalOut EN_BTRY_HT;
extern DigitalOut phase_TR_x;
extern DigitalOut phase_TR_y;
extern DigitalOut phase_TR_z;
extern BAE_HK_quant quant_data;
//extern DigitalOut TRXY_SW_EN;
//extern DigitalOut TRZ_SW_EN; //same as TRZ_SW
extern uint32_t BAE_ENABLE;
//extern DigitalOut ACS_ACQ_DATA_ENABLE;
/*given a default value as of now shuld read value from flash and increment it write it back very time it starts(bae)*/
extern uint8_t BAE_RESET_COUNTER=0;
extern uint8_t BCN_FAIL_COUNT;
extern PwmOut PWM1; //x //Functions used to generate PWM signal
extern PwmOut PWM2; //y
extern PwmOut PWM3; //z //PWM output comes from pins p6
extern void F_ACS();
extern void F_BCN();
//extern void FCTN_ACS_GENPWM_MAIN();
extern void F_EPS();
extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]);
extern void FCTN_ATS_DATA_ACQ();
extern void FCTN_ACS_CNTRLALGO(float*,float*);
void FCTN_CONVERT_UINT (uint8_t input[2], float* output)
{
*output = (float) input[1];
*output = *output/100.0; //input[0] integer part
*output = *output + (float) input[0]; //input[1] decimal part correct to two decimal places
}
float angle(float x,float y,float z)
{
float mag_total=sqrt(x*x + y*y + z*z);
float cos_z = z/mag_total;
float theta_z = acosf(cos_z);
return theta_z;
//printf("/n cos_zz= %f /t theta_z= %f /n",cos_z,theta_z);
}
uint8_t* FCTN_BAE_TM_TC (uint8_t* tc)
{
uint8_t service_type=(tc[2]&0xF0);
uint8_t* tm;
uint16_t crc16;
switch(service_type)
{
case 0x60:
{
printf("Memory Management Service\r\n");
uint8_t service_subtype=(tc[2]&0x0F);
switch(service_subtype)
{
case 0x01:
{
printf("Read from Flash\r\n");
}
case 0x02:
{
uint16_t MID = ((uint16_t)tc[3] << 8) | tc[4];
switch(MID)
{
case 0x0001:
{
printf("Read from RAM\r\n");
/*taking some varible till we find some thing more useful*/
//uint8_t ref_val=0x01;
tm[0] = 0x60;
tm[1] = tc[0];
tm[2] = ACK_CODE;
/*random or with bcn_tx_sw_enable assuming it is 1 bit in length
how to get that we dont know, now we just assume it to be so*/
tm[3] = (BCN_SW);
tm[3] = tm[3]|(TRXY_SW_EN<<1);
tm[3] = tm[3]|(TRZ_SW<<2);
tm[3] = tm[3]|(ATS1_SW_ENABLE<<3);
tm[3] = tm[3]|(ATS2_SW_ENABLE<<4);
if(BCN_TX_STATUS==2)
tm[3] = tm[3]|0x20;
else
tm[3] = tm[3] & 0xDF;
tm[3] = tm[3]|(BCN_FEN<<6);
uint8_t mask_val=BAE_ENABLE & 0x00000008;
/*can be a problem see if any error occurs*/
tm[3] = tm[3]|(mask_val<<7);
/*not included in the code yet*/
tm[4] = BAE_RESET_COUNTER;
tm[5] = ACS_STATE;
tm[5] = tm[5]|(EN_BTRY_HT<<3);
tm[5] = tm[5]|(phase_TR_x<<4);
tm[5] = tm[5]|(phase_TR_y<<5);
tm[5] = tm[5]|(phase_TR_z<<6);
/*spare to be fixed*/
//tm[5] = tm[5]|(Spare))<<7);
/**/
uint8_t soc_powerlevel_2=50;
uint8_t soc_powerlevel_3=65;
tm[6] = soc_powerlevel_2;
tm[7] = soc_powerlevel_3;
/*to be fixed*/
tm[8] = 0;
tm[9] = 0;
tm[10] = 0;
tm[11] = 0;
//tm[8] = Torque Rod X Offset;
//tm[9] = Torque Rod Y Offset;
//tm[10] = Torque Rod Z Offset;
//tm[11] = ACS_DEMAG_TIME_DELAY;
tm[12] = (BAE_STATUS>>24) & 0xFF;
tm[13] = (BAE_STATUS>>16) & 0xFF;
tm[14] = (BAE_STATUS>>8) & 0xFF;
tm[15] = BAE_STATUS & 0xFF;
/*to be fixed*/
tm[16] = BCN_FAIL_COUNT;
tm[17] = actual_data.power_mode;
/*to be fixed*/
uint16_t P_BAE_I2CRX_COUNTER=0;
uint16_t P_ACS_MAIN_COUNTER=0;
uint16_t FCTN_BCN_TX_MAIN_COUNTER=0;
uint16_t P_EPS_MAIN_COUNTER=0;
tm[18] = P_BAE_I2CRX_COUNTER>>8;
tm[19] = P_BAE_I2CRX_COUNTER;
tm[20] = P_ACS_MAIN_COUNTER>>8;
tm[21] = P_ACS_MAIN_COUNTER;
tm[22] = FCTN_BCN_TX_MAIN_COUNTER>>8;
tm[23] = FCTN_BCN_TX_MAIN_COUNTER;
tm[24] = P_EPS_MAIN_COUNTER>>8;
tm[25] = P_EPS_MAIN_COUNTER;
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[26+ (i*4)]);
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[38+(i*4)]);
//FAULT_FLAG();
tm[50] = actual_data.faultIr_status;
tm[51] = actual_data.faultPoll_status;
//Bdot Rotation Speed of Command tm[52-53]
//Bdot Output Current tm[54]
//float l_pmw1 = (PWM1);
//float l_pmw2 = PWM2;
//float l_pmw3 = PWM3;
/*__________________________________________________________________*/
/*change and check if changing it to PWM1 causes problem*/
/*___________________________________________________________________*/
float PWM_measured[3];
PWM_measured[0] = PWM1.read();
PWM_measured[1] = PWM2.read();
PWM_measured[2] = PWM3.read();
FCTN_CONVERT_FLOAT(PWM_measured[0], &tm[55]);
FCTN_CONVERT_FLOAT(PWM_measured[1], &tm[59]);
FCTN_CONVERT_FLOAT(PWM_measured[2], &tm[63]);
float attitude_ang = angle(actual_data.Bvalue_actual[0],actual_data.Bvalue_actual[1],actual_data.Bvalue_actual[2]);
FCTN_CONVERT_FLOAT(attitude_ang, &tm[67]);
for (int i=0; i<16; i++)
tm[68+i] = quant_data.voltage_quant[i];
for (int i=0; i<12; i++)
tm[84+i] = quant_data.current_quant[i];
//tm[96]
//tm[97]
//tm[98]
//tm[99]
tm[100] = quant_data.Batt_voltage_quant;
tm[101] = quant_data.BAE_temp_quant;
tm[102] = quant_data.Batt_gauge_quant[1];
tm[103] = quant_data.Batt_temp_quant[0];
tm[104] = quant_data.Batt_temp_quant[1];
//tm[105] = beacon temperature;
for (int i=105; i<132;i++)
{
tm[i] = 0x00;
}
crc16 = CRC::crc16_gen(tm,132);
tm[132] = (uint8_t)((crc16&0xFF00)>>8);
tm[133] = (uint8_t)(crc16&0x00FF);
return tm;
}
case 0x0002:
{
tm[0] = 0x60;
tm[1] = tc[0];
tm[2] = ACK_CODE;
for(int i;i<16;i++)
tm[i+3] = bae_HK_minmax.voltage_max[i];
for(int i;i<12;i++)
tm[i+18] = bae_HK_minmax.current_max[i];
tm[29] = bae_HK_minmax.Batt_voltage_max;;
tm[30] = bae_HK_minmax.BAE_temp_max;
/*battery soc*/
//tm[31] = BAE_HK_min_max bae_HK_minmax.voltage_max;
tm[32] = bae_HK_minmax.Batt_temp_max[1];
tm[33] = bae_HK_minmax.Batt_temp_max[2];
/*BCN temp not there*/
//tm[34] = BAE_HK_min_max bae_HK_minmax.;
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[i],&tm[35+(i*4)]);
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[i],&tm[47+(i*4)]);
/*min data*/
for(int i;i<16;i++)
tm[i+59] = bae_HK_minmax.voltage_min[i];
for(int i;i<12;i++)
tm[i+74] = bae_HK_minmax.current_min[i];
tm[86] = bae_HK_minmax.Batt_voltage_min;
tm[87] = bae_HK_minmax.BAE_temp_min;
/*battery soc*/
//tm[88] = BAE_HK_min_max bae_HK_minmax.voltage_max;
tm[89] = bae_HK_minmax.Batt_temp_min[1];
tm[90] = bae_HK_minmax.Batt_temp_min[2];
//huhu//
/*BCN temp not there*/
//tm[91] = BAE_HK_min_max bae_HK_minmax.;
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[i],&tm[91+(i*4)]);
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[i],&tm[103+(i*4)]);
for (int i=115; i<132;i++)
{
tm[i] = 0x00;
}
crc16 = CRC::crc16_gen(tm,132);
tm[132] = (uint8_t)((crc16&0xFF00)>>8);
tm[133] = (uint8_t)(crc16&0x00FF);
return tm;
}
}
}
/*
switch(tc[3])
{
case 0x01:
{
printf("Read MUX DATA\r\n");
tm[0] = 0x60;
tm[1] = tc[0];
tm[2] = ACK_CODE;
for(int i=0; i<16; i++) //16*4 = 64 bytes //tm[4] to tm[67] filled
FCTN_CONVERT_FLOAT(actual_data.voltage_actual[i], &tm[4+(i*4)]);
for(int i=0; i<12; i++) //12*4 = 48 //tm[68] to tm[115] filled
FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&tm[68 + (i*4)]);
for (int i=116; i<132;i++)
{
tm[i] = 0x00;
}
crc16 = CRC::crc16_gen(tm,132);
tm[132] = (uint8_t)((crc16&0xFF00)>>8);
tm[133] = (uint8_t)(crc16&0x00FF);
return tm;
}
case 0x02:
{
printf("Read HK\r\n");
tm[0] = 0x60;
tm[1] = tc[0];
tm[2] = ACK_CODE;
FCTN_CONVERT_FLOAT(actual_data.Batt_temp_actual[0],&tm[4]); //tm[4]-tm[7]
FCTN_CONVERT_FLOAT(actual_data.Batt_temp_actual[1],&tm[8]); //tm[8]- tm[11]
for(int i=0; i<4; i++)
FCTN_CONVERT_FLOAT(actual_data.Batt_gauge_actual[i],&tm[12+(i*4)]); //tm[12] - tm[27]
FCTN_CONVERT_FLOAT(actual_data.BAE_temp_actual,&tm[28]); //tm[28] - tm[31]
tm[32] = (uint8_t)actual_data.power_mode;
tm[33] = actual_data.faultPoll_status;
tm[34] = actual_data.faultIr_status;
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[35+(i*4)]); //35 -46
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[47+(i*4)]); //47 -58
FCTN_CONVERT_FLOAT(actual_data.Batt_voltage_actual,&tm[59]); //59 - 62
for (int i=63; i<132;i++)
{
tm[i] = 0x00;
}
crc16 = CRC::crc16_gen(tm,132);
tm[132] = (uint8_t)((crc16&0xFF00)>>8);
tm[133] = (uint8_t)(crc16&0x00FF);
return tm;
}
case 0x03:
{
printf("Read min max data");
tm[0] = 0x60;
tm[1] = tc[0];
tm[2] = ACK_CODE;
for(int i=4; i<20; i++)
tm[i] = (uint8_t)bae_HK_minmax.voltage_max[i-4];
for(int i=20; i<32; i++)
tm[i] = (uint8_t)bae_HK_minmax.current_max[i-20];
tm[32] = (uint8_t)bae_HK_minmax.Batt_temp_max[0];
tm[33] = (uint8_t)bae_HK_minmax.Batt_temp_max[1];
tm[34] = (uint8_t)bae_HK_minmax.Batt_gauge_max[0];
tm[35] = (uint8_t)bae_HK_minmax.Batt_gauge_max[1];
tm[36] = (uint8_t)bae_HK_minmax.Batt_gauge_max[2];
tm[37] = (uint8_t)bae_HK_minmax.BAE_temp_max;
FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[0],&tm[38]); //tm[38] - tm[41]
FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[1],&tm[42]); //tm[42] - tm[45]
FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_max[2],&tm[46]); //tm[46] - tm[49]
FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[0],&tm[50]); //tm[50] - tm[53]
FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[1],&tm[54]); //tm[54] - tm[57]
FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_max[2],&tm[58]); //tm[58] - tm[61]
tm[62] = (uint8_t)bae_HK_minmax.Bvalue_max[0];
tm[63] = (uint8_t)bae_HK_minmax.Bvalue_max[1];
tm[64] = (uint8_t)bae_HK_minmax.Bvalue_max[2];
tm[65] = (uint8_t)bae_HK_minmax.Batt_voltage_max;
for(int i=66; i<82; i++)
tm[i] = (uint8_t)bae_HK_minmax.voltage_min[i-66];
for(int i=82; i<94; i++)
tm[i] = (uint8_t)bae_HK_minmax.current_min[i-82];
tm[94] = (uint8_t)bae_HK_minmax.Batt_temp_min[0];
tm[95] = (uint8_t)bae_HK_minmax.Batt_temp_min[1];
tm[96] = (uint8_t)bae_HK_minmax.Batt_gauge_min[0];
tm[97] = (uint8_t)bae_HK_minmax.Batt_gauge_min[1];
tm[98] = (uint8_t)bae_HK_minmax.Batt_gauge_min[2];
tm[99] = (uint8_t)bae_HK_minmax.BAE_temp_min;
FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[0],&tm[100]); //tm[100] - tm[103]
FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[1],&tm[104]); //tm[104] - tm[107]
FCTN_CONVERT_FLOAT(bae_HK_minmax.AngularSpeed_min[2],&tm[108]); //tm[108] - tm[111]
FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[0],&tm[112]); //tm[112] - tm[115]
FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[1],&tm[116]); //tm[116] - tm[119]
FCTN_CONVERT_FLOAT(bae_HK_minmax.Bvalue_min[2],&tm[120]); //tm[120] - tm[123]
tm[124] = (uint8_t)bae_HK_minmax.Batt_voltage_min;
for (int i=125; i<132;i++)
{
tm[i] = 0x00;
}
crc16 = CRC::crc16_gen(tm,132);
tm[132] = (uint8_t)((crc16&0xFF00)>>8);
tm[133] = (uint8_t)(crc16&0x00FF);
return tm;
}
case 0x04:
{
printf("Read status");
tm[0] = 0x60;
tm[1] = tc[0];
tm[2] = ACK_CODE;
tm[4] = (BAE_STATUS>>24) & 0xFF;
tm[5] = (BAE_STATUS>>16) & 0xFF;
tm[6] = (BAE_STATUS>>8) & 0xFF;
tm[7] = BAE_STATUS & 0xFF;
for (int i=8; i<132;i++)
{
tm[i] = 0x00;
}
crc16 = CRC::crc16_gen(tm,132);
tm[132] = (uint8_t)((crc16&0xFF00)>>8);
tm[133] = (uint8_t)(crc16&0x00FF);
return tm;
}
}
*/
case 0x05:
{
printf("Write on Flash\r\n");
}
default:
{
printf("Invalid TC");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
}
}
case 0x80:
{
printf("Function Management Service\r\n");
uint8_t service_subtype=(tc[2]&0x0F);
switch(service_subtype)
{
case 0x01:
{
printf("FMS Activated\r\n");
uint8_t pid=tc[3];
switch(pid)
{
case 0xE0:
{
float B[3],W[3];
printf("ACS_COMSN\r\n");
//ACK_L234_TM
uint8_t B_x[2];
uint8_t B_y[2];
uint8_t B_z[2];
uint8_t W_x[2];
uint8_t W_y[2];
uint8_t W_z[2];
B_x[0]=tc[3];
B_x[1]=tc[4];
B_y[0]=tc[5];
B_y[1]=tc[6];
B_z[0]=tc[7];
B_z[1]=tc[8];
W_x[0]=tc[9];
W_x[1]=tc[10];
W_y[0]=tc[11];
W_y[1]=tc[12];
W_z[0]=tc[13];
W_z[1]=tc[14];
FCTN_CONVERT_UINT(B_x,&B[0]);
FCTN_CONVERT_UINT(B_y,&B[1]);
FCTN_CONVERT_UINT(B_z,&B[2]);
FCTN_CONVERT_UINT (W_x, &W[0]);
FCTN_CONVERT_UINT (W_y, &W[1]);
FCTN_CONVERT_UINT (W_z, &W[2]);
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
//FCTN_ATS_DATA_ACQ(); //get data
printf("gyro values\n\r");
for(int i=0; i<3; i++)
printf("%f\n\r",W[i]);
printf("mag values\n\r");
for(int i=0; i<3; i++)
printf("%f\n\r",B[i]);
/* FCTN_CONVERT_FLOAT(data[0],&tm[4]); //tm[4] - tm[7]
FCTN_CONVERT_FLOAT(data[1],&tm[8]); //tm[8] - tm[11]
FCTN_CONVERT_FLOAT(data[2],&tm[12]); //tm[12] - tm[15]
FCTN_CONVERT_FLOAT(data[0],&tm[16]); //tm[16] - tm[19]
FCTN_CONVERT_FLOAT(data[1],&tm[20]); //tm[20] - tm[23]
FCTN_CONVERT_FLOAT(data[2],&tm[24]); //tm[24] - tm[27]
if((data[0]<8) && (data[1]<8) && (data[2] <8))
tm[28] = 1; // gyro values in correct range
else
tm[28] = 0;
if ((data[3] > 20 ) && (data[4] >20) && (data[5]>20)&& (data[3] < 50 ) && (data[4] <50) && (data[5]<50))
tm[29] = 1; // mag values in correct range
else
tm[29] = 0;
*/
// float B[3],W[3];
// B[0] = B0;
// B[1] = B1;
// B[2] = B2;
// W[0] = W0;
// W[1] = W1;
// W[2] = W2;
// Control algo commissioning
/* FCTN_ACS_CNTRLALGO(B,W);
FCTN_CONVERT_FLOAT(moment[0],&tm[30]); //tm[30] - tm[33]
FCTN_CONVERT_FLOAT(moment[1],&tm[34]); //tm[34] - tm[37]
FCTN_CONVERT_FLOAT(moment[2],&tm[38]); //tm[38] - tm[41]
// to include commission TR as well
for(uint8_t i=42;i<132;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,132);
tm[133] = (uint8_t)((crc16&0xFF00)>>8);
tm[134] = (uint8_t)(crc16&0x00FF);
return tm;
*/
// Control algo commissioning
FCTN_ACS_CNTRLALGO(B,W);
FCTN_CONVERT_FLOAT(moment[0],&tm[4]); //tm[4] - tm[7]
FCTN_CONVERT_FLOAT(moment[1],&tm[8]); //tm[8] - tm[11]
FCTN_CONVERT_FLOAT(moment[2],&tm[12]); //tm[12] - tm[15]
// to include commission TR as well
for(uint8_t i=16;i<132;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,132);
tm[133] = (uint8_t)((crc16&0xFF00)>>8);
tm[134] = (uint8_t)(crc16&0x00FF);
return tm;
}
case 0xE1:
{
float moment_tc[3];
printf("HARDWARE_COMSN\r\n");
//ACK_L234_TM
uint8_t M0[2];
uint8_t M1[2];
uint8_t M2[2];
M0[0]=tc[3];
M0[1]=tc[4];
M1[0]=tc[5];
M1[1]=tc[6];
M2[0]=tc[7];
M2[1]=tc[8];
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
FCTN_CONVERT_UINT(M0,&moment_tc[0]);
FCTN_CONVERT_UINT(M1, &moment_tc[1]);
FCTN_CONVERT_UINT(M2, &moment_tc[2]);
FCTN_ACS_GENPWM_MAIN(moment_tc);
float PWM_measured[3];
PWM_measured[0] = PWM1.read();
PWM_measured[1] = PWM2.read();
PWM_measured[2] = PWM3.read();
FCTN_CONVERT_FLOAT(PWM_measured[0],&tm[4]); //4-7
FCTN_CONVERT_FLOAT(PWM_measured[1],&tm[8]); //8-11
FCTN_CONVERT_FLOAT(PWM_measured[2],&tm[12]); //12-15
for(int i=0; i<12; i++)
FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&tm[16 + (i*4)]);
FCTN_ATS_DATA_ACQ(); //get data
// to include commission TR as well
for(uint8_t i=64;i<132;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,132);
tm[133] = (uint8_t)((crc16&0xFF00)>>8);
tm[134] = (uint8_t)(crc16&0x00FF);
return tm;
}
case 0x02:
{
F_EPS();
/* printf("Run P_EPS_MAIN\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
*/
for(uint8_t i=0;i<133;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,132);
tm[132] = (uint8_t)((crc16&0xFF00)>>8);
tm[133] = (uint8_t)(crc16&0x00FF);
/*
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
*/
return tm;
}
/* case 0x03:
{
/* printf("Run P_ACS_INIT\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x04:
{
printf("Run P_ACS_ACQ_DATA\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
*/
case 0x05:
{
printf("Run P_ACS_MAIN\r\n");
F_ACS();
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[i],&tm[(i*4)]);
for(int i=0; i<3; i++)
FCTN_CONVERT_FLOAT(actual_data.AngularSpeed_actual[i],&tm[12+(i*4)]);
tm[24] = ACS_STATE;
tm[24] = tm[5]|(EN_BTRY_HT<<3);
tm[24] = tm[5]|(phase_TR_x<<4);
tm[24] = tm[5]|(phase_TR_y<<5);
tm[24] = tm[5]|(phase_TR_z<<6);
/*___________________change / check pwm working__________________________________*/
FCTN_CONVERT_FLOAT(PWM1,&tm[25]);
FCTN_CONVERT_FLOAT(PWM2,&tm[29]);
FCTN_CONVERT_FLOAT(PWM3,&tm[33]);
//ACK_L234_TM
/* tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
*/
crc16 = CRC::crc16_gen(tm,37);
tm[37] = (uint8_t)((crc16&0xFF00)>>8);
tm[38] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=39;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x06:
{
F_BCN();
/* printf("Run FCTN_BCN_INIT\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
*/
crc16 = CRC::crc16_gen(tm,0);
tm[0] = (uint8_t)((crc16&0xFF00)>>8);
tm[1] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=2;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
/*
case 0x07:
{
printf("Run FCTN_BCN_TX_MAIN\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}*/
case 0x11:
{
printf("SW_ON_ACS_ATS1_SW_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
ATS1_SW_ENABLE = 1; // making sure we switch off the other
ATS1_SW_ENABLE = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x12:
{
printf("SW_ON_ACS_ATS2_SW_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
ATS1_SW_ENABLE = 1; //make sure u switch off the other
ATS2_SW_ENABLE = 0;
tm[2]=1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x13:
{
printf("SW_ON_ACS_TR_XY_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
TRXY_SW_EN = 1;
tm[2]=1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x14:
{
printf("SW_ON_ACS_TR_Z_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
TRZ_SW = 1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x15:
{
printf("SW_ON_BCN_TX_SW_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
BCN_SW = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x21:
{
printf("SW_OFF_ACS_ATS1_SW_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
ATS1_SW_ENABLE = 1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x22:
{
printf("SW_OFF_ACS_ATS2_SW_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
ATS2_SW_ENABLE = 1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x23:
{
printf("SW_OFF_ACS_TR_XY_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
TRXY_SW_EN= 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x24:
{
printf("SW_OFF_ACS_TR_Z_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
TRZ_SW = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x25:
{
printf("SW_OFF_BCN_TX_SW_ENABLE\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
BCN_SW = 1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x31:
{
printf("ACS_ATS1_SW_RESET\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
ATS1_SW_ENABLE = 1;
wait_us(1);
ATS1_SW_ENABLE = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x32:
{
printf("BCN_SW_RESET\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
BCN_SW = 1;
wait_us(1);
BCN_SW = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x33:
{
printf("ACS_ATS2_SW_RESET\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
ATS1_SW_ENABLE = 1;
wait_us(1);
ATS1_SW_ENABLE = 0;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
case 0x34:
{
printf("CDMS_SW_RESET\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=1;
CDMS_RESET = 0;
wait_us(1);
CDMS_RESET = 1;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
default:
{
printf("Invalid TC\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
}
default:
{
printf("Invalid TC\r\n");
//ACK_L234_TM
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
for(uint8_t i=3;i<11;i++)
{
tm[i]=0x00;
}
crc16 = CRC::crc16_gen(tm,11);
tm[11] = (uint8_t)((crc16&0xFF00)>>8);
tm[12] = (uint8_t)(crc16&0x00FF);
for(uint8_t i=13;i<134;i++)
{
tm[i]=0x00;
}
return tm;
}
}
}
}
}
}
int strt_add = flash_size() - (2*SECTOR_SIZE);
uint32_t flasharray[8]; //256+(3*1024)
char *nativeflash = (char*)strt_add;
/*Writing to the Flash*/
void FCTN_CDMS_WR_FLASH(uint16_t j,uint32_t fdata) //j-position to write address ; fdata - flash data to be written
{
for(int i=0;i<8;i++)
{
flasharray[i]=nativeflash[i];
}
flasharray[j]=fdata;
erase_sector(strt_add);
program_flash(strt_add, (char*)&flasharray,4*8);
}
/*End*/
/*Reading from Flash*/
uint32_t FCTN_CDMS_RD_FLASH(uint16_t j)
{
for(int i=0;i<8;i++)
{
flasharray[i]=nativeflash[i];
}
return flasharray[j];
}
/*End*/
// Convert float to 4 uint8_t
void FCTN_CONVERT_FLOAT(float input, uint8_t output[4])
{
assert(sizeof(float) == sizeof(uint32_t));
uint32_t* temp = reinterpret_cast<uint32_t*>(&input);
//float* output1 = reinterpret_cast<float*>(temp);
printf("\n\r %f ", input);
std::cout << "\n\r uint32"<<*temp << std::endl;
output[0] =(uint8_t )(((*temp)>>24)&0xFF);
output[2] =(uint8_t ) (((*temp)>>16)&0xFF);
output[1] =(uint8_t ) (((*temp)>>8)&0xFF);
output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic
//printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]);
//std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl;
}