fix Serial I2C operation for Here3+ and Here4

Author: bugobliterator

AP_Periph/AP_Periph.cpp

@@ -164,9 +164,11 @@ void AP_Periph_FW::init()
 #endif
     }
 
+#ifdef I2C_SLAVE_ENABLED
     i2c_event_handle.set_source(&i2c_event_source);
     i2c_event_handle.register_event(1);
     i2c_setup();
+#endif
 
     compass.init();
 
@@ -283,6 +285,7 @@ void AP_Periph_FW::update()
 
     rcout_update();
 
+#ifdef I2C_SLAVE_ENABLED
     if (_setup_ser_i2c_mode && AP_Periph_FW::no_iface_finished_dna) {
         hal.scheduler->expect_delay_ms(100);
         g.serial_i2c_mode.set_and_save(1);
@@ -297,7 +300,9 @@ void AP_Periph_FW::update()
         hal.scheduler->reboot(false);
     }
 
-    if (!g.serial_i2c_mode) {
+    if (!g.serial_i2c_mode)
+#endif
+    {
         update_rainbow();
     }
 

AP_Periph/AP_Periph.h

@@ -122,8 +122,9 @@ class AP_Periph_FW {
 
     void toshibaled_interface_recv_byte(uint8_t recv_byte_idx, uint8_t recv_byte);
 
+#ifdef I2C_SLAVE_ENABLED
     void i2c_setup();
-
+#endif
     uint8_t compass_reg;
     struct reg_list {
         struct reg_list *next;
@@ -169,6 +170,7 @@ class AP_Periph_FW {
     uint64_t last_time_sync_usec;
     int64_t time_offset_usec;
 
+#ifdef I2C_SLAVE_ENABLED
     uint8_t i2c_led_color_red;
     uint8_t i2c_led_color_green;
     uint8_t i2c_led_color_blue;
@@ -182,7 +184,8 @@ class AP_Periph_FW {
 
     HAL_EventHandle i2c_event_handle;
     ChibiOS::EventSource i2c_event_source;
-    
+#endif
+
     static AP_Periph_FW *_singleton;
 
     enum class DebugOptions {

AP_Periph/Parameters.cpp

@@ -153,9 +153,10 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GARRAY(can_fdbaudrate, 1,     "CAN2_FDBAUDRATE", 8000000),
 #endif
 
+#ifdef I2C_SLAVE_ENABLED
     // select serial i2c mode and disable can gps
     GSCALAR(serial_i2c_mode,     "SER_I2C_MODE", 0),
-
+#endif
     //select terminator setting
     GARRAY(can_terminator, 0,    "CAN_TERMINATOR", 0),
 

AP_Periph/Parameters.h

@@ -94,7 +94,9 @@ class Parameters {
 
     AP_Int8 can_fdmode;
     AP_Int32 can_fdbaudrate[HAL_NUM_CAN_IFACES];
+#ifdef I2C_SLAVE_ENABLED
     AP_Int8 serial_i2c_mode;
+#endif
     AP_Int8 can_terminator[HAL_NUM_CAN_IFACES];
     AP_Int8 node_stats;
     AP_Int8 cubeid_fw_update_enabled;

AP_Periph/can.cpp

@@ -673,32 +673,29 @@ void AP_Periph_FW::can_update()
     if (AP_HAL::millis() > send_next_node_id_allocation_request_at_ms) {
         can_do_dna();
     }
+    bool enable_gps = true;
+#ifdef I2C_SLAVE_ENABLED
+    enable_gps = !g.serial_i2c_mode;
+#endif
 
+#ifdef I2C_SLAVE_ENABLED
+    // update LEDs as well
+    if (i2c_new_led_data && g.serial_i2c_mode) {
+        i2c_new_led_data = false;
+        set_rgb_led(i2c_led_color_red, i2c_led_color_green, i2c_led_color_blue);
+    }
+#endif
     if (!AP_Periph_FW::no_iface_finished_dna) {
         static uint32_t last_1Hz_ms;
         if (now - last_1Hz_ms >= 1000) {
             last_1Hz_ms = now;
             process1HzTasks(AP_HAL::micros64());
         }
 
-        bool enable_gps = true;
-#ifdef I2C_SLAVE_ENABLED
-        enable_gps = !g.serial_i2c_mode;
-#endif
-
         if (enable_gps) {
             dronecan->can_gps_update();
             dronecan->send_serial_monitor_data();
         }
-#ifdef I2C_SLAVE_ENABLED
-        else {
-            // update LEDs as well
-            if (i2c_new_led_data) {
-                i2c_new_led_data = false;
-                set_rgb_led(i2c_led_color_red, i2c_led_color_green, i2c_led_color_blue);
-            }
-        }
-#endif
 
         dronecan->can_mag_update();
         dronecan->can_baro_update();

AP_Periph/i2c_slave.cpp

@@ -1,7 +1,7 @@
 #include "AP_Periph.h"
 #include <ch.h>
 #include <hal.h>
-
+#ifdef I2C_SLAVE_ENABLED
 #define TOSHIBALED_I2C_ADDRESS 0x55
 #define RM3100_I2C_ADDR1 0x20
 #define RM3100_I2C_ADDR2 0x21
@@ -254,3 +254,4 @@ OSAL_IRQ_HANDLER(STM32_I2C2_EVENT_HANDLER) {
 
     OSAL_IRQ_EPILOGUE();
 }
+#endif

Here3+/hwdef.dat

@@ -82,6 +82,7 @@ define STM32_ADC_USE_ADC1 FALSE
 define HAL_DISABLE_ADC_DRIVER TRUE
 
 define HAL_INS_ACCELCAL_ENABLED FALSE
+define I2C_SLAVE_ENABLED
 
 SPIDEV rm3100 SPI3 DEVID1    MAG_CS MODE0  1*MHZ  1*MHZ
 COMPASS RM3100 SPI:rm3100 false ROTATION_YAW_180

Here4/hwdef.dat

@@ -131,6 +131,7 @@ define HAL_PERIPH_ENABLE_GPS 1
 define HAL_PERIPH_ENABLE_MAG 1
 define GPS_MOVING_BASELINE 1
 define AP_INERTIALSENSOR_ENABLED 1
+define I2C_SLAVE_ENABLED
 
 define HAL_NO_MONITOR_THREAD