灯的IIC地址重设，解锁，返航

ArduCopter/Parameters.cpp

@@ -1634,6 +1634,8 @@ const char* Copter::get_sysid_board_id(void)
 
     // snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
     snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
+    // snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
+    // snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
     AP::logger().Write_Message(buf);
     return buf;
 }

ArduCopter/UserCode.cpp

@@ -9,6 +9,8 @@ void Copter::userhook_init()
     // put your initialisation code here
     // this will be called once at start-up
 
+    relay.on(1);
+
 }
 #endif
 
@@ -93,8 +95,7 @@ void Copter::userhook_SuperSlowLoop()
         }
         cacl_volt_pst = 100 - cnt;
         battery.reset_remaining(1, cacl_volt_pst);
-    }
-
+    } 
 }
 #endif
 
@@ -102,6 +103,17 @@ void Copter::userhook_SuperSlowLoop()
 void Copter::userhook_auxSwitch1(uint8_t ch_flag)
 {
     // put your aux switch #1 handler here (CHx_OPT = 47)
+
+    // switch (ch_flag) {
+    //     case 2: {
+    //         relay.on(2);
+    //         break;
+    //     }
+    //     case 0: {
+    //         relay.off(2);
+    //         break;
+    //     }
+    // }
 }
 
 void Copter::userhook_auxSwitch2(uint8_t ch_flag)

ArduCopter/events.cpp

@@ -110,7 +110,7 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
             if (home_loc.lat != 0 || home_loc.lng != 0) {
                 // distance between vehicle and home_loc in meters
                 home = home_loc.get_distance(loc);
-                if(home < 10)
+                if(home < 10) // @Brown, if distance to home less than 10m,don't action
                 {
                     desired_action = Failsafe_Action_None;
                 }

ArduCopter/mode_rtl.cpp

@@ -66,8 +66,8 @@ void ModeRTL::run(bool disarm_on_land)
             }
             break;
         case RTL_FinalDescent:
-            gcs().send_text(MAV_SEVERITY_INFO,"complete:%d,_state:%d",_state_complete,_state);
-            set_mode(Mode::Number::BRAKE,ModeReason::MISSION_END);
+            // gcs().send_text(MAV_SEVERITY_INFO,"complete:%d,_state:%d",_state_complete,_state);
+            // set_mode(Mode::Number::BRAKE,ModeReason::MISSION_END);
             // do nothing
             break;
         case RTL_Land:

ArduCopter/motors.cpp

@@ -28,8 +28,8 @@ void Copter::arm_motors_check()
 
 
     int16_t yaw_in = channel_yaw->get_control_in();
-    // int16_t pitch_in = channel_pitch->get_control_in();
-    // int16_t roll_in = channel_roll->get_control_in();
+    int16_t pitch_in = channel_pitch->get_control_in();
+    int16_t roll_in = channel_roll->get_control_in();
     int16_t throttle_in = channel_throttle->get_control_in();
     static const uint16_t trim_trg_value = arming.get_rudder_arm_value()*40;
     static const int8_t con_arm_delay = arming.get_rudder_arm_time();
@@ -46,8 +46,8 @@ void Copter::arm_motors_check()
 
     // full right
     // if (yaw_in > 4000) {
-    // if(yaw_in>trim_trg_value&&pitch_in>trim_trg_value&&roll_in< -trim_trg_value){
-    if(yaw_in>trim_trg_value){
+    if(yaw_in>trim_trg_value&&pitch_in>trim_trg_value&&roll_in< -trim_trg_value){
+    // if(yaw_in>trim_trg_value){
         // increase the arming counter to a maximum of 1 beyond the auto trim counter
         if (arming_counter <= AUTO_TRIM_DELAY) {
             arming_counter++;
@@ -71,8 +71,8 @@ void Copter::arm_motors_check()
 
     // full left and rudder disarming is enabled
     // } else if ((yaw_in < -4000) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) {
-    // } else if((yaw_in<-trim_trg_value&&pitch_in>trim_trg_value&&roll_in>trim_trg_value)&&(arming_rudder==AP_Arming::RudderArming::ARMDISARM)){
-    } else if((yaw_in<-trim_trg_value)&&(arming_rudder==AP_Arming::RudderArming::ARMDISARM)){
+    } else if((yaw_in<-trim_trg_value&&pitch_in>trim_trg_value&&roll_in>trim_trg_value)&&(arming_rudder==AP_Arming::RudderArming::ARMDISARM)){
+    // } else if((yaw_in<-trim_trg_value)&&(arming_rudder==AP_Arming::RudderArming::ARMDISARM)){
         if (!flightmode->has_manual_throttle() && !ap.land_complete) {
             arming_counter = 0;
             return;

libraries/AP_Notify/AP_Notify.cpp

@@ -241,10 +241,10 @@ void AP_Notify::add_backends(void)
 #endif
                 break;
             case Notify_LED_ToshibaLED_I2C_Internal:
-                ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL));
+                ADD_BACKEND(new ToshibaLED_I2C(2));
                 break;
             case Notify_LED_ToshibaLED_I2C_External:
-                ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
+                ADD_BACKEND(new ToshibaLED_I2C(3));
                 break;
 #if !HAL_MINIMIZE_FEATURES
             case Notify_LED_NCP5623_I2C_External:

libraries/AP_Notify/ToshibaLED_I2C.cpp

@@ -48,11 +48,12 @@ ToshibaLED_I2C::ToshibaLED_I2C(uint8_t bus)
 bool ToshibaLED_I2C::hw_init(void)
 {
     // first look for led on external bus
-    // _dev = std::move(hal.i2c_mgr->get_device(_bus, TOSHIBA_LED_I2C_ADDR));
+    _dev = std::move(hal.i2c_mgr->get_device(_bus, TOSHIBA_LED_I2C_ADDR));
 
-    FOREACH_I2C(i) {
-        _dev = std::move(hal.i2c_mgr->get_device(i, TOSHIBA_LED_I2C_ADDR));
-    }
+    // FOREACH_I2C(i) {
+    //     index_iic[i] = i;
+    //     _dev = std::move(hal.i2c_mgr->get_device(i, TOSHIBA_LED_I2C_ADDR));
+    // }
     
     if (!_dev) {
         return false;
@@ -76,16 +77,42 @@ bool ToshibaLED_I2C::hw_init(void)
     if (ret) {
         _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&ToshibaLED_I2C::_timer, void));
     }
-
+    // index_iic[4] = 99;
+    // _dev->transfer(index_iic, 5, nullptr, 0);
+    // test[0] = 99;
+    // test[1] = 99;
+    // test[2] = 99;
+    // test[3] = 99;
+    // test[4] = 99;
+    // _dev->transfer(test, 5, nullptr, 0);
     return ret;
 }
 
 // set_rgb - set color as a combination of red, green and blue values
 bool ToshibaLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 {
+    // rgb = {red, green, blue};
+    // _need_update = true;
+    // return true;
+
+
+    static uint8_t cnt;
+    static uint8_t l_mode,l_red,l_green,l_blue;
     rgb = {red, green, blue};
-    _need_update = true;
+    
+    cnt++;
+
+    if(l_mode != rgb_mode || l_red != rgb.r || l_green != rgb.g || l_blue != rgb.b || cnt > 50)
+    {
+        cnt =0;
+        _need_update = true;
+        l_mode = rgb_mode ;
+        l_red = rgb.r ;
+        l_green = rgb.g ;
+        l_blue = rgb.b;
+    }
     return true;
+    
 }
 
 void ToshibaLED_I2C::set_rgb_mode(uint8_t mode)

libraries/AP_Notify/ToshibaLED_I2C.h

@@ -37,4 +37,6 @@ private:
     } rgb;
     uint8_t _bus;
     uint8_t rgb_mode;
+    uint8_t  index_iic[5];
+    uint8_t  test[5];
 };