From f91eccb9b362e5e2048d40cb70c43d69fb368ec3 Mon Sep 17 00:00:00 2001 From: tcfshcrw <48719709+tcfshcrw@users.noreply.github.com> Date: Sun, 28 Jul 2024 11:05:56 +0800 Subject: [PATCH] Rudder implement 1. Add Rudder checkbox into Advanced area 2. Recommend using linear force curve for rudder 3. Use Rudder function in your own Risk 4. Add ESPNow comunication 5. I2C is a nogo 6. Displacement-calucation for Rudder is in Plan --- ESP32/include/ABSOscillation.h | 138 ++++++++ ESP32/include/DiyActivePedal_types.h | 9 +- ESP32/include/ESPNOW_lib.h | 95 ++++++ ESP32/include/I2CSync.h | 75 +++++ ESP32/include/Main.h | 9 +- ESP32/src/DiyActivePedal_types.cpp | 1 + ESP32/src/Main.cpp | 382 ++++++++++++++++++++++- SimHubPlugin/DataPluginDemo.cs | 91 +++++- SimHubPlugin/SettingsControlDemo.xaml | 12 +- SimHubPlugin/SettingsControlDemo.xaml.cs | 24 +- SimHubPlugin/bin/DiyActivePedal.dll | Bin 188928 -> 189440 bytes 11 files changed, 815 insertions(+), 21 deletions(-) create mode 100644 ESP32/include/ESPNOW_lib.h create mode 100644 ESP32/include/I2CSync.h diff --git a/ESP32/include/ABSOscillation.h b/ESP32/include/ABSOscillation.h index e41e725a..1a1dcb22 100644 --- a/ESP32/include/ABSOscillation.h +++ b/ESP32/include/ABSOscillation.h @@ -430,5 +430,143 @@ class Custom_vibration { _lastCallTimeMillis = timeNowMillis; + } +}; +MovingAverageFilter averagefilter_rudder(30); +MovingAverageFilter averagefilter_rudder_force(50); +class Rudder{ + public: + int32_t Center_offset; + int32_t offset_raw; + int32_t offset_filter; + int32_t stepper_range; + int32_t dead_zone_upper; + int32_t dead_zone_lower; + int32_t dead_zone; + int16_t sync_pedal_position; + int16_t current_pedal_position; + float endpos_travel; + float force_range; + float force_offset_raw; + float force_offset_filter; + float force_center_offset; + float position_ratio_sync; + float position_ratio_current; + int debug_count=0; + + void offset_calculate(DAP_calculationVariables_st* calcVars_st) + { + dead_zone=20; + Center_offset=calcVars_st->stepperPosRange/2; + dead_zone_upper=Center_offset+dead_zone/2; + dead_zone_lower=Center_offset-dead_zone/2; + sync_pedal_position=calcVars_st->sync_pedal_position; + current_pedal_position=calcVars_st->current_pedal_position; + stepper_range=calcVars_st->stepperPosRange; + force_range=calcVars_st->Force_Range; + force_center_offset=force_range/2; + if(calcVars_st->Rudder_status) + { + if(calcVars_st->pedal_type==1) + { + //pedal as brake + if(sync_pedal_position>dead_zone_upper) + { + offset_raw=-1*(sync_pedal_position-Center_offset)/65536*stepper_range; + } + else + { + offset_raw=0; + } + } + if(calcVars_st->pedal_type==2) + { + //pedal as gas + if(sync_pedal_position>dead_zone_upper) + { + offset_raw=-1*(sync_pedal_position-Center_offset)/65536*stepper_range; + } + else + { + offset_raw=0; + } + } + offset_filter=averagefilter_rudder.process(offset_raw+Center_offset); + } + else + { + offset_filter=0; + } + } + void force_offset_calculate(DAP_calculationVariables_st* calcVars_st) + { + dead_zone=20; + Center_offset=calcVars_st->stepperPosRange/2; + dead_zone_upper=Center_offset+dead_zone/2; + dead_zone_lower=Center_offset-dead_zone/2; + sync_pedal_position=calcVars_st->sync_pedal_position; + current_pedal_position=calcVars_st->current_pedal_position; + stepper_range=calcVars_st->stepperPosRange; + force_range=calcVars_st->Force_Range; + force_center_offset=force_range/2+calcVars_st->Force_Min; + endpos_travel=(float)calcVars_st->stepperPosRange; + //endpos_travel=((float)(calcVars_st->current_pedal_position-calcVars_st->stepperPosMin))/((float)calcVars_st->stepperPosRange); + position_ratio_sync=calcVars_st->Sync_pedal_position_ratio; + position_ratio_current=((float)(current_pedal_position-calcVars_st->stepperPosMin))/endpos_travel; + /* + if(calcVars_st->Rudder_status) + { + if(debug_count>2000) + { + Serial.print(calcVars_st->pedal_type); + Serial.print("--sync_ratio----:"); + Serial.print(position_ratio_sync); + Serial.print("---sync position---"); + Serial.println(sync_pedal_position); + debug_count=0; + } + else + { + debug_count++; + } + } + */ + + + float center_deadzone = 0.51; + if(calcVars_st->Rudder_status) + { + if(calcVars_st->pedal_type==1) + { + //pedal as brake + if(position_ratio_sync>center_deadzone) + { + force_offset_raw=(float)(-1*(position_ratio_sync-0.50)*force_range); + //Serial.println(force_offset_raw); + } + else + { + force_offset_raw=0; + } + } + if(calcVars_st->pedal_type==2) + { + //pedal as gas + if(position_ratio_sync>center_deadzone) + { + force_offset_raw=(float)(-1*(position_ratio_sync-0.50)*force_range); + //Serial.println(force_offset_raw); + } + else + { + force_offset_raw=0; + } + } + force_offset_filter=averagefilter_rudder_force.process(force_offset_raw+force_center_offset); + } + else + { + force_offset_filter=0; + } } }; \ No newline at end of file diff --git a/ESP32/include/DiyActivePedal_types.h b/ESP32/include/DiyActivePedal_types.h index 88d483fb..4352ce35 100644 --- a/ESP32/include/DiyActivePedal_types.h +++ b/ESP32/include/DiyActivePedal_types.h @@ -3,7 +3,7 @@ #include // define the payload revision -#define DAP_VERSION_CONFIG 137 +#define DAP_VERSION_CONFIG 138 // define the payload types #define DAP_PAYLOAD_TYPE_CONFIG 100 @@ -35,6 +35,7 @@ struct payloadPedalAction { uint8_t impact_value_u8; uint8_t Trigger_CV_1; uint8_t Trigger_CV_2; + uint8_t Rudder_action; }; @@ -245,6 +246,12 @@ struct DAP_calculationVariables_st float Force_Max_default; float WS_amp; float WS_freq; + bool Rudder_status; + uint8_t pedal_type; + uint16_t sync_pedal_position; + uint16_t current_pedal_position; + float current_pedal_position_ratio; + float Sync_pedal_position_ratio; void updateFromConfig(DAP_config_st& config_st); void updateEndstops(long newMinEndstop, long newMaxEndstop); diff --git a/ESP32/include/ESPNOW_lib.h b/ESP32/include/ESPNOW_lib.h new file mode 100644 index 00000000..ff05c189 --- /dev/null +++ b/ESP32/include/ESPNOW_lib.h @@ -0,0 +1,95 @@ +#include +#include +#include +#include "ESPNowW.h" +//#define ESPNow_debug +uint8_t Clu_mac[] = {0x36, 0x33, 0x33, 0x33, 0x33, 0x32}; +uint8_t Gas_mac[] = {0x36, 0x33, 0x33, 0x33, 0x33, 0x33}; +uint8_t Brk_mac[] = {0x36, 0x33, 0x33, 0x33, 0x33, 0x34}; +uint8_t* Recv_mac; +uint16_t ESPNow_send=0; +uint16_t ESPNow_recieve=0; +//bool MAC_get=false; +bool ESPNOW_status =false; +bool ESPNow_initial_status=false; +bool ESPNow_update= false; +//https://github.com/nickgammon/I2C_Anything/tree/master +struct ESPNow_Send_Struct +{ + uint16_t pedal_position; + float pedal_position_ratio; +}; +ESPNow_Send_Struct _ESPNow_Recv; +ESPNow_Send_Struct _ESPNow_Send; + +void onRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) +{ + /* + if(ESPNOW_status) + { + memcpy(&ESPNow_recieve, data, sizeof(ESPNow_recieve)); + ESPNow_update=true; + } + */ + if(ESPNOW_status) + { + memcpy(&_ESPNow_Recv, data, sizeof(_ESPNow_Recv)); + ESPNow_update=true; + } + +} +void OnSent(const uint8_t *mac_addr, esp_now_send_status_t status) +{ + +} +void ESPNow_initialize() +{ + + WiFi.mode(WIFI_MODE_STA); + Serial.println("Initializing Rudder, please wait"); + Serial.print("Current MAC Address: "); + Serial.println(WiFi.macAddress()); + if(dap_config_st.payLoadPedalConfig_.pedal_type==0) + { + esp_wifi_set_mac(WIFI_IF_STA, &Clu_mac[0]); + } + if(dap_config_st.payLoadPedalConfig_.pedal_type==1) + { + esp_wifi_set_mac(WIFI_IF_STA, &Brk_mac[0]); + } + if(dap_config_st.payLoadPedalConfig_.pedal_type==2) + { + esp_wifi_set_mac(WIFI_IF_STA, &Gas_mac[0]); + } + delay(300); + Serial.print("Modified MAC Address: "); + Serial.println(WiFi.macAddress()); + ESPNow.init(); + Serial.println("wait 10s for ESPNOW initialized"); + delay(10000); + + if(dap_config_st.payLoadPedalConfig_.pedal_type==1) + { + Recv_mac=Gas_mac; + } + + if(dap_config_st.payLoadPedalConfig_.pedal_type==2) + { + Recv_mac=Brk_mac; + } + if(ESPNow.add_peer(Recv_mac)== ESP_OK) + { + ESPNOW_status=true; + Serial.println("Sucess to add peer"); + } + else + { + ESPNOW_status=false; + Serial.println("Fail to add peer"); + } + ESPNow.reg_recv_cb(onRecv); + ESPNow.reg_send_cb(OnSent); + ESPNow_initial_status=true; + Serial.println("Rudder Initialized"); + +} diff --git a/ESP32/include/I2CSync.h b/ESP32/include/I2CSync.h new file mode 100644 index 00000000..e5b07dad --- /dev/null +++ b/ESP32/include/I2CSync.h @@ -0,0 +1,75 @@ +#include +#include +#include "Main.h" + +uint16_t I2C_Read; +#define I2C_rate 100000 +#define I2C_debug_out +//TwoWire I2C_sync(0); +bool I2C_sync_status=false; +bool I2C_data_read=false; +uint16_t I2C_send; +uint32_t iii = 0; + +//https://github.com/nickgammon/I2C_Anything/tree/master +template unsigned int I2C_writeAnything (const T& value) + { + Wire.write((byte *) &value, sizeof (value)); + return sizeof (value); + } // end of I2C_writeAnything + +template unsigned int I2C_readAnything(T& value) + { + byte * p = (byte*) &value; + unsigned int i; + for (i = 0; i < sizeof value; i++) + *p++ = Wire.read(); + return i; + } // end of I2C_readAnything + +void SynconReceive(int len) +{ + /* + Serial.printf("onReceive[%d]: ", len); + while (I2C_sync.available()) { + Serial.write(I2C_sync.read()); + } + Serial.println(); + */ + if(len>0) + { + I2C_readAnything(I2C_Read); + I2C_data_read=true; + } + + + //Serial.println(); + +} +void SynconRequest() +{ + /* + I2C_sync.print(iii++); + I2C_sync.print(" Packets."); + Serial.println("onRequest"); + Serial.println(); + */ + I2C_writeAnything(I2C_send); + //Wire.write(I2C_send); +} + +void I2C_initialize() +{ + Wire.begin(I2C_SDA,I2C_SCL,I2C_rate); + delay(3000); + Serial.println("Sync as Master"); +} + +void I2C_initialize_slave() +{ + Wire.onReceive(SynconReceive); + Wire.onRequest(SynconRequest); + Wire.begin((uint8_t)I2C_slave_address,I2C_SDA,I2C_SCL,I2C_rate); + //I2C_sync.begin(I2C_SDA,I2C_SCL,(uint8_t)I2C_slave_address); + Serial.println("Sync as Slave"); +} diff --git a/ESP32/include/Main.h b/ESP32/include/Main.h index e243a1d0..2344f74e 100644 --- a/ESP32/include/Main.h +++ b/ESP32/include/Main.h @@ -133,7 +133,10 @@ #define dirPinStepper 22 #define stepPinStepper 23 //analog output pin - #define D_O 25 + #define D_O 25 + //I2Cpins + #define I2C_SDA 32 + #define I2C_SCL 33 // endstop pins #define minPin 12 @@ -145,7 +148,9 @@ #define ISV57_RXPIN 26 // 16 #define Using_analog_output - + //#define Using_I2C_Sync + #define ESPNOW_Enable + #define I2C_slave_address 0x15 #define BLUETOOTH_GAMEPAD //#define USB_JOYSTICK #define SERIAL_COOMUNICATION_TASK_DELAY_IN_MS 1 diff --git a/ESP32/src/DiyActivePedal_types.cpp b/ESP32/src/DiyActivePedal_types.cpp index 58fa00c6..7f40dc14 100644 --- a/ESP32/src/DiyActivePedal_types.cpp +++ b/ESP32/src/DiyActivePedal_types.cpp @@ -181,6 +181,7 @@ void DAP_calculationVariables_st::updateFromConfig(DAP_config_st& config_st) { Force_Max = ((float)config_st.payLoadPedalConfig_.maxForce); Force_Range = Force_Max - Force_Min; Force_Max_default=((float)config_st.payLoadPedalConfig_.maxForce); + pedal_type=config_st.payLoadPedalConfig_.pedal_type; } void DAP_calculationVariables_st::dynamic_update() diff --git a/ESP32/src/Main.cpp b/ESP32/src/Main.cpp index c347dd11..290db36d 100644 --- a/ESP32/src/Main.cpp +++ b/ESP32/src/Main.cpp @@ -41,6 +41,8 @@ bool isv57LifeSignal_b = false; bool MCP_status =false; #endif + + //#define ALLOW_SYSTEM_IDENTIFICATION /**********************************************************************************************/ @@ -53,7 +55,8 @@ void pedalUpdateTask( void * pvParameters ); void serialCommunicationTask( void * pvParameters ); void servoCommunicationTask( void * pvParameters ); void OTATask( void * pvParameters ); - +void I2C_SyncTask( void * pvParameters); +void ESPNOW_SyncTask( void * pvParameters); // https://www.tutorialspoint.com/cyclic-redundancy-check-crc-in-arduino uint16_t checksumCalculator(uint8_t * data, uint16_t length) { @@ -93,6 +96,7 @@ WSOscillation _WSOscillation; Road_impact_effect _Road_impact_effect; Custom_vibration CV1; Custom_vibration CV2; +Rudder _rudder; #define ABS_OSCILLATION @@ -240,6 +244,20 @@ TaskHandle_t Task4; #endif +//I2C sync +#ifdef Using_I2C_Sync + + #include "I2CSync.h" + TaskHandle_t Task5; +#endif + +//ESPNOW +#ifdef ESPNOW_Enable + #include "ESPNOW_lib.h" + TaskHandle_t Task6; +#endif + + /**********************************************************************************************/ /* */ @@ -467,7 +485,7 @@ void setup() } } - + disableCore0WDT(); @@ -586,7 +604,105 @@ void setup() //MCP.begin(); } #endif + #ifdef Using_I2C_Sync + dap_calculationVariables_st.Rudder_status=false; + if(dap_config_st.payLoadPedalConfig_.pedal_type==1) + { + I2C_initialize(); + + } + if(dap_config_st.payLoadPedalConfig_.pedal_type==2) + { + I2C_initialize_slave(); + } + if(dap_config_st.payLoadPedalConfig_.pedal_type==0) + { + Serial.println("No sync with Clutch"); + } + if(dap_config_st.payLoadPedalConfig_.pedal_type>2) + { + Serial.println("Please send a config in and try again"); + } + // make the update as task + xTaskCreatePinnedToCore( + I2C_SyncTask, + "I2C_update_Task", + 3000, + //STACK_SIZE_FOR_TASK_2, + NULL, + 1, + &Task5, + 1); + delay(500); + + #endif + + //enable ESP-NOW + #ifdef ESPNOW_Enable + + if(dap_config_st.payLoadPedalConfig_.OTA_flag==0) + { + /* + WiFi.mode(WIFI_MODE_STA); + Serial.print("MAC Address: "); + Serial.println(WiFi.macAddress()); + if(dap_config_st.payLoadPedalConfig_.pedal_type==0) + { + esp_wifi_set_mac(WIFI_IF_STA, &Clu_mac[0]); + } + if(dap_config_st.payLoadPedalConfig_.pedal_type==1) + { + esp_wifi_set_mac(WIFI_IF_STA, &Brk_mac[0]); + } + if(dap_config_st.payLoadPedalConfig_.pedal_type==2) + { + esp_wifi_set_mac(WIFI_IF_STA, &Gas_mac[0]); + } + delay(300); + Serial.print("new MAC Address: "); + Serial.println(WiFi.macAddress()); + ESPNow.init(); + Serial.println("wait for ESPNOW initialized"); + delay(10000); + + if(dap_config_st.payLoadPedalConfig_.pedal_type==1) + { + Recv_mac=Gas_mac; + } + + if(dap_config_st.payLoadPedalConfig_.pedal_type==2) + { + Recv_mac=Brk_mac; + } + if(ESPNow.add_peer(Recv_mac)== ESP_OK) + { + ESPNOW_status=true; + Serial.println("Sucess to add peer"); + } + else + { + ESPNOW_status=false; + Serial.println("Fail to add peer"); + } + ESPNow.reg_recv_cb(onRecv); + ESPNow.reg_send_cb(OnSent); + */ + xTaskCreatePinnedToCore( + ESPNOW_SyncTask, + "ESPNOW_update_Task", + 3000, + //STACK_SIZE_FOR_TASK_2, + NULL, + 1, + &Task6, + 1); + delay(500); + + +} + + #endif Serial.println("Setup end"); @@ -774,6 +890,8 @@ void pedalUpdateTask( void * pvParameters ) _Road_impact_effect.forceOffset(&dap_calculationVariables_st, dap_config_st.payLoadPedalConfig_.Road_multi); CV1.forceOffset(dap_config_st.payLoadPedalConfig_.CV_freq_1,dap_config_st.payLoadPedalConfig_.CV_amp_1); CV2.forceOffset(dap_config_st.payLoadPedalConfig_.CV_freq_2,dap_config_st.payLoadPedalConfig_.CV_amp_2); + //_rudder.offset_calculate(&dap_calculationVariables_st); + _rudder.force_offset_calculate(&dap_calculationVariables_st); #endif //update max force with G force effect @@ -935,11 +1053,20 @@ void pedalUpdateTask( void * pvParameters ) //Add effect by force float effect_force=absForceOffset+ _BitePointOscillation.BitePoint_Force_offset+_WSOscillation.WS_Force_offset+CV1.CV_Force_offset+CV2.CV_Force_offset; - + effect_force=effect_force+_rudder.force_offset_filter; // use interpolation to determine local linearized spring stiffness double stepperPosFraction = stepper->getCurrentPositionFraction(); int32_t Position_Next = 0; + //Position_Next=_rudder.offset_filter; + + + + + + + + // select control loop algo if (dap_config_st.payLoadPedalConfig_.control_strategy_b <= 1) { @@ -951,6 +1078,9 @@ void pedalUpdateTask( void * pvParameters ) Position_Next = MoveByForceTargetingStrategy(filteredReading, stepper, &forceCurve, &dap_calculationVariables_st, &dap_config_st, effect_force, changeVelocity, stepper_vel_filtered_fl32, stepper_accel_filtered_fl32, d_phi_d_x, d_x_hor_d_phi); } + //add rudder + + @@ -984,7 +1114,7 @@ void pedalUpdateTask( void * pvParameters ) Position_Next +=absPosOffset; Position_Next = (int32_t)constrain(Position_Next, dap_calculationVariables_st.stepperPosMinEndstop, dap_calculationVariables_st.stepperPosMaxEndstop); - + dap_calculationVariables_st.current_pedal_position=Position_Next; //bitepoint trigger int32_t BP_trigger_value=dap_config_st.payLoadPedalConfig_.BP_trigger_value; @@ -1347,6 +1477,23 @@ void serialCommunicationTask( void * pvParameters ) Serial.write((char*)dap_config_st_local_ptr, sizeof(DAP_config_st)); Serial.print("\r\n"); } + if(dap_actions_st.payloadPedalAction_.Rudder_action==1) + { + if(dap_calculationVariables_st.Rudder_status==false) + { + dap_calculationVariables_st.Rudder_status=true; + Serial.println("Rudder on"); + //Serial.print("status:"); + //Serial.println(dap_calculationVariables_st.Rudder_status); + } + else + { + dap_calculationVariables_st.Rudder_status=false; + Serial.println("Rudder off"); + //Serial.print("status:"); + //Serial.println(dap_calculationVariables_st.Rudder_status); + } + } } @@ -1471,6 +1618,233 @@ void OTATask( void * pvParameters ) } } +//pedal I2C multitask +#ifdef Using_I2C_Sync +int I2C_count=0; +int error_count=0; +int print_count=0; +uint8_t error_out; +void I2C_SyncTask( void * pvParameters ) +{ + + for(;;) + { + //Serial.println("syncing"); + if(dap_calculationVariables_st.Rudder_status) + { + //Serial.println("Rudder status:1"); + if(I2C_count>100) + { + if(dap_config_st.payLoadPedalConfig_.pedal_type==1) + { + //Serial.println("syncing"); + + + Wire.beginTransmission((uint8_t)I2C_slave_address); + //I2C_sync.endTransmission(); + error_out=Wire.endTransmission(); + if(error_out==0) + { + //Serial.print("Start get I2C sync"); + Wire.beginTransmission((uint8_t)I2C_slave_address); + //uint8_t Pedal_position_u8=(uint8_t)(dap_state_basic_st.payloadPedalState_Basic_.pedalPosition_u16/65535*256); + //Wire.write(Pedal_position_u8); + I2C_writeAnything(dap_state_basic_st.payloadPedalState_Basic_.pedalPosition_u16); + delay(1); + Wire.endTransmission(); + //Serial.println(Pedal_position_u8); + //Serial.print("sended---------------Get--------------"); + + uint8_t bytesReceived=Wire.requestFrom((uint8_t)I2C_slave_address, 2); + //Serial.println(bytesReceived); + delay(1); + while(Wire.available()) + { + //uint8_t temp=Wire.read(); + uint16_t temp=0; + I2C_readAnything(temp); + dap_calculationVariables_st.sync_pedal_position=temp; + + + } + /* + if(print_count>300) + { + Serial.print("-------Read:----------"); + Serial.println(dap_calculationVariables_st.sync_pedal_position); + print_count=0; + } + print_count++; + */ + /* + if ((bool)bytesReceived) + { + + + uint8_t temp[bytesReceived]; + I2C_sync.readBytes(temp, bytesReceived); + log_print_buf(temp, bytesReceived); + + } + */ + + + //I2C_sync.write(Pedal_position_u8); + /* + I2C_sync.printf("Hello World! %lu", iii++); + delay(1); + I2C_sync.endTransmission(); + uint8_t bytesReceived = I2C_sync.requestFrom((uint8_t)I2C_slave_address, 16); + Serial.printf("requestFrom: %u\n", bytesReceived); + if ((bool)bytesReceived) { //If received more than zero bytes + uint8_t temp[bytesReceived]; + I2C_sync.readBytes(temp, bytesReceived); + log_print_buf(temp, bytesReceived); + } + */ + + + + + } + + } + if(dap_config_st.payLoadPedalConfig_.pedal_type==2) + { + + if(I2C_data_read) + { + + I2C_send=dap_state_basic_st.payloadPedalState_Basic_.pedalPosition_u16; + dap_calculationVariables_st.sync_pedal_position=I2C_Read; + I2C_data_read=false; + } + + /* + if(print_count>10) + { + Serial.print("Slave_Sync:"); + Serial.println(I2C_Read); + print_count=0; + } + print_count++; + */ + + + } + + + + I2C_count=0; + } + I2C_count=I2C_count+1; + } + #ifdef I2C_debug_out + if(print_count>30000000) + { + Serial.print("Rudder Status:"); + Serial.println(dap_calculationVariables_st.Rudder_status); + Serial.print("Pedal type:"); + Serial.println(dap_config_st.payLoadPedalConfig_.pedal_type); + Serial.print("---Sync Value--"); + Serial.println(dap_calculationVariables_st.sync_pedal_position); + Serial.print("---Send Value--"); + Serial.println(dap_calculationVariables_st.current_pedal_position); + if(error_out>0) + { + Serial.println("I2C failed"); + } + + + print_count=0; + } + else + { + print_count++; + } + #endif + + + + + } + +} +#endif + +#ifdef ESPNOW_Enable +int ESPNOW_count=0; +int error_count=0; +int print_count=0; +uint8_t error_out; +void ESPNOW_SyncTask( void * pvParameters ) +{ + for(;;) + { + if(ESPNOW_count>20) + { + if(dap_calculationVariables_st.Rudder_status) + { + if(ESPNow_initial_status==false) + { + ESPNow_initialize(); + } + else + { + dap_calculationVariables_st.current_pedal_position_ratio=((float)(dap_calculationVariables_st.current_pedal_position-dap_calculationVariables_st.stepperPosMin))/((float)dap_calculationVariables_st.stepperPosRange); + _ESPNow_Send.pedal_position_ratio=dap_calculationVariables_st.current_pedal_position_ratio; + _ESPNow_Send.pedal_position=dap_calculationVariables_st.current_pedal_position; + //ESPNow_send=dap_calculationVariables_st.current_pedal_position; + esp_err_t result = ESPNow.send_message(Recv_mac,(uint8_t *) &_ESPNow_Send,sizeof(_ESPNow_Send)); + if (result != ESP_OK) + { + Serial.println("Error sending the data"); + } + + if(ESPNow_update) + { + //dap_calculationVariables_st.sync_pedal_position=ESPNow_recieve; + dap_calculationVariables_st.sync_pedal_position=_ESPNow_Recv.pedal_position; + dap_calculationVariables_st.Sync_pedal_position_ratio=_ESPNow_Recv.pedal_position_ratio; + ESPNow_update=false; + } + ESPNOW_count=0; + } + } + } + else + { + ESPNOW_count++; + } + + #ifdef ESPNow_debug + if(print_count>1500) + { + Serial.print("Rudder Status:"); + Serial.println(dap_calculationVariables_st.Rudder_status); + Serial.print("Pedal type:"); + Serial.println(dap_config_st.payLoadPedalConfig_.pedal_type); + Serial.print("---Sync Value--"); + Serial.println(dap_calculationVariables_st.sync_pedal_position); + Serial.print("---Recieve Value--"); + Serial.println(_ESPNow_Recv.pedal_position_ratio); + Serial.print("---Send Value--"); + Serial.println(dap_calculationVariables_st.current_pedal_position); + + print_count=0; + } + else + { + print_count++; + + } + + + #endif + delay(1); + } +} +#endif #ifdef ISV_COMMUNICATION diff --git a/SimHubPlugin/DataPluginDemo.cs b/SimHubPlugin/DataPluginDemo.cs index 392d0c7b..91da7bd2 100644 --- a/SimHubPlugin/DataPluginDemo.cs +++ b/SimHubPlugin/DataPluginDemo.cs @@ -23,7 +23,7 @@ static class Constants { // payload revisiom - public const uint pedalConfigPayload_version = 137; + public const uint pedalConfigPayload_version = 138; // pyload types @@ -59,6 +59,7 @@ public struct payloadPedalAction public byte impact_value; public byte Trigger_CV_1; public byte Trigger_CV_2; + public byte Rudder_action; }; public struct payloadPedalState_Basic @@ -284,6 +285,9 @@ public class DIY_FFB_Pedal : IPlugin, IDataPlugin, IWPFSettingsV2 public string simhub_theme_color = "#7E87CEFA"; public uint debug_value = 0; public bool Rudder_enable_flag=false; + public bool clear_action = false; + public bool Rudder_status = false; + @@ -544,6 +548,7 @@ unsafe public void DataUpdate(PluginManager pluginManager, ref GameData data) tmp.payloadPedalAction_.impact_value = 0; tmp.payloadPedalAction_.Trigger_CV_1 = 0; tmp.payloadPedalAction_.Trigger_CV_2 = 0; + tmp.payloadPedalAction_.Rudder_action = 0; if (Settings.G_force_enable_flag[pedalIdx] == 1) { tmp.payloadPedalAction_.G_value = (Byte)g_force_last_value; @@ -734,6 +739,8 @@ unsafe public void DataUpdate(PluginManager pluginManager, ref GameData data) if (game_running_index == 1) { game_running_index = 0; + clear_action = true; + /* DAP_action_st tmp; tmp.payloadHeader_.version = (byte)Constants.pedalConfigPayload_version; tmp.payloadHeader_.payloadType = (byte)Constants.pedalActionPayload_type; @@ -744,6 +751,7 @@ unsafe public void DataUpdate(PluginManager pluginManager, ref GameData data) tmp.payloadPedalAction_.impact_value = 0; tmp.payloadPedalAction_.Trigger_CV_1 = 0; tmp.payloadPedalAction_.Trigger_CV_2 = 0; + tmp.payloadPedalAction_.Rudder_action = 0; rpm_last_value = 0; Road_impact_last = 0; debug_value = 0; @@ -764,6 +772,7 @@ unsafe public void DataUpdate(PluginManager pluginManager, ref GameData data) _serialPort[pedalIdx].Write(newBuffer, 0, newBuffer.Length); } } + */ } } @@ -782,6 +791,7 @@ unsafe public void DataUpdate(PluginManager pluginManager, ref GameData data) tmp.payloadPedalAction_.impact_value = 0; tmp.payloadPedalAction_.Trigger_CV_1 = 0; tmp.payloadPedalAction_.Trigger_CV_2 = 0; + tmp.payloadPedalAction_.Rudder_action = 0; DAP_action_st* v = &tmp; byte* p = (byte*)v; tmp.payloadFooter_.checkSum = checksumCalc(p, sizeof(payloadHeader) + sizeof(payloadPedalAction)); @@ -805,6 +815,85 @@ unsafe public void DataUpdate(PluginManager pluginManager, ref GameData data) _serialPort[1].Write(newBuffer, 0, newBuffer.Length); } } + if (Rudder_enable_flag) + { + if (Rudder_status == false) + { + Rudder_status = true; + } + else + { + Rudder_status = false; + } + DAP_action_st tmp; + tmp.payloadHeader_.version = (byte)Constants.pedalConfigPayload_version; + tmp.payloadHeader_.payloadType = (byte)Constants.pedalActionPayload_type; + tmp.payloadPedalAction_.triggerAbs_u8 = 1; + tmp.payloadPedalAction_.RPM_u8 = 0; + tmp.payloadPedalAction_.G_value = 128; + tmp.payloadPedalAction_.WS_u8 = 0; + tmp.payloadPedalAction_.impact_value = 0; + tmp.payloadPedalAction_.Trigger_CV_1 = 0; + tmp.payloadPedalAction_.Trigger_CV_2 = 0; + tmp.payloadPedalAction_.Rudder_action = 1; + DAP_action_st* v = &tmp; + byte* p = (byte*)v; + tmp.payloadFooter_.checkSum = checksumCalc(p, sizeof(payloadHeader) + sizeof(payloadPedalAction)); + int length = sizeof(DAP_action_st); + byte[] newBuffer = new byte[length]; + newBuffer = getBytes_Action(tmp); + for (uint PIDX = 1; PIDX < 3; PIDX++) + { + if (_serialPort[PIDX].IsOpen) + { + // clear inbuffer + _serialPort[PIDX].DiscardInBuffer(); + + // send query command + _serialPort[PIDX].Write(newBuffer, 0, newBuffer.Length); + } + Rudder_enable_flag = false; + System.Threading.Thread.Sleep(50); + } + + } + + if (clear_action) + { + + DAP_action_st tmp; + tmp.payloadHeader_.version = (byte)Constants.pedalConfigPayload_version; + tmp.payloadHeader_.payloadType = (byte)Constants.pedalActionPayload_type; + tmp.payloadPedalAction_.triggerAbs_u8 = 0; + tmp.payloadPedalAction_.RPM_u8 = 0; + tmp.payloadPedalAction_.G_value = 128; + tmp.payloadPedalAction_.WS_u8 = 0; + tmp.payloadPedalAction_.impact_value = 0; + tmp.payloadPedalAction_.Trigger_CV_1 = 0; + tmp.payloadPedalAction_.Trigger_CV_2 = 0; + tmp.payloadPedalAction_.Rudder_action = 0; + rpm_last_value = 0; + Road_impact_last = 0; + debug_value = 0; + DAP_action_st* v = &tmp; + byte* p = (byte*)v; + tmp.payloadFooter_.checkSum = checksumCalc(p, sizeof(payloadHeader) + sizeof(payloadPedalAction)); + int length = sizeof(DAP_action_st); + byte[] newBuffer = new byte[length]; + newBuffer = getBytes_Action(tmp); + for (uint pedalIdx = 0; pedalIdx < 3; pedalIdx++) + { + if (_serialPort[pedalIdx].IsOpen) + { + // clear inbuffer + _serialPort[pedalIdx].DiscardInBuffer(); + + // send query command + _serialPort[pedalIdx].Write(newBuffer, 0, newBuffer.Length); + } + } + clear_action = false; + } this.AttachDelegate("CurrentProfile", () => current_profile); diff --git a/SimHubPlugin/SettingsControlDemo.xaml b/SimHubPlugin/SettingsControlDemo.xaml index 4b042991..a700ab49 100644 --- a/SimHubPlugin/SettingsControlDemo.xaml +++ b/SimHubPlugin/SettingsControlDemo.xaml @@ -778,11 +778,11 @@