diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 891f53509f..3a19ff72e8 100644 --- a/ArduCopter/Parameters.cpp +++ b/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; } diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index f16effdcd6..61f188b81f 100644 --- a/ArduCopter/UserCode.cpp +++ b/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) diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 0e11c7c371..7a7a83029c 100644 --- a/ArduCopter/events.cpp +++ b/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; } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index b52903deb4..08fe95ffd5 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/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: diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 07f0704260..d848b1a7cf 100644 --- a/ArduCopter/motors.cpp +++ b/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; diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 1905d0f90b..26f4d466da 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/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: diff --git a/libraries/AP_Notify/ToshibaLED_I2C.cpp b/libraries/AP_Notify/ToshibaLED_I2C.cpp index 534313d257..84d6d77bd6 100644 --- a/libraries/AP_Notify/ToshibaLED_I2C.cpp +++ b/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) diff --git a/libraries/AP_Notify/ToshibaLED_I2C.h b/libraries/AP_Notify/ToshibaLED_I2C.h index c5c5125bcf..78d1d72339 100644 --- a/libraries/AP_Notify/ToshibaLED_I2C.h +++ b/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]; };