From f101648e4170f9454c924ac8bad052e4b76e1466 Mon Sep 17 00:00:00 2001 From: z Date: Tue, 7 Jul 2020 18:31:07 +0800 Subject: [PATCH] add display --- ArduCopter/motors.cpp | 29 +- libraries/AP_Arming/AP_Arming.cpp | 5 + libraries/AP_Arming/AP_Arming.h | 4 + libraries/AP_Camera/AP_Camera.cpp | 4 + libraries/AP_Camera/AP_Camera.h | 6 + .../AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat | 22 +- libraries/AP_Notify/AP_Notify.h | 2 + libraries/AP_Notify/Display.cpp | 463 +++++++++++++++++- libraries/AP_Notify/Display.h | 21 + 9 files changed, 526 insertions(+), 30 deletions(-) diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 4b8bc879cc..49774ca370 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -26,24 +26,34 @@ void Copter::arm_motors_check() } #endif + 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 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(); + // static uint8_t cnt; + // cnt++; + // if(cnt%20 == 0) + // gcs().send_text(MAV_SEVERITY_INFO,"v:%4d,t:%4d,y:%4d,p:%4d,r:%4d",trim_trg_value,throttle_in,yaw_in,pitch_in,roll_in); // ensure throttle is down - if (channel_throttle->get_control_in() > 0) { + if (throttle_in > 1000-arming.get_rudder_arm_value()*10) { arming_counter = 0; return; } - int16_t yaw_in = channel_yaw->get_control_in(); // full right - if (yaw_in > 4000) { - + // if (yaw_in > 4000) { + if(yaw_in>trim_trg_value&&pitch_in>trim_trg_value&&roll_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++; } // arm the motors and configure for flight - if (arming_counter == ARM_DELAY && !motors->armed()) { + // if (arming_counter == ARM_DELAY && !motors->armed()) { + if (arming_counter == con_arm_delay && !motors->armed()) { // reset arming counter if arming fail if (!arming.arm(AP_Arming::Method::RUDDER)) { arming_counter = 0; @@ -58,19 +68,22 @@ 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 < -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)){ if (!flightmode->has_manual_throttle() && !ap.land_complete) { arming_counter = 0; return; } // increase the counter to a maximum of 1 beyond the disarm delay - if (arming_counter <= DISARM_DELAY) { + // if (arming_counter <= DISARM_DELAY) { + if (arming_counter <= con_arm_delay) { arming_counter++; } // disarm the motors - if (arming_counter == DISARM_DELAY && motors->armed()) { + // if (arming_counter == DISARM_DELAY && motors->armed()) { + if (arming_counter == con_arm_delay && motors->armed()) { arming.disarm(); } diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 713e156f31..30f51da083 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -115,6 +115,11 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = { // @User: Standard AP_GROUPINFO("CHECK", 8, AP_Arming, checks_to_perform, ARMING_CHECK_ALL), + // @Brown ,摇杆解锁延时， *0.1s + AP_GROUPINFO("DELAY", 9, AP_Arming, rudder_arm_delay_dsec, 5), + // @Brown ,摇杆解锁量，百分比 + AP_GROUPINFO("VPERC", 10, AP_Arming, rudder_arm_value_perc, 90), + AP_GROUPEND }; diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index 80396e7239..2f1fd88872 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -79,6 +79,8 @@ public: }; RudderArming get_rudder_arming_type() const { return (RudderArming)_rudder_arming.get(); } + int8_t get_rudder_arm_time() const { return rudder_arm_delay_dsec.get(); } + int8_t get_rudder_arm_value() const { return rudder_arm_value_perc.get(); } static const struct AP_Param::GroupInfo var_info[]; @@ -89,6 +91,8 @@ protected: AP_Int32 checks_to_perform; // bitmask for which checks are required AP_Float accel_error_threshold; AP_Int8 _rudder_arming; + AP_Int8 rudder_arm_delay_dsec; + AP_Int8 rudder_arm_value_perc; AP_Int32 _required_mission_items; // internal members diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 4f328c8324..7de13f46a8 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -156,6 +156,8 @@ void AP_Camera::trigger_pic() } log_picture(); + AP_Notify::flags.image_index_log = number_of_log(); + AP_Notify::flags.image_index = number_of_index(); } /// de-activate the trigger after some delay, but without using a delay() function @@ -410,12 +412,14 @@ void AP_Camera::log_picture() logger->Write_Camera(current_loc); last_pos_lat = current_loc.lat; last_pos_lng = current_loc.lng; + _image_index_log++; } } else { if (logger->should_log(log_camera_bit)) { logger->Write_Trigger(current_loc); last_pos_lat = current_loc.lat; last_pos_lng = current_loc.lng; + _image_index_log++; } } } diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 80d1d219eb..e3ee5ecf90 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -78,6 +78,12 @@ public: void get_last_trigger_pos(int32_t &lat,int32_t &lng){ lat=last_pos_lat;lng=last_pos_lng; } // @Brown,for get camera pos + // modivy by @Brown + uint16_t number_of_log(void) const { return _image_index_log; } + uint16_t number_of_index(void) const { return _image_index; } + + uint16_t _image_index_log; // number of pictures taken to log @Brown + private: static AP_Camera *_singleton; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat index 0f989d96a7..08629bdceb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat @@ -160,20 +160,20 @@ PD0 UART4_RX UART4 NODMA PD1 UART4_TX UART4 NODMA # # USART6 is telem3 -PG9 USART6_RX USART6 NODMA -# we leave PG14 as an input to prevent it acting as a pullup -# on the IOMCU SBUS input -# PG14 USART6_TX USART6 NODMA -PG15 USART6_CTS USART6 -PG8 USART6_RTS USART6 - -# # USART6 is telem3 for cube # PG9 USART6_RX USART6 NODMA # # we leave PG14 as an input to prevent it acting as a pullup # # on the IOMCU SBUS input -# PG14 USART6_TX USART6 NODMA -# # PG15 USART6_CTS USART6 -# # PG8 USART6_RTS USART6 +# # PG14 USART6_TX USART6 NODMA +# PG15 USART6_CTS USART6 +# PG8 USART6_RTS USART6 + +# # USART6 is telem3 for cube +PG9 USART6_RX USART6 NODMA +# we leave PG14 as an input to prevent it acting as a pullup +# on the IOMCU SBUS input +PG14 USART6_TX USART6 NODMA +# PG15 USART6_CTS USART6 +# PG8 USART6_RTS USART6 # UART7 is debug PF6 UART7_RX UART7 NODMA diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 8171f16860..89c815600d 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -100,6 +100,8 @@ public: bool waiting_for_throw; // true when copter is in THROW mode and waiting to detect the user hand launch bool powering_off; // true when the vehicle is powering off bool video_recording; // true when the vehicle is recording video + uint16_t image_index ; // camera images ,add by @Brown + uint16_t image_index_log ; // camera images to log }; /// notify_events_type - bitmask of active events. diff --git a/libraries/AP_Notify/Display.cpp b/libraries/AP_Notify/Display.cpp index c5870bbdaa..d8760ce4b1 100644 --- a/libraries/AP_Notify/Display.cpp +++ b/libraries/AP_Notify/Display.cpp @@ -322,6 +322,197 @@ static_assert(ARRAY_SIZE(_font) == 1280, "_font is correct size"); static_assert(ARRAY_SIZE(_font) == 475, "_font is correct size"); #endif +static const unsigned char _font_16[]= +{ + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,// 0 + 0x00,0x00,0x00,0xF8,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x33,0x30,0x00,0x00,0x00,//! 1 + 0x00,0x10,0x0C,0x06,0x10,0x0C,0x06,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,//" 2 + 0x40,0xC0,0x78,0x40,0xC0,0x78,0x40,0x00,0x04,0x3F,0x04,0x04,0x3F,0x04,0x04,0x00,//# 3 + 0x00,0x70,0x88,0xFC,0x08,0x30,0x00,0x00,0x00,0x18,0x20,0xFF,0x21,0x1E,0x00,0x00,//$ 4 + 0xF0,0x08,0xF0,0x00,0xE0,0x18,0x00,0x00,0x00,0x21,0x1C,0x03,0x1E,0x21,0x1E,0x00,//% 5 + 0x00,0xF0,0x08,0x88,0x70,0x00,0x00,0x00,0x1E,0x21,0x23,0x24,0x19,0x27,0x21,0x10,//& 6 + 0x10,0x16,0x0E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,//' 7 + 0x00,0x00,0x00,0xE0,0x18,0x04,0x02,0x00,0x00,0x00,0x00,0x07,0x18,0x20,0x40,0x00,//( 8 + 0x00,0x02,0x04,0x18,0xE0,0x00,0x00,0x00,0x00,0x40,0x20,0x18,0x07,0x00,0x00,0x00,//) 9 + 0x40,0x40,0x80,0xF0,0x80,0x40,0x40,0x00,0x02,0x02,0x01,0x0F,0x01,0x02,0x02,0x00,//* 10 + 0x00,0x00,0x00,0xF0,0x00,0x00,0x00,0x00,0x01,0x01,0x01,0x1F,0x01,0x01,0x01,0x00,//+ 11 + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0xB0,0x70,0x00,0x00,0x00,0x00,0x00,//, 12 + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01,//- 13 + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x30,0x00,0x00,0x00,0x00,0x00,//. 14 + 0x00,0x00,0x00,0x00,0x80,0x60,0x18,0x04,0x00,0x60,0x18,0x06,0x01,0x00,0x00,0x00,/// 15 + 0x00,0xE0,0x10,0x08,0x08,0x10,0xE0,0x00,0x00,0x0F,0x10,0x20,0x20,0x10,0x0F,0x00,//0 16 + 0x00,0x10,0x10,0xF8,0x00,0x00,0x00,0x00,0x00,0x20,0x20,0x3F,0x20,0x20,0x00,0x00,//1 17 + 0x00,0x70,0x08,0x08,0x08,0x88,0x70,0x00,0x00,0x30,0x28,0x24,0x22,0x21,0x30,0x00,//2 18 + 0x00,0x30,0x08,0x88,0x88,0x48,0x30,0x00,0x00,0x18,0x20,0x20,0x20,0x11,0x0E,0x00,//3 19 + 0x00,0x00,0xC0,0x20,0x10,0xF8,0x00,0x00,0x00,0x07,0x04,0x24,0x24,0x3F,0x24,0x00,//4 20 + 0x00,0xF8,0x08,0x88,0x88,0x08,0x08,0x00,0x00,0x19,0x21,0x20,0x20,0x11,0x0E,0x00,//5 21 + 0x00,0xE0,0x10,0x88,0x88,0x18,0x00,0x00,0x00,0x0F,0x11,0x20,0x20,0x11,0x0E,0x00,//6 22 + 0x00,0x38,0x08,0x08,0xC8,0x38,0x08,0x00,0x00,0x00,0x00,0x3F,0x00,0x00,0x00,0x00,//7 23 + 0x00,0x70,0x88,0x08,0x08,0x88,0x70,0x00,0x00,0x1C,0x22,0x21,0x21,0x22,0x1C,0x00,//8 24 + 0x00,0xE0,0x10,0x08,0x08,0x10,0xE0,0x00,0x00,0x00,0x31,0x22,0x22,0x11,0x0F,0x00,//9 25 + 0x00,0x00,0x00,0xC0,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x30,0x00,0x00,0x00,//: 26 + 0x00,0x00,0x00,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x60,0x00,0x00,0x00,0x00,//; 27 + 0x00,0x00,0x80,0x40,0x20,0x10,0x08,0x00,0x00,0x01,0x02,0x04,0x08,0x10,0x20,0x00,//< 28 + 0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x00,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x00,//= 29 + 0x00,0x08,0x10,0x20,0x40,0x80,0x00,0x00,0x00,0x20,0x10,0x08,0x04,0x02,0x01,0x00,//> 30 + 0x00,0x70,0x48,0x08,0x08,0x08,0xF0,0x00,0x00,0x00,0x00,0x30,0x36,0x01,0x00,0x00,//? 31 + 0xC0,0x30,0xC8,0x28,0xE8,0x10,0xE0,0x00,0x07,0x18,0x27,0x24,0x23,0x14,0x0B,0x00,//@ 32 + 0x00,0x00,0xC0,0x38,0xE0,0x00,0x00,0x00,0x20,0x3C,0x23,0x02,0x02,0x27,0x38,0x20,//A 33 + 0x08,0xF8,0x88,0x88,0x88,0x70,0x00,0x00,0x20,0x3F,0x20,0x20,0x20,0x11,0x0E,0x00,//B 34 + 0xC0,0x30,0x08,0x08,0x08,0x08,0x38,0x00,0x07,0x18,0x20,0x20,0x20,0x10,0x08,0x00,//C 35 + 0x08,0xF8,0x08,0x08,0x08,0x10,0xE0,0x00,0x20,0x3F,0x20,0x20,0x20,0x10,0x0F,0x00,//D 36 + 0x08,0xF8,0x88,0x88,0xE8,0x08,0x10,0x00,0x20,0x3F,0x20,0x20,0x23,0x20,0x18,0x00,//E 37 + 0x08,0xF8,0x88,0x88,0xE8,0x08,0x10,0x00,0x20,0x3F,0x20,0x00,0x03,0x00,0x00,0x00,//F 38 + 0xC0,0x30,0x08,0x08,0x08,0x38,0x00,0x00,0x07,0x18,0x20,0x20,0x22,0x1E,0x02,0x00,//G 39 + 0x08,0xF8,0x08,0x00,0x00,0x08,0xF8,0x08,0x20,0x3F,0x21,0x01,0x01,0x21,0x3F,0x20,//H 40 + 0x00,0x08,0x08,0xF8,0x08,0x08,0x00,0x00,0x00,0x20,0x20,0x3F,0x20,0x20,0x00,0x00,//I 41 + 0x00,0x00,0x08,0x08,0xF8,0x08,0x08,0x00,0xC0,0x80,0x80,0x80,0x7F,0x00,0x00,0x00,//J 42 + 0x08,0xF8,0x88,0xC0,0x28,0x18,0x08,0x00,0x20,0x3F,0x20,0x01,0x26,0x38,0x20,0x00,//K 43 + 0x08,0xF8,0x08,0x00,0x00,0x00,0x00,0x00,0x20,0x3F,0x20,0x20,0x20,0x20,0x30,0x00,//L 44 + 0x08,0xF8,0xF8,0x00,0xF8,0xF8,0x08,0x00,0x20,0x3F,0x00,0x3F,0x00,0x3F,0x20,0x00,//M 45 + 0x08,0xF8,0x30,0xC0,0x00,0x08,0xF8,0x08,0x20,0x3F,0x20,0x00,0x07,0x18,0x3F,0x00,//N 46 + 0xE0,0x10,0x08,0x08,0x08,0x10,0xE0,0x00,0x0F,0x10,0x20,0x20,0x20,0x10,0x0F,0x00,//O 47 + 0x08,0xF8,0x08,0x08,0x08,0x08,0xF0,0x00,0x20,0x3F,0x21,0x01,0x01,0x01,0x00,0x00,//P 48 + 0xE0,0x10,0x08,0x08,0x08,0x10,0xE0,0x00,0x0F,0x18,0x24,0x24,0x38,0x50,0x4F,0x00,//Q 49 + 0x08,0xF8,0x88,0x88,0x88,0x88,0x70,0x00,0x20,0x3F,0x20,0x00,0x03,0x0C,0x30,0x20,//R 50 + 0x00,0x70,0x88,0x08,0x08,0x08,0x38,0x00,0x00,0x38,0x20,0x21,0x21,0x22,0x1C,0x00,//S 51 + 0x18,0x08,0x08,0xF8,0x08,0x08,0x18,0x00,0x00,0x00,0x20,0x3F,0x20,0x00,0x00,0x00,//T 52 + 0x08,0xF8,0x08,0x00,0x00,0x08,0xF8,0x08,0x00,0x1F,0x20,0x20,0x20,0x20,0x1F,0x00,//U 53 + 0x08,0x78,0x88,0x00,0x00,0xC8,0x38,0x08,0x00,0x00,0x07,0x38,0x0E,0x01,0x00,0x00,//V 54 + 0xF8,0x08,0x00,0xF8,0x00,0x08,0xF8,0x00,0x03,0x3C,0x07,0x00,0x07,0x3C,0x03,0x00,//W 55 + 0x08,0x18,0x68,0x80,0x80,0x68,0x18,0x08,0x20,0x30,0x2C,0x03,0x03,0x2C,0x30,0x20,//X 56 + 0x08,0x38,0xC8,0x00,0xC8,0x38,0x08,0x00,0x00,0x00,0x20,0x3F,0x20,0x00,0x00,0x00,//Y 57 + 0x10,0x08,0x08,0x08,0xC8,0x38,0x08,0x00,0x20,0x38,0x26,0x21,0x20,0x20,0x18,0x00,//Z 58 + 0x00,0x00,0x00,0xFE,0x02,0x02,0x02,0x00,0x00,0x00,0x00,0x7F,0x40,0x40,0x40,0x00,//[ 59 + 0x00,0x0C,0x30,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x06,0x38,0xC0,0x00,//\ 60 + 0x00,0x02,0x02,0x02,0xFE,0x00,0x00,0x00,0x00,0x40,0x40,0x40,0x7F,0x00,0x00,0x00,//] 61 + 0x00,0x00,0x04,0x02,0x02,0x02,0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,//^ 62 + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,//_ 63 + 0x00,0x02,0x02,0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,//` 64 + 0x00,0x00,0x80,0x80,0x80,0x80,0x00,0x00,0x00,0x19,0x24,0x22,0x22,0x22,0x3F,0x20,//a 65 + 0x08,0xF8,0x00,0x80,0x80,0x00,0x00,0x00,0x00,0x3F,0x11,0x20,0x20,0x11,0x0E,0x00,//b 66 + 0x00,0x00,0x00,0x80,0x80,0x80,0x00,0x00,0x00,0x0E,0x11,0x20,0x20,0x20,0x11,0x00,//c 67 + 0x00,0x00,0x00,0x80,0x80,0x88,0xF8,0x00,0x00,0x0E,0x11,0x20,0x20,0x10,0x3F,0x20,//d 68 + 0x00,0x00,0x80,0x80,0x80,0x80,0x00,0x00,0x00,0x1F,0x22,0x22,0x22,0x22,0x13,0x00,//e 69 + 0x00,0x80,0x80,0xF0,0x88,0x88,0x88,0x18,0x00,0x20,0x20,0x3F,0x20,0x20,0x00,0x00,//f 70 + 0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x6B,0x94,0x94,0x94,0x93,0x60,0x00,//g 71 + 0x08,0xF8,0x00,0x80,0x80,0x80,0x00,0x00,0x20,0x3F,0x21,0x00,0x00,0x20,0x3F,0x20,//h 72 + 0x00,0x80,0x98,0x98,0x00,0x00,0x00,0x00,0x00,0x20,0x20,0x3F,0x20,0x20,0x00,0x00,//i 73 + 0x00,0x00,0x00,0x80,0x98,0x98,0x00,0x00,0x00,0xC0,0x80,0x80,0x80,0x7F,0x00,0x00,//j 74 + 0x08,0xF8,0x00,0x00,0x80,0x80,0x80,0x00,0x20,0x3F,0x24,0x02,0x2D,0x30,0x20,0x00,//k 75 + 0x00,0x08,0x08,0xF8,0x00,0x00,0x00,0x00,0x00,0x20,0x20,0x3F,0x20,0x20,0x00,0x00,//l 76 + 0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x00,0x20,0x3F,0x20,0x00,0x3F,0x20,0x00,0x3F,//m 77 + 0x80,0x80,0x00,0x80,0x80,0x80,0x00,0x00,0x20,0x3F,0x21,0x00,0x00,0x20,0x3F,0x20,//n 78 + 0x00,0x00,0x80,0x80,0x80,0x80,0x00,0x00,0x00,0x1F,0x20,0x20,0x20,0x20,0x1F,0x00,//o 79 + 0x80,0x80,0x00,0x80,0x80,0x00,0x00,0x00,0x80,0xFF,0xA1,0x20,0x20,0x11,0x0E,0x00,//p 80 + 0x00,0x00,0x00,0x80,0x80,0x80,0x80,0x00,0x00,0x0E,0x11,0x20,0x20,0xA0,0xFF,0x80,//q 81 + 0x80,0x80,0x80,0x00,0x80,0x80,0x80,0x00,0x20,0x20,0x3F,0x21,0x20,0x00,0x01,0x00,//r 82 + 0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x33,0x24,0x24,0x24,0x24,0x19,0x00,//s 83 + 0x00,0x80,0x80,0xE0,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x1F,0x20,0x20,0x00,0x00,//t 84 + 0x80,0x80,0x00,0x00,0x00,0x80,0x80,0x00,0x00,0x1F,0x20,0x20,0x20,0x10,0x3F,0x20,//u 85 + 0x80,0x80,0x80,0x00,0x00,0x80,0x80,0x80,0x00,0x01,0x0E,0x30,0x08,0x06,0x01,0x00,//v 86 + 0x80,0x80,0x00,0x80,0x00,0x80,0x80,0x80,0x0F,0x30,0x0C,0x03,0x0C,0x30,0x0F,0x00,//w 87 + 0x00,0x80,0x80,0x00,0x80,0x80,0x80,0x00,0x00,0x20,0x31,0x2E,0x0E,0x31,0x20,0x00,//x 88 + 0x80,0x80,0x80,0x00,0x00,0x80,0x80,0x80,0x80,0x81,0x8E,0x70,0x18,0x06,0x01,0x00,//y 89 + 0x00,0x80,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x21,0x30,0x2C,0x22,0x21,0x30,0x00,//z 90 + 0x00,0x00,0x00,0x00,0x80,0x7C,0x02,0x02,0x00,0x00,0x00,0x00,0x00,0x3F,0x40,0x40,//{ 91 + 0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x00,//| 92 + 0x00,0x02,0x02,0x7C,0x80,0x00,0x00,0x00,0x00,0x40,0x40,0x3F,0x00,0x00,0x00,0x00,//} 93 + 0x00,0x06,0x01,0x01,0x02,0x02,0x04,0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,//~ 94 +}; + + + /// 致(0) 睿(1) 智(2) 控(3) 云(4) 台(5) +static unsigned char ZRZKYT[][32]={ + + {0x00,0x00,0xFF,0x05,0x05,0xF5,0x55,0x5D,0x55,0x55,0x55,0x55,0xF5,0x05,0x01,0x00}, + {0x40,0x30,0x0F,0x80,0xA0,0x97,0xBD,0x55,0x55,0x55,0x55,0x55,0xB7,0x80,0x80,0x00},/*"厦",0*/ + + {0x00,0xF8,0x01,0x06,0x00,0x00,0x02,0x02,0x02,0x02,0x02,0x02,0x02,0xFE,0x00,0x00}, + {0x00,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x40,0x80,0x7F,0x00,0x00},/*"门",1*/ + + {0x42,0x62,0x5A,0xC6,0x52,0x62,0xC2,0x20,0xD8,0x17,0x10,0x10,0xF0,0x10,0x10,0x00}, + {0x40,0xC4,0x44,0x7F,0x24,0x24,0xA0,0x40,0x21,0x16,0x08,0x16,0x21,0x40,0x80,0x00},/*"致",2*/ + + {0x20,0x18,0x88,0xA8,0x68,0x28,0xA8,0x6F,0xAA,0x2A,0x6A,0xAA,0x0A,0x28,0x18,0x00}, + {0x04,0x04,0x02,0xFE,0xAB,0xAB,0xAA,0xAA,0xAA,0xAB,0xAB,0xFE,0x02,0x04,0x04,0x00},/*"睿",3*/ + + {0x10,0x94,0x53,0x32,0x1E,0x32,0x52,0x10,0x00,0x7E,0x42,0x42,0x42,0x7E,0x00,0x00}, + {0x00,0x00,0x00,0xFF,0x49,0x49,0x49,0x49,0x49,0x49,0x49,0xFF,0x00,0x00,0x00,0x00},/*"智",4*/ + + {0x10,0x10,0x10,0xFF,0x90,0x20,0x98,0x48,0x28,0x09,0x0E,0x28,0x48,0xA8,0x18,0x00}, + {0x02,0x42,0x81,0x7F,0x00,0x40,0x40,0x42,0x42,0x42,0x7E,0x42,0x42,0x42,0x40,0x00},/*"控",5*/ + +}; + +unsigned char gImage_zr12864[] = { +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x08,0x38,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8,0xF8, +0xF8,0xF8,0xF8,0xF8,0x78,0x18,0x00,0xC0,0xF0,0xF0,0xF0,0xE0,0xC0,0x80,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x10,0xB0,0xF0,0x70,0x30,0x30,0x30,0xB0,0x30, +0x00,0xC0,0xF0,0x30,0x30,0x30,0x30,0xF0,0xF0,0x30,0x00,0x00,0x00,0x00,0xC0,0xC0, +0x40,0x78,0x78,0x50,0x50,0x50,0x50,0x50,0x50,0x50,0x50,0x50,0x50,0xD0,0xD0,0xC0, +0x00,0x00,0x00,0x00,0x00,0xF0,0xB0,0x90,0xF0,0xF0,0xB0,0x90,0x90,0x80,0x00,0xF0, +0xF0,0x30,0x30,0x30,0xF0,0xF0,0x00,0x00,0x00,0x00,0xC0,0xC0,0xF8,0xF0,0xC0,0x00, +0x00,0xF0,0xF0,0x30,0x30,0x30,0x38,0x38,0x30,0x30,0xF0,0xF0,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x03,0x83,0xE3,0xFB,0xFF,0xFF,0xFF,0x7F, +0x3F,0x0F,0x83,0xE0,0xF8,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFC, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x67,0x67,0x66,0xFE,0xFE,0x66,0x67,0x67, +0x00,0x01,0x03,0x98,0xF8,0xF0,0xFC,0x0F,0x03,0x00,0x00,0x00,0x00,0x00,0x01,0xF5, +0xF4,0xB5,0xBD,0xBF,0xBF,0xA9,0xA9,0xA9,0xAF,0xBF,0xBF,0xBD,0xBC,0xF4,0xF5,0x01, +0x00,0x00,0x00,0x00,0x00,0xEC,0xE4,0x36,0x37,0x33,0x37,0x34,0x3C,0x30,0x30,0x37, +0x37,0x36,0x36,0xF6,0xE7,0xE7,0x00,0x00,0x00,0x00,0x60,0x60,0xFF,0xFF,0x30,0x10, +0x00,0x05,0x67,0x66,0x63,0x63,0xE0,0xE0,0x63,0x66,0x66,0x65,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x80,0xC0,0xF0,0xFC,0xFF,0xFF,0xFF,0x7F,0x1F,0x87,0x01,0x10, +0x7C,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x7F,0x3F,0x1F,0x0F,0x03, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x88,0x88,0x8C,0x8C,0x8F,0x0F,0x0C,0x8C,0x04, +0x00,0x00,0x8E,0x07,0x01,0x80,0x03,0x07,0x0E,0x00,0x00,0x00,0x80,0x80,0x80,0x0F, +0x0F,0x0A,0x8A,0x0A,0x0A,0x0A,0x8A,0x0A,0x0A,0x8A,0x0A,0x0A,0x0A,0x0F,0x0F,0x00, +0x80,0x80,0x80,0x80,0x00,0x0F,0x8F,0x09,0x09,0x09,0x89,0x09,0x09,0x89,0x09,0x09, +0x09,0x09,0x09,0x0F,0x8F,0x0F,0x00,0x00,0x80,0x00,0x08,0x88,0x8F,0x8F,0x00,0x00, +0x00,0x88,0x88,0x08,0x08,0x88,0x0F,0x0F,0x08,0x08,0x88,0x88,0x80,0x00,0x00,0x00, +0x00,0x00,0x00,0x3E,0x3F,0x3F,0x3F,0x1F,0x0F,0x03,0x20,0x38,0x3E,0x3F,0x3F,0x3E, +0x38,0x01,0x03,0x0F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3E,0x38,0x20,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x38,0x24,0x23,0x21,0x00,0x00,0x3F,0x04, +0x04,0x04,0x3F,0x00,0x00,0x3F,0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x04,0x04,0x3B, +0x00,0x00,0x1F,0x20,0x20,0x20,0x1F,0x00,0x00,0x3F,0x00,0x00,0x00,0x00,0x00,0x00, +0x30,0x28,0x26,0x23,0x00,0x00,0x3F,0x04,0x04,0x04,0x3F,0x00,0x00,0x3F,0x00,0x00, +0x00,0x00,0x00,0x00,0x3F,0x06,0x0A,0x11,0x20,0x00,0x1F,0x20,0x20,0x20,0x1F,0x00, +0x00,0x3F,0x03,0x06,0x18,0x3F,0x00,0x00,0x0E,0x11,0x20,0x20,0x24,0x1C,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, +0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,/*"d:\Users\z\Pictures\应用\转尺寸\ZR12864w2.bmp",0*/ +}; + + bool Display::init(void) { // exit immediately if already initialised @@ -399,6 +590,9 @@ void Display::update() void Display::update_all() { + update_display_manual(); + return; + update_text(0); update_mode(1); update_battery(2); @@ -433,21 +627,62 @@ void Display::draw_char(uint16_t x, uint16_t y, const char c) { uint8_t line; + static uint8_t Size = 16; + uint8_t i,j; // draw char to pixel - for (uint8_t i = 0; i < 6; i++) { - if (i == 5) { - line = 0; - } else { - line = _font[(c * 5) + i]; + if(Size == 16) + { + for ( i = 0; i < 9; i++) { + if (i == 8) { + line = 0; + } else { + line = _font_16[(c * 16) + i]; + } + + for ( j = 0; j < 8; j++) { + if (line & 1) { + _driver->set_pixel(x + i, y + j ); + } else { + _driver->clear_pixel(x + i, y + j ); + } + line >>= 1; + } + } - for (uint8_t j = 0; j < 8; j++) { - if (line & 1) { - _driver->set_pixel(x + i, y + j); + for ( i = 0; i < 9; i++) { + if (i == 8) { + line = 0; } else { - _driver->clear_pixel(x + i, y + j); + line = _font_16[(c * 16) + 8 + i]; + } + + for ( j = 0; j < 8; j++) { + if (line & 1) { + _driver->set_pixel(x + i, y + 8 + j); + } else { + _driver->clear_pixel(x + i, y + 8 + j); + } + line >>= 1; + } + + } + }else{ + for ( i = 0; i < 6; i++) { + if (i == 5) { + line = 0; + } else { + line = _font[(c * 5) + i]; + } + + for ( j = 0; j < 8; j++) { + if (line & 1) { + _driver->set_pixel(x + i, y + j); + } else { + _driver->clear_pixel(x + i, y + j); + } + line >>= 1; } - line >>= 1; } } } @@ -523,9 +758,14 @@ void Display::update_ekf(uint8_t r) void Display::update_battery(uint8_t r) { + // char msg [DISPLAY_MESSAGE_SIZE]; + // snprintf(msg, DISPLAY_MESSAGE_SIZE, "BAT1: %4.2fV", (double)AP::battery().voltage()) ; + // draw_text(COLUMN(0), ROW(r), msg); + char msg [DISPLAY_MESSAGE_SIZE]; - snprintf(msg, DISPLAY_MESSAGE_SIZE, "BAT1: %4.2fV", (double)AP::battery().voltage()) ; + snprintf(msg, DISPLAY_MESSAGE_SIZE, "BAT1: %2.1fV,GPS:%2u", (double)AP::battery().voltage(), (unsigned)AP_Notify::flags.gps_num_sats) ; draw_text(COLUMN(0), ROW(r), msg); + } void Display::update_mode(uint8_t r) @@ -580,3 +820,204 @@ void Display::update_text(uint8_t r) draw_text(COLUMN(0), ROW(0), msg); } + + + +void Display::draw_chinese(uint8_t x,uint8_t y,unsigned char hz[][32],uint8_t no) +{ + uint8_t i,j,line = 0; + for ( i = 0; i < 17; i++) { + if (i == 16) { + line = 0; + } else { + line = hz[2 * no][i]; + } + + for ( j = 0; j < 8; j++) { + if (line & 1) { + _driver->set_pixel(x + i, y + j ); + } else { + _driver->clear_pixel(x + i, y + j ); + } + line >>= 1; + } + + } + + for ( i = 0; i < 17; i++) { + if (i == 16) { + line = 0; + } else { + line = hz[2 * no + 1][i]; + } + + for ( j = 0; j < 8; j++) { + if (line & 1) { + _driver->set_pixel(x + i, y + 8 + j); + } else { + _driver->clear_pixel(x + i, y + 8 + j); + } + line >>= 1; + } + + } +} + +void Display::draw_BMP(uint8_t x0,uint8_t y0,uint8_t x1,uint8_t y1,unsigned char BMP[]) +{ + uint16_t i = 0; + uint16_t j = 0; + uint8_t x,y; + uint8_t line = BMP[0]; + + // if(y1%8 == 0) + // y = y1/8; + // else + // y = y1/8 +1; + + for(y=0; y<64; y+=8) + { + for(x=0; x<128; x++) + { + + line = BMP[i++]; + for ( j = 0; j < 8; j++) { + if (line & 1) { + _driver->set_pixel(x, y + j); + } else { + _driver->clear_pixel(x + i, y + j); + } + line >>= 1; + } + } + } +} + + +void Display::display_clear(void) +{ + for(int i = 0; i < 64; i++) + { + for(int j = 0; j<128; j++){ + + _driver->clear_pixel(j , i); + } + } +} + + +void Display::update_chinese(uint8_t r) +{ + draw_chinese(COLUMN(0), ROW(r),ZRZKYT,0); + draw_chinese(COLUMN(3), ROW(r),ZRZKYT,1); + draw_chinese(COLUMN(6), ROW(r),ZRZKYT,2); + draw_chinese(COLUMN(9), ROW(r),ZRZKYT,3); + draw_chinese(COLUMN(12), ROW(r),ZRZKYT,4); + draw_chinese(COLUMN(15), ROW(r),ZRZKYT,5); +} + +void Display::update_camera(uint8_t r) +{ + + // char msg [DISPLAY_MESSAGE_SIZE]; + // snprintf(msg, DISPLAY_MESSAGE_SIZE, "BAT1: %2.2fV,GPS:%2u", (double)AP::battery().voltage(), (unsigned)AP_Notify::flags.gps_num_sats) ; + // draw_text(COLUMN(0), ROW(r), msg); + + char msg2 [DISPLAY_MESSAGE_SIZE]; + snprintf(msg2, DISPLAY_MESSAGE_SIZE, "IMG: %4d,L: %4d ", AP_Notify::flags.image_index,AP_Notify::flags.image_index_log) ; + draw_text(COLUMN(0), ROW(r), msg2); + + } + +void Display::update_display_manual() +{ + static uint8_t time_cnt; + + static uint8_t display_stage = 1; + + // if (time_cnt++ > 10) { + // time_cnt = 0; + // display_stage++; + // } + // if(display_stage>5) + // display_stage = 1; + RESTART1: + switch (display_stage) + { + case 1: + display_clear(); + display_stage = 2; + case 2: + draw_BMP(0,0,128,8,gImage_zr12864); // 绘制图片 + if (time_cnt++ > 10) { + time_cnt = 0; + display_stage++; + } + break; + case 3: + display_clear(); + display_stage = 4; + case 4: + // update_text(0); + update_chinese(0); + update_camera(2); + update_battery(4); + + break; + // case 5: + // // update_text(0); + // update_chinese(0); + // // update_gps(2); + // update_camera(4); + // break; + // case 6: + + // display_stage = 1; + // goto RESTART1; + // // update_text(0); + // update_chinese(0); + // update_mode(2); + // update_battery(4); + // break; + // case 7: + // // update_text(0); + // update_chinese(0); + // update_gps(2); + // update_camera(4); + // break; + // case 8: + // display_stage = 1; + // goto RESTART1; + + + default: + break; + } +} + + + +/* + check if feedback pin is high + */ +// void Display::trigger_pin_timer(void) +// { + // int8_t dpin = hal.gpio->analogPinToDigitalPin(_feedback_pin); + // if (dpin == -1) { + // return; + // } + // // ensure we are in input mode + // hal.gpio->pinMode(dpin, HAL_GPIO_INPUT); + + // // enable pullup + // hal.gpio->write(dpin, 1); + + // uint8_t pin_state = hal.gpio->read(dpin); + // uint8_t trigger_polarity = _feedback_polarity==0?0:1; + // if (pin_state == trigger_polarity && + // _last_pin_state != trigger_polarity) { + // _page_turn = true; + // } + // _last_pin_state = pin_state; +// } + diff --git a/libraries/AP_Notify/Display.h b/libraries/AP_Notify/Display.h index 25369e1c7d..c4f0feb8da 100644 --- a/libraries/AP_Notify/Display.h +++ b/libraries/AP_Notify/Display.h @@ -16,6 +16,8 @@ public: bool init(void) override; void update() override; + void update_display_manual(); + private: void draw_char(uint16_t x, uint16_t y, const char c); void draw_text(uint16_t x, uint16_t y, const char *c); @@ -30,6 +32,15 @@ private: void update_text(uint8_t r); void update_text_empty(uint8_t r); + + void draw_chinese(uint8_t x,uint8_t y,unsigned char hz[][32],uint8_t no); + void update_chinese(uint8_t r); + void update_camera(uint8_t r); + void display_clear(void); + void draw_BMP(uint8_t x0,uint8_t y0,uint8_t x1,uint8_t y1,unsigned char BMP[]); + + + Display_Backend *_driver; uint8_t _mstartpos; // ticker shift position @@ -38,5 +49,15 @@ private: // stop showing text in display after this many millis: const uint16_t _send_text_valid_millis = 20000; + + + /////////////////// @Brown /////////////////// + // void trigger_pin_timer(void); + // AP_Int8 _feedback_pin; + // AP_Int8 _feedback_polarity; + // uint8_t _last_pin_state; + // bool _page_turn; + // bool _timer_installed = false; + ///////////////////////////////////////////// };