/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ #pragma once /* This is the main Sub class */ //////////////////////////////////////////////////////////////////////////////// // Header includes //////////////////////////////////////////////////////////////////////////////// #include #include #include #include // Common dependencies #include #include #include #include #include #include // interface and maths for accelerometer calibration #include // ArduPilot Mega Vector/Matrix math Library #include // ArduPilot Mega Declination Helper Library // Application dependencies #include // Serial manager library #include // ArduPilot GPS library #include // ArduPilot Mega Flash Memory Library #include #include // ArduPilot Mega Magnetometer Library #include // ArduPilot Mega Inertial Sensor (accel & gyro) Library #include #include #include #include // Mission command library #include // P library #include // PID library #include // PI library (2-axis) #include // PID library (2-axis) #include // Attitude control library #include // Position control library #include // AP Motors library #include // Range finder library #include // Filter library #include // APM relay #include #include // Camera/Antenna mount #include // needed for AHRS build #include // ArduPilot Mega inertial navigation library #include // Waypoint navigation library #include #include // circle navigation library #include // Fence library #include // main loop scheduler #include // loop perf monitoring #include // Notify library #include // Battery monitor library #include // board configuration library #include #include #include // Joystick/gamepad button function assignment #include // Leak detector #include #include // Local modules #include "defines.h" #include "config.h" #include "GCS_Mavlink.h" #include "RC_Channel.h" // RC Channel Library #include "Parameters.h" #include "AP_Arming_Sub.h" #include "GCS_Sub.h" //------self define start------------------- #include "Gearmotorpid.h" #define OUSHENCAN_MAX_NUM_ESCS 6 #ifndef USERHOOK_50HZLOOP #define USERHOOK_50HZLOOP 1 #endif #ifndef USERHOOK_MEDIUMLOOP #define USERHOOK_MEDIUMLOOP 1 #endif #define startval 0 #define Speedmax_hand 62 #define Speedmax_hand_f 62.0 #define speedmin 31 #define maxerror 41//31 #define maxerror_f 41.0//31.0 #define Horizontal 0 #define Vertical 1 #define section0 0 #define section90 1 #define section_90 2 #define action_Hor 0 #define action_Ver_postive 1 #define action_Ver_negtive 2 #define Orign 0 #define foward 1 #define backward 2 #define Speedmax 60 #define STARTCONT 2000 //压力等级 包含下压和上浮总共10级 第五级为1500 即中值 enum PressNetLevel { first = 0, second = 1, third = 2, forth = 3, fifth = 4, no =5, sixth = 6, seventh = 7, eighth = 8, ninth = 9, tenth = 10 }; #define SaberDatapacketID 0x8106 #define RespAllStatusID 0x8602 #define SystemResetAckID 0x8401 #define Saber_selftest_Failed 0x08 //自检失败 #define Saber_WakeUpHost 0x0101 //WakeUp模式 #define Saber_MesureMode 0x02 //测量模式 //usigned char to float将char类型转换成float类型 typedef union{ unsigned char u[4]; float f; }uTof; //------self define end------------------- // libraries which are dependent on #defines in defines.h and/or config.h #if OPTFLOW == ENABLED #include // Optical Flow library #endif #if RCMAP_ENABLED == ENABLED #include // RC input mapping library #endif #if RPM_ENABLED == ENABLED #include #endif #if GRIPPER_ENABLED == ENABLED #include // gripper stuff #endif #if PROXIMITY_ENABLED == ENABLED #include #endif #if AVOIDANCE_ENABLED == ENABLED #include // Stop at fence library #endif #if AC_RALLY == ENABLED #include // Rally point library #endif #if CAMERA == ENABLED #include // Photo or video camera #endif #ifdef ENABLE_SCRIPTING #include #endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include #endif class Sub : public AP_HAL::HAL::Callbacks { public: friend class GCS_MAVLINK_Sub; friend class GCS_Sub; friend class Parameters; friend class ParametersG2; friend class AP_Arming_Sub; friend class RC_Channels_Sub; Sub(void); // HAL::Callbacks implementation. void setup() override; void loop() override; private: static const AP_FWVersion fwver; // key aircraft parameters passed to multiple libraries AP_Vehicle::MultiCopter aparm; // Global parameters are all contained within the 'g' class. Parameters g; ParametersG2 g2; // main loop scheduler AP_Scheduler scheduler{FUNCTOR_BIND_MEMBER(&Sub::fast_loop, void)}; // AP_Notify instance AP_Notify notify; // primary input control channels RC_Channel *channel_roll; RC_Channel *channel_pitch; RC_Channel *channel_throttle; RC_Channel *channel_yaw; RC_Channel *channel_forward; RC_Channel *channel_lateral; AP_Logger logger; AP_GPS gps; AP_LeakDetector leak_detector; TSYS01 celsius; Compass compass; AP_InertialSensor ins; RangeFinder rangefinder; struct { bool enabled:1; bool alt_healthy:1; // true if we can trust the altitude from the rangefinder int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder uint32_t last_healthy_ms; LowPassFilterFloat alt_cm_filt; // altitude filter } rangefinder_state = { false, false, 0, 0 }; #if RPM_ENABLED == ENABLED AP_RPM rpm_sensor; #endif // Inertial Navigation EKF NavEKF2 EKF2{&ahrs, rangefinder}; NavEKF3 EKF3{&ahrs, rangefinder}; AP_AHRS_NavEKF ahrs{EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF}; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL SITL::SITL sitl; #endif // Mission library AP_Mission mission{ FUNCTOR_BIND_MEMBER(&Sub::start_command, bool, const AP_Mission::Mission_Command &), FUNCTOR_BIND_MEMBER(&Sub::verify_command_callback, bool, const AP_Mission::Mission_Command &), FUNCTOR_BIND_MEMBER(&Sub::exit_mission, void)}; // Optical flow sensor #if OPTFLOW == ENABLED OpticalFlow optflow; #endif // system time in milliseconds of last recorded yaw reset from ekf uint32_t ekfYawReset_ms = 0; AP_SerialManager serial_manager; // GCS selection GCS_Sub _gcs; // avoid using this; use gcs() GCS_Sub &gcs() { return _gcs; } // User variables #ifdef USERHOOK_VARIABLES # include USERHOOK_VARIABLES #endif // Documentation of Globals: union { struct { uint8_t pre_arm_check : 1; // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed uint8_t logging_started : 1; // true if logging has started uint8_t compass_mot : 1; // true if we are currently performing compassmot calibration uint8_t motor_test : 1; // true if we are currently performing the motors test uint8_t initialised : 1; // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes uint8_t gps_base_pos_set : 1; // true when the gps base position has been set (used for RTK gps only) uint8_t at_bottom : 1; // true if we are at the bottom uint8_t at_surface : 1; // true if we are at the surface uint8_t depth_sensor_present: 1; // true if there is a depth sensor detected at boot uint8_t compass_init_location:1; // true when the compass's initial location has been set }; uint32_t value; } ap; // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, #if RCMAP_ENABLED == ENABLED RCMapper rcmap; #endif // board specific config AP_BoardConfig BoardConfig; #if HAL_WITH_UAVCAN // board specific config for CAN bus AP_BoardConfig_CAN BoardConfig_CAN; #endif // Failsafe struct { uint32_t last_leak_warn_ms; // last time a leak warning was sent to gcs uint32_t last_gcs_warn_ms; uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe uint32_t last_pilot_input_ms; // last time we received pilot input in the form of MANUAL_CONTROL or RC_CHANNELS_OVERRIDE messages uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed uint32_t last_crash_warn_ms; // last time a crash warning was sent to gcs uint32_t last_ekf_warn_ms; // last time an ekf warning was sent to gcs uint8_t pilot_input : 1; // true if pilot input failsafe is active, handles things like joystick being disconnected during operation uint8_t gcs : 1; // A status flag for the ground station failsafe uint8_t ekf : 1; // true if ekf failsafe has occurred uint8_t terrain : 1; // true if the missing terrain data failsafe has occurred uint8_t leak : 1; // true if leak recently detected uint8_t internal_pressure : 1; // true if internal pressure is over threshold uint8_t internal_temperature : 1; // true if temperature is over threshold uint8_t crash : 1; // true if we are crashed uint8_t sensor_health : 1; // true if at least one sensor has triggered a failsafe (currently only used for depth in depth enabled modes) } failsafe; bool any_failsafe_triggered() const { return (failsafe.pilot_input || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain); } // sensor health for logging struct { uint8_t baro : 1; // true if any single baro is healthy uint8_t depth : 1; // true if depth sensor is healthy uint8_t compass : 1; // true if compass is healthy } sensor_health; // Baro sensor instance index of the external water pressure sensor uint8_t depth_sensor_idx; // GPS variables // Sometimes we need to remove the scaling for distance calcs float scaleLongDown; // Auto AutoMode auto_mode; // controls which auto controller is run // Guided GuidedMode guided_mode; // controls which controller is run (pos or vel) // Circle bool circle_pilot_yaw_override; // true if pilot is overriding yaw // Stores initial bearing when armed int32_t initial_armed_bearing; // Throttle variables int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only // Loiter control uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) uint32_t loiter_time; // How long have we been loitering - The start time in millis // Delay the next navigation command uint32_t nav_delay_time_max_ms; // used for delaying the navigation commands uint32_t nav_delay_time_start_ms; // Battery Sensors AP_BattMonitor battery{MASK_LOG_CURRENT, FUNCTOR_BIND_MEMBER(&Sub::handle_battery_failsafe, void, const char*, const int8_t), _failsafe_priorities}; // Altitude // The cm/s we are moving up or down based on filtered data - Positive = UP int16_t climb_rate; float target_rangefinder_alt; // desired altitude in cm above the ground bool holding_depth; // Turn counter int32_t quarter_turn_count; uint8_t last_turn_state; // Input gain float gain; // Flag indicating if we are currently using input hold bool input_hold_engaged; // 3D Location vectors // Current location of the Sub (altitude is relative to home) Location current_loc; // Navigation Yaw control // auto flight mode's yaw mode uint8_t auto_yaw_mode; // Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI Vector3f roi_WP; // bearing from current location to the yaw_look_at_WP float yaw_look_at_WP_bearing; float yaw_xtrack_correct_heading; // yaw used for YAW_LOOK_AT_HEADING yaw_mode int32_t yaw_look_at_heading; // Deg/s we should turn int16_t yaw_look_at_heading_slew; // heading when in yaw_look_ahead_bearing float yaw_look_ahead_bearing; // Delay Mission Scripting Command int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) uint32_t condition_start; // IMU variables // Integration time (in seconds) for the gyros (DCM algorithm) // Updated with the fast loop float G_Dt; // Inertial Navigation AP_InertialNav_NavEKF inertial_nav; AP_AHRS_View ahrs_view; // Attitude, Position and Waypoint navigation objects // To-Do: move inertial nav up or other navigation variables down here AC_AttitudeControl_Sub attitude_control; AC_PosControl_Sub pos_control; AC_WPNav wp_nav; AC_Loiter loiter_nav; AC_Circle circle_nav; // Reference to the relay object AP_Relay relay; // handle repeated servo and relay events AP_ServoRelayEvents ServoRelayEvents; // Camera #if CAMERA == ENABLED AP_Camera camera{MASK_LOG_CAMERA, current_loc}; #endif // Camera/Antenna mount tracking and stabilisation stuff #if MOUNT == ENABLED // current_loc uses the baro/gps soloution for altitude rather than gps only. AP_Mount camera_mount{current_loc}; #endif // AC_Fence library to reduce fly-aways #if AC_FENCE == ENABLED AC_Fence fence; #endif #if AVOIDANCE_ENABLED == ENABLED AC_Avoid avoid; #endif // Rally library #if AC_RALLY == ENABLED AP_Rally rally; #endif // terrain handling #if AP_TERRAIN_AVAILABLE && AC_TERRAIN AP_Terrain terrain{mission}; #endif // used to allow attitude and depth control without a position system struct attitude_no_gps_struct { uint32_t last_message_ms; mavlink_set_attitude_target_t packet; }; attitude_no_gps_struct set_attitude_target_no_gps {0}; // Top-level logic // setup the var_info table AP_Param param_loader; uint32_t last_pilot_heading; uint32_t last_input_ms; uint32_t last_input_ms_stable; int32_t last_roll_s; int32_t last_pitch_s; int32_t last_yaw_s; int32_t last_roll; int32_t last_pitch; int32_t last_yaw; uint32_t last_pilot_yaw_input_ms; uint32_t fs_terrain_recover_start_ms; static const AP_Scheduler::Task scheduler_tasks[]; static const AP_Param::Info var_info[]; static const struct LogStructure log_structure[]; void fast_loop(); void fifty_hz_loop(); void update_batt_compass(void); void ten_hz_logging_loop(); void twentyfive_hz_logging(); void three_hz_loop(); void one_hz_loop(); void update_GPS(void); void update_turn_counter(); void read_AHRS(void); void update_altitude(); float get_smoothing_gain(); void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max); float get_pilot_desired_yaw_rate(int16_t stick_angle); void check_ekf_yaw_reset(); float get_roi_yaw(); float get_look_ahead_yaw(); float get_pilot_desired_climb_rate(float throttle_control); float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); void update_poscon_alt_max(); void rotate_body_frame_to_NE(float &x, float &y); void send_heartbeat(mavlink_channel_t chan); #if RPM_ENABLED == ENABLED void rpm_update(); #endif void Log_Write_Control_Tuning(); void Log_Write_Performance(); void Log_Write_Attitude(); void Log_Write_MotBatt(); void Log_Write_Event(Log_Event id); void Log_Write_Data(uint8_t id, int32_t value); void Log_Write_Data(uint8_t id, uint32_t value); void Log_Write_Data(uint8_t id, int16_t value); void Log_Write_Data(uint8_t id, uint16_t value); void Log_Write_Data(uint8_t id, float value); void Log_Sensor_Health(); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); void Log_Write_Vehicle_Startup_Messages(); void load_parameters(void); void userhook_init(); void userhook_FastLoop(); void userhook_50Hz(); void userhook_MediumLoop(); void userhook_SlowLoop(); void userhook_SuperSlowLoop(); void update_home_from_EKF(); void set_home_to_current_location_inflight(); bool set_home_to_current_location(bool lock) WARN_IF_UNUSED; bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED; bool far_from_EKF_origin(const Location& loc); void exit_mission(); bool verify_loiter_unlimited(); bool verify_loiter_time(); bool verify_wait_delay(); bool verify_within_distance(); bool verify_yaw(); bool acro_init(void); void acro_run(); void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); bool althold_init(void); void althold_run(); // Handles attitude control for stabilize and althold modes void handle_attitude(); void handle_attitude_stablize(); void handle_attitude_sport(); bool auto_init(void); void auto_run(); void auto_wp_start(const Vector3f& destination); void auto_wp_start(const Location& dest_loc); void auto_wp_run(); void auto_spline_run(); void auto_circle_movetoedge_start(const Location &circle_center, float radius_m); void auto_circle_start(); void auto_circle_run(); void auto_nav_guided_start(); void auto_nav_guided_run(); bool auto_loiter_start(); void auto_loiter_run(); uint8_t get_default_auto_yaw_mode(bool rtl); void set_auto_yaw_mode(uint8_t yaw_mode); void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle); void set_auto_yaw_roi(const Location &roi_location); float get_auto_heading(void); bool circle_init(void); void circle_run(); bool guided_init(bool ignore_checks = false); void guided_pos_control_start(); void guided_vel_control_start(); void guided_posvel_control_start(); void guided_angle_control_start(); bool guided_set_destination(const Vector3f& destination); bool guided_set_destination(const Location& dest_loc); void guided_set_velocity(const Vector3f& velocity); bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); void guided_set_angle(const Quaternion &q, float climb_rate_cms); void guided_run(); void guided_pos_control_run(); void guided_vel_control_run(); void guided_posvel_control_run(); void guided_angle_control_run(); void guided_limit_clear(); void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); void guided_limit_init_time_and_pos(); bool guided_limit_check(); bool poshold_init(void); void poshold_run(); bool motordetect_init(); void motordetect_run(); bool stabilize_init(void); void stabilize_run(); bool manual_init(void); void manual_run(); void failsafe_sensors_check(void); void failsafe_crash_check(); void failsafe_ekf_check(void); void handle_battery_failsafe(const char* type_str, const int8_t action); void failsafe_gcs_check(); void failsafe_pilot_input_check(void); void set_neutral_controls(void); void failsafe_terrain_check(); void failsafe_terrain_set_status(bool data_ok); void failsafe_terrain_on_event(); void mainloop_failsafe_enable(); void mainloop_failsafe_disable(); void fence_check(); bool set_mode(control_mode_t mode, mode_reason_t reason); void update_flight_mode(); void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode); bool mode_requires_GPS(control_mode_t mode); bool mode_has_manual_throttle(control_mode_t mode); bool mode_allows_arming(control_mode_t mode, bool arming_from_gcs); void notify_flight_mode(control_mode_t mode); void read_inertia(); void update_surface_and_bottom_detector(); void set_surfaced(bool at_surface); void set_bottomed(bool at_bottom); void motors_output(); Vector3f pv_location_to_vector(const Location& loc); float pv_alt_above_origin(float alt_above_home_cm); void init_rc_in(); void init_rc_out(); void enable_motor_output(); void init_joystick(); void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons); void handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false); void handle_jsbutton_release(uint8_t button, bool shift); JSButton* get_button(uint8_t index); void default_js_buttons(void); void clear_input_hold(); void read_barometer(void); void init_rangefinder(void); void read_rangefinder(void); bool rangefinder_alt_ok(void); #if OPTFLOW == ENABLED void init_optflow(); #endif void terrain_update(); void terrain_logging(); bool terrain_use(); void init_ardupilot(); void startup_INS_ground(); bool position_ok(); bool ekf_position_ok(); bool optflow_position_ok(); bool should_log(uint32_t mask); bool start_command(const AP_Mission::Mission_Command& cmd); bool verify_command(const AP_Mission::Mission_Command& cmd); bool verify_command_callback(const AP_Mission::Mission_Command& cmd); bool do_guided(const AP_Mission::Mission_Command& cmd); void do_nav_wp(const AP_Mission::Mission_Command& cmd); void do_surface(const AP_Mission::Mission_Command& cmd); void do_RTL(void); void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); void do_circle(const AP_Mission::Mission_Command& cmd); void do_loiter_time(const AP_Mission::Mission_Command& cmd); void do_spline_wp(const AP_Mission::Mission_Command& cmd); #if NAV_GUIDED == ENABLED void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd); void do_guided_limits(const AP_Mission::Mission_Command& cmd); #endif void do_nav_delay(const AP_Mission::Mission_Command& cmd); void do_wait_delay(const AP_Mission::Mission_Command& cmd); void do_within_distance(const AP_Mission::Mission_Command& cmd); void do_yaw(const AP_Mission::Mission_Command& cmd); void do_change_speed(const AP_Mission::Mission_Command& cmd); void do_set_home(const AP_Mission::Mission_Command& cmd); void do_roi(const AP_Mission::Mission_Command& cmd); void do_mount_control(const AP_Mission::Mission_Command& cmd); bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); bool verify_surface(const AP_Mission::Mission_Command& cmd); bool verify_RTL(void); bool verify_circle(const AP_Mission::Mission_Command& cmd); bool verify_spline_wp(const AP_Mission::Mission_Command& cmd); #if NAV_GUIDED == ENABLED bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd); #endif bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); void auto_spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination); void log_init(void); void accel_cal_update(void); void failsafe_leak_check(); void failsafe_internal_pressure_check(); void failsafe_internal_temperature_check(); void failsafe_terrain_act(void); bool auto_terrain_recover_start(void); void auto_terrain_recover_run(void); void translate_wpnav_rp(float &lateral_out, float &forward_out); void translate_circle_nav_rp(float &lateral_out, float &forward_out); void translate_pos_control_rp(float &lateral_out, float &forward_out); bool surface_init(void); void surface_run(); uint16_t get_pilot_speed_dn(); void convert_old_parameters(void); bool handle_do_motor_test(mavlink_command_long_t command); bool init_motor_test(); bool verify_motor_test(); uint32_t last_do_motor_test_fail_ms = 0; uint32_t last_do_motor_test_ms = 0; bool control_check_barometer(); enum Failsafe_Action { Failsafe_Action_None = 0, Failsafe_Action_Warn = 1, Failsafe_Action_Disarm = 2, Failsafe_Action_Surface = 3 }; static constexpr int8_t _failsafe_priorities[] = { Failsafe_Action_Disarm, Failsafe_Action_Surface, Failsafe_Action_Warn, Failsafe_Action_None, -1 // the priority list must end with a sentinel of -1 }; static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, "_failsafe_priorities is missing the sentinel"); public: void mavlink_delay_cb(); void mainloop_failsafe_check(); //------------selfdefine START----------------------------- control_mode_t control_mode; mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN; control_mode_t prev_control_mode; mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN; TrackPidClass trackpid;//履带pid控制对象 //------ATOM ------------ float GyroX;//roll角速度 float GyroY;//pitch角速度 float GyroZ;//yaw角速度 float Roll;//roll角度 float Pitch;//pitch角度 float Yaw;//yaw角度 // uint8_t agl_sec;//机器人俯仰所处于的角度区间 uint8_t agl_act;//机器人当前的姿态动作 水平 or 竖直 //----ATOM--------- unsigned char ucRxBufferATOM[256];//原子九轴接收缓冲区 unsigned char SaberCommandRes[24];//原子配置数据 unsigned char ucRxCnt_atom ;//原子数据接收个数 bool usart_state_atom;//原子九轴通信状态 void updat_Atom(void);//原子九轴数据更新 void GetAngle(void);//计算角度 void uart2_read_Atom(AP_HAL::UARTDriver *uart);//USART2即是temlem2 读取 atom接在temlem2上 void Atom_config(AP_HAL::UARTDriver *uart);//配置 unsigned char Atom_BBC(unsigned char *addr,uint16_t len);//异或校验 float char_to_float(unsigned char u1,unsigned char u2,unsigned char u3,unsigned char u4); void direction0_90(void); //------九轴读到的数---------------------- float Roll_Raian;//roll角度弧度 float Pitch_Raian;//pitch角度弧度 float Yaw_Raian;//yaw角度弧度 float Yaw_Angle;//yaw角度，纠正了安装角度 uTof data_floatfromchar; AP_Motors6DOF motors; AP_Arming_Sub arming; AP_Baro barometer; //灯光 int16_t lights; //---------USBL --------------- uint8_t usblpoweroff; void USBL_PowerSwitch(void); //------------------- int16_t yaw_press;//yaw角度给定 void getyaw(void); void getgain(void); //---------track---------------- int16_t brushleft;//左毛刷 int16_t brushright;//右毛刷 int16_t motor1_speed_target;//履带电机1 目标PWM int16_t motor2_speed_target;//履带电机2 目标PWM float turn_angle; float track_head_gd;//履带的方位角度给定 uint8_t prepare_state;//水平或者竖直命令 int16_t min_depth;// 最小深度 int16_t max_depth;//最大深度 int16_t autoclean_orgin;//自动洗网开始时，机器人的深度 uint8_t autoclean_step;//自动洗网的阶段：开始 、向上、向下 bool autoclean_flag;//自动洗网状态 bool autoclean_command;//自动洗网状态 bool handclean; uint8_t clean_mode; bool clean_bottom_flag;//清洗底网 bool clean_bottom_command; uint8_t track_motor_arm;//测试履带时的前进2 后退0 停1 PressNetLevel PressLevel;//压力分级枚举类型 float PressLevel_f;//压力分级float类型 float updowmgain; float delaygain; int16_t pitch_input_inc;//pitch给定 int16_t lights1; bool stable_alt_control; uint16_t continuous_count; uint16_t continuous_countdec; uint8_t last_continuous; float velocity_z_filer; bool bottom_set; Matrix3f rotpitch; Matrix3f rotyaw; struct telemetry_info_t { int16_t dutycycle; int16_t totalcurrent; int32_t rpm;// 速度 int32_t amphours; int32_t watthours; int16_t toalcurrentIn; int16_t mottremperature; int16_t mostemperature; int16_t voltage; } _telemetry[OUSHENCAN_MAX_NUM_ESCS]; bool sport_init(void); void sport_run(); void sport_run_alt(void); bool clean_init(void); void clean_run(); void track_reset(void); void autoclean_flag_chose(void); void clean_net_joystick(void); void clean_sidenet_auto(void); void slowly_speed1(int16_t &p1, int16_t p2,int16_t step,int16_t per) ; void slowly_speed2(int16_t &p1, int16_t p2,int16_t step,int16_t per) ; float Constrate1(float d1); void motor_toCan(void); void clean_sidenet_state(void); void clean_sidenet_run(void); void track_pidcontrol(float _targethead,int16_t &_motor1,int16_t &_motor2); float get_yaw_error(float yaw_heading); float getgainf(float data); float getgainfstatble(float data); bool throttle_continuous(float gain1); float surface_depth_use(void); float gaindelay(float gain1); void altcontrol_ff(); void altcontrol(); void ratecontrol(); float bottom_detector(float z); void bottom_detectorgain(void); void get_heading_to_ned(Matrix3f &mat_half,Matrix3f mat_body); float get_pitch_to_ned(Matrix3f mat_body); float sign_in(float f); unsigned char RxDVL[240];//惯导接收缓冲区 unsigned char TxDVL[51];//惯导接收缓冲区 unsigned char DVL_RxCnt ;//数据接收个数 float arryf[20] ; int64_t timearry[3]; struct wrz_t { float Vx;//x方向的速度 m/s float Vy;//y方向的速度 m/s float Vz;//z方向的速度 m/s char valid;//当DVL锁定在反射物表面时，y表示高度和速度有效 y/n float altitude;//高度 m float fom;//测量经度 m/s float covariancearr[9];//协方差矩阵 int64_t timeofvalidity;//相当于ping的时间 微秒 int64_t timeoftransmission;//微秒 TCP float time; //两次报告中间的时间ms int status;//0表示正常，1表示有问题比如温度过高 } DVLdata_wrz; struct wru_t { int id;//换能器的号 float velocity;//换能器方向上的速度，如果没有接收到反馈信号 0 float distance; //换能器和反射物体的平行距离 如果没有接收到反馈信号 -1 int rssi;//信号强度 DBM int nsd;//噪音强度dbm } DVLdata_wru; struct wrp_t { int64_t time_stamp;//报告的时间戳 unix时间 s int16_t time_Mulfactor; float Dx;//x方向距离 m float Dy;//y方向距离 m float Dz;//z方向距离 m float pos_std;//标准差 m float roll;//度 float pitch; float yaw; int status;//0没有error 1表示其他 } DVLdata_wrp; void read_external_DVL(AP_HAL::UARTDriver *uart); void write_external_DVL(AP_HAL::UARTDriver *uart); int chartodec(unsigned char _ch); void transfer_position_delta_msg(void); void charArrtobyteArr(unsigned char *charArr ,uint8_t * dec,int16_t len); void charArrtodecArr(unsigned char *charArr ,uint8_t * dec,int16_t len); }; extern const AP_HAL::HAL& hal; extern Sub sub; extern AP_HAL::AnalogSource* chan_adc;