From fd52ff923d5817ebe0b1b5916d5ce424d08bfb37 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 29 Sep 2022 14:54:05 +1000 Subject: [PATCH] AP_EFI: removed array from cylinder_status this array was never used and just makes doing drivers harder. Removed to reduce complexity --- libraries/AP_EFI/AP_EFI.cpp | 37 +++++++++++------------ libraries/AP_EFI/AP_EFI_Currawong_ECU.cpp | 9 ++---- libraries/AP_EFI/AP_EFI_DroneCAN.cpp | 2 +- libraries/AP_EFI/AP_EFI_NWPMU.cpp | 8 ++--- libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp | 6 ++-- libraries/AP_EFI/AP_EFI_Serial_MS.cpp | 10 +++--- libraries/AP_EFI/AP_EFI_State.h | 3 +- 7 files changed, 34 insertions(+), 41 deletions(-) diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index c94dc871c2..b422870ae9 100644 --- a/libraries/AP_EFI/AP_EFI.cpp +++ b/libraries/AP_EFI/AP_EFI.cpp @@ -213,7 +213,6 @@ void AP_EFI::log_status(void) uint8_t(state.spark_plug_usage), uint8_t(state.ecu_index)); - for (uint8_t i = 0; i < ENGINE_MAX_CYLINDERS; i++) { // @LoggerMessage: ECYL // @Description: EFI per-cylinder information // @Field: TimeUS: Time since system startup @@ -224,20 +223,19 @@ void AP_EFI::log_status(void) // @Field: EGT: Exhaust gas temperature // @Field: Lambda: Estimated lambda coefficient (dimensionless ratio) // @Field: IDX: Index of the publishing ECU - AP::logger().WriteStreaming("ECYL", - "TimeUS,Inst,IgnT,InjT,CHT,EGT,Lambda,IDX", - "s#dsOO--", - "F-0C0000", - "QBfffffB", - AP_HAL::micros64(), - i, - state.cylinder_status[i].ignition_timing_deg, - state.cylinder_status[i].injection_time_ms, - state.cylinder_status[i].cylinder_head_temperature, - state.cylinder_status[i].exhaust_gas_temperature, - state.cylinder_status[i].lambda_coefficient, - state.ecu_index); - } + AP::logger().WriteStreaming("ECYL", + "TimeUS,Inst,IgnT,InjT,CHT,EGT,Lambda,IDX", + "s#dsOO--", + "F-0C0000", + "QBfffffB", + AP_HAL::micros64(), + 0, + state.cylinder_status.ignition_timing_deg, + state.cylinder_status.injection_time_ms, + state.cylinder_status.cylinder_head_temperature, + state.cylinder_status.exhaust_gas_temperature, + state.cylinder_status.lambda_coefficient, + state.ecu_index); } #endif // LOGGING_ENABLED @@ -262,10 +260,10 @@ void AP_EFI::send_mavlink_status(mavlink_channel_t chan) state.atmospheric_pressure_kpa, state.intake_manifold_pressure_kpa, KELVIN_TO_C(state.intake_manifold_temperature), - KELVIN_TO_C(state.cylinder_status[0].cylinder_head_temperature), - state.cylinder_status[0].ignition_timing_deg, - state.cylinder_status[0].injection_time_ms, - 0, // exhaust gas temperature + KELVIN_TO_C(state.cylinder_status.cylinder_head_temperature), + state.cylinder_status.ignition_timing_deg, + state.cylinder_status.injection_time_ms, + state.cylinder_status.exhaust_gas_temperature, 0, // throttle out 0, // pressure/temperature compensation 0 // ignition voltage (spark supply voltage) @@ -286,7 +284,6 @@ void AP_EFI::handle_scripting(const EFI_State &efi_state) if (!backend || (Type(type.get()) != Type::SCRIPTING)) { return; } - backend->handle_scripting(efi_state); } diff --git a/libraries/AP_EFI/AP_EFI_Currawong_ECU.cpp b/libraries/AP_EFI/AP_EFI_Currawong_ECU.cpp index 2d9e957f91..091537807c 100644 --- a/libraries/AP_EFI/AP_EFI_Currawong_ECU.cpp +++ b/libraries/AP_EFI/AP_EFI_Currawong_ECU.cpp @@ -84,16 +84,13 @@ bool AP_EFI_Currawong_ECU::handle_message(AP_HAL::CANFrame &frame) } else if (decodeECU_TelemetrySlow0PacketStructure(&frame, &telemetry_slow0)) { internal_state.intake_manifold_pressure_kpa = telemetry_slow0.map; internal_state.atmospheric_pressure_kpa = telemetry_slow0.baro; - internal_state.cylinder_status[0].cylinder_head_temperature = C_TO_KELVIN(telemetry_slow0.cht); + internal_state.cylinder_status.cylinder_head_temperature = C_TO_KELVIN(telemetry_slow0.cht); } else if (decodeECU_TelemetrySlow1PacketStructure(&frame, &telemetry_slow1)) { internal_state.intake_manifold_temperature = C_TO_KELVIN(telemetry_slow1.mat); internal_state.fuel_pressure = telemetry_slow1.fuelPressure; } else if (decodeECU_TelemetrySlow2PacketStructure(&frame, &telemetry_slow2)) { - internal_state.cylinder_status[0].ignition_timing_deg = telemetry_slow2.ignAngle1; - if (ENGINE_MAX_CYLINDERS > 1) { - internal_state.cylinder_status[1].ignition_timing_deg = telemetry_slow2.ignAngle2; - } - + internal_state.cylinder_status.ignition_timing_deg = telemetry_slow2.ignAngle1; + internal_state.fuel_consumption_rate_cm3pm = telemetry_slow2.flowRate / KG_PER_M3_TO_G_PER_CM3(get_ecu_fuel_density()); } else { valid = false; diff --git a/libraries/AP_EFI/AP_EFI_DroneCAN.cpp b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp index 9f88650b5b..50388c1508 100644 --- a/libraries/AP_EFI/AP_EFI_DroneCAN.cpp +++ b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp @@ -154,7 +154,7 @@ void AP_EFI_DroneCAN::handle_status(const uavcan::equipment::ice::reciprocating: // assume max one cylinder status if (pkt.cylinder_status.size() > 0) { const auto &cs = pkt.cylinder_status[0]; - auto &c = istate.cylinder_status[0]; + auto &c = istate.cylinder_status; c.ignition_timing_deg = cs.ignition_timing_deg; c.injection_time_ms = cs.injection_time_ms; c.cylinder_head_temperature = cs.cylinder_head_temperature; diff --git a/libraries/AP_EFI/AP_EFI_NWPMU.cpp b/libraries/AP_EFI/AP_EFI_NWPMU.cpp index dfb098579e..3c0316772c 100644 --- a/libraries/AP_EFI/AP_EFI_NWPMU.cpp +++ b/libraries/AP_EFI/AP_EFI_NWPMU.cpp @@ -45,7 +45,7 @@ void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame) memcpy(&data, frame.data, sizeof(data)); internal_state.engine_speed_rpm = data.rpm; internal_state.throttle_position_percent = data.tps * 0.1f; - internal_state.cylinder_status[0].ignition_timing_deg = data.ignition_angle * 0.1f; + internal_state.cylinder_status.ignition_timing_deg = data.ignition_angle * 0.1f; break; } @@ -65,7 +65,7 @@ void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame) default: break; } - internal_state.cylinder_status[0].lambda_coefficient = data.lambda * 0.01f; + internal_state.cylinder_status.lambda_coefficient = data.lambda * 0.01f; break; } @@ -87,11 +87,11 @@ void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame) switch((NWPMU_TEMPERATURE_TYPE)data.temp_type) { case NWPMU_TEMPERATURE_TYPE::C: internal_state.coolant_temperature = C_TO_KELVIN(data.coolant_temp * 0.1f); - internal_state.cylinder_status[0].cylinder_head_temperature = C_TO_KELVIN(data.coolant_temp * 0.1f); + internal_state.cylinder_status.cylinder_head_temperature = C_TO_KELVIN(data.coolant_temp * 0.1f); break; case NWPMU_TEMPERATURE_TYPE::F: internal_state.coolant_temperature = F_TO_KELVIN(data.coolant_temp * 0.1f); - internal_state.cylinder_status[0].cylinder_head_temperature = F_TO_KELVIN(data.coolant_temp * 0.1f); + internal_state.cylinder_status.cylinder_head_temperature = F_TO_KELVIN(data.coolant_temp * 0.1f); break; default: break; diff --git a/libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp b/libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp index f15e7d6916..3812c69abd 100644 --- a/libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp +++ b/libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp @@ -67,19 +67,19 @@ void AP_EFI_Serial_Lutan::update() if (crc == crc2) { // valid data internal_state.spark_dwell_time_ms = int16_t(be16toh(data.dwell))*0.1; - internal_state.cylinder_status[0].injection_time_ms = be16toh(data.pulseWidth1)*0.00666; + internal_state.cylinder_status.injection_time_ms = be16toh(data.pulseWidth1)*0.00666; internal_state.engine_speed_rpm = be16toh(data.rpm); internal_state.atmospheric_pressure_kpa = int16_t(be16toh(data.barometer))*0.1; internal_state.intake_manifold_pressure_kpa = int16_t(be16toh(data.map))*0.1; internal_state.intake_manifold_temperature = degF_to_Kelvin(int16_t(be16toh(data.mat))*0.1); internal_state.coolant_temperature = degF_to_Kelvin(int16_t(be16toh(data.coolant))*0.1); // CHT is in coolant field - internal_state.cylinder_status[0].cylinder_head_temperature = internal_state.coolant_temperature; + internal_state.cylinder_status.cylinder_head_temperature = internal_state.coolant_temperature; internal_state.throttle_position_percent = int16_t(be16toh(data.tps))*0.1; // integrate fuel consumption if (internal_state.engine_speed_rpm > RPM_THRESHOLD) { - const float duty_cycle = (internal_state.cylinder_status[0].injection_time_ms * internal_state.engine_speed_rpm)/600.0f; + const float duty_cycle = (internal_state.cylinder_status.injection_time_ms * internal_state.engine_speed_rpm)/600.0f; internal_state.fuel_consumption_rate_cm3pm = duty_cycle*get_coef1() - get_coef2(); internal_state.estimated_consumed_fuel_volume_cm3 += internal_state.fuel_consumption_rate_cm3pm * (now - internal_state.last_updated_ms)/60000.0f; } else { diff --git a/libraries/AP_EFI/AP_EFI_Serial_MS.cpp b/libraries/AP_EFI/AP_EFI_Serial_MS.cpp index 753c8e1c71..1d8d40f067 100644 --- a/libraries/AP_EFI/AP_EFI_Serial_MS.cpp +++ b/libraries/AP_EFI/AP_EFI_Serial_MS.cpp @@ -86,7 +86,7 @@ bool AP_EFI_Serial_MS::read_incoming_realtime_data() float temp_float; switch (offset) { case PW1_MSB: - internal_state.cylinder_status[0].injection_time_ms = (float)((data << 8) + read_byte_CRC32())/1000.0f; + internal_state.cylinder_status.injection_time_ms = (float)((data << 8) + read_byte_CRC32())/1000.0f; offset++; // increment the counter because we read a byte in the previous line break; case RPM_MSB: @@ -95,7 +95,7 @@ bool AP_EFI_Serial_MS::read_incoming_realtime_data() offset++; break; case ADVANCE_MSB: - internal_state.cylinder_status[0].ignition_timing_deg = (float)((data << 8) + read_byte_CRC32())/10.0f; + internal_state.cylinder_status.ignition_timing_deg = (float)((data << 8) + read_byte_CRC32())/10.0f; offset++; break; case ENGINE_BM: @@ -116,7 +116,7 @@ bool AP_EFI_Serial_MS::read_incoming_realtime_data() case CHT_MSB: temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f; offset++; - internal_state.cylinder_status[0].cylinder_head_temperature = degF_to_Kelvin(temp_float); + internal_state.cylinder_status.cylinder_head_temperature = degF_to_Kelvin(temp_float); break; case TPS_MSB: temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f; @@ -126,7 +126,7 @@ bool AP_EFI_Serial_MS::read_incoming_realtime_data() case AFR1_MSB: temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f; offset++; - internal_state.cylinder_status[0].lambda_coefficient = temp_float; + internal_state.cylinder_status.lambda_coefficient = temp_float; break; case DWELL_MSB: temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f; @@ -160,7 +160,7 @@ bool AP_EFI_Serial_MS::read_incoming_realtime_data() // Calculate Fuel Consumption // Duty Cycle (Percent, because that's how HFE gives us the calibration coefficients) - float duty_cycle = (internal_state.cylinder_status[0].injection_time_ms * internal_state.engine_speed_rpm)/600.0f; + float duty_cycle = (internal_state.cylinder_status.injection_time_ms * internal_state.engine_speed_rpm)/600.0f; uint32_t current_time = AP_HAL::millis(); // Super Simplified integration method - Error Analysis TBD // This calculation gives erroneous results when the engine isn't running diff --git a/libraries/AP_EFI/AP_EFI_State.h b/libraries/AP_EFI/AP_EFI_State.h index f08daec667..6c25add5e6 100644 --- a/libraries/AP_EFI/AP_EFI_State.h +++ b/libraries/AP_EFI/AP_EFI_State.h @@ -17,7 +17,6 @@ #define EFI_MAX_INSTANCES 2 #define EFI_MAX_BACKENDS 2 -#define ENGINE_MAX_CYLINDERS 1 #include #include @@ -197,6 +196,6 @@ struct EFI_State { Spark_Plug_Usage spark_plug_usage; // Status for each cylinder in the engine - Cylinder_Status cylinder_status[ENGINE_MAX_CYLINDERS]; + Cylinder_Status cylinder_status; };