diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 25c62d1ca6..3bfbbe5b03 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -14,7 +14,7 @@ //#include #endif -#define CAM_DEBUG 0 +#define CAM_DEBUG 1 #ifdef USERHOOK_INIT void Copter::userhook_init() @@ -122,11 +122,12 @@ void Copter::userhook_SuperSlowLoop() last_manual_avoid = avoid.get_avoid_action(); } #if CAM_DEBUG - if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO){ - camera.take_picture(); - } -#endif - if(1) + // if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO){ + // camera.take_picture(); + // } + // if(1) + // 拍照发送角度测试 + if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO) { static uint8_t ccnt; static float cpitch = 0.0; @@ -168,6 +169,7 @@ void Copter::userhook_SuperSlowLoop() ccnt = 0; } +#endif } #endif @@ -187,11 +189,12 @@ void Copter::userhook_auxSwitch1(uint8_t ch_flag) // break; // } // } +#if CAM_DEBUG switch (ch_flag) { case 2: { // relay.on(2); gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 2!"); - in_debug_mode = 1; + in_debug_mode = 2; break; } // case 1: { @@ -207,12 +210,12 @@ void Copter::userhook_auxSwitch1(uint8_t ch_flag) break; } } -#if CAM_DEBUG +#else switch (ch_flag) { case 2: { // relay.on(2); gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 2!"); - in_debug_mode = 2; + in_debug_mode = 1; break; } // case 1: { @@ -252,6 +255,18 @@ void Copter::zr_set_uav_state_to_uavcan() { zr_uav_state_data_t data{0}; data.armd = motors->armed(); + +#if CAM_DEBUG + static bool into_debug = false; + if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO) + { + into_debug = true; + copter.updownStatus = UpDown_InMission; // 任务模式云台才转角度 + }else if(into_debug){ + copter.updownStatus = UpDown_RequestDescent; // 降落阶段角度回到水平用 + into_debug = false; + } +#endif data.fly_status = copter.updownStatus; data.gps_state = AP::gps().status(); data.wp_number = g.hardware_flag&0xFF;//硬件版本 diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 2db85caf25..af79443e66 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -499,8 +499,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) 0, (angle - ahrs.yaw_sensor/100.0f), 0); - - // gcs().send_text(MAV_SEVERITY_INFO, "**GIMBAL yaw:%.2f,%d",angle,ahrs.yaw_sensor); } break; diff --git a/hrs100h.sh b/hrs100h.sh index 5031ffdf5e..4a60ff3536 100755 --- a/hrs100h.sh +++ b/hrs100h.sh @@ -1,3 +1,3 @@ ./waf configure --board rs100h ./waf copter -cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/rs100h-v4.1.18.px4 +cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/rs100h-v4.1.19.px4 diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 5ea49fce06..c42fcf2558 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -184,21 +184,41 @@ AP_Camera::trigger_pic_cleanup() _trigger_counter--; } else { switch (_trigger_type) { - case AP_CAMERA_TRIGGER_TYPE_SERVO: - SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _servo_off_pwm); - break; - case AP_CAMERA_TRIGGER_TYPE_RELAY: { - AP_Relay *_apm_relay = AP::relay(); - if (_apm_relay == nullptr) { + case AP_CAMERA_TRIGGER_TYPE_SERVO: + SRV_Channels::set_output_pwm(SRV_Channel::k_cam_trigger, _servo_off_pwm); + break; + case AP_CAMERA_TRIGGER_TYPE_RELAY: { + AP_Relay *_apm_relay = AP::relay(); + if (_apm_relay == nullptr) { + break; + } + if (_relay_on) { + _apm_relay->off(0); + } else { + _apm_relay->on(0); + } break; } - if (_relay_on) { - _apm_relay->off(0); - } else { - _apm_relay->on(0); - } - break; - } + case AP_CAMERA_TRIGGER_TYPE_UAVCAN_ATT:{ // 角度和拍照分开发送 + // #if HAL_WITH_UAVCAN + // if(new_att){ + // uint8_t can_num_drivers = AP::can().get_num_drivers(); + // for (uint8_t i = 0; i < can_num_drivers; i++) + // { + // AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); + // if (uavcan != nullptr) + // { + // uavcan->set_cam_angular(camera_state.id,2,_camera_pitch,0,_camera_yaw); + // } + // } + // new_att = false; + // } + + // #endif + } + break; + default: + break; } } @@ -525,10 +545,11 @@ void AP_Camera::uavcan_pic_att() AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); if (uavcan != nullptr) { - uavcan->set_zr_cam_cmd(camera_state.id, mode, 1, _image_index&0xff, _image_index>>8, 0); uavcan->set_cam_angular(camera_state.id,2,_camera_pitch,0,_camera_yaw); + uavcan->set_zr_cam_cmd(camera_state.id, mode, 1, _image_index&0xff, _image_index>>8, 0); } } + // new_att = true; _trigger_counter = constrain_int16(_trigger_duration * 5, 0, 255); #endif } diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index dcec943a18..7de98fbbd8 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -95,8 +95,8 @@ public: enum trigger_types { AP_CAMERA_TRIGGER_TYPE_SERVO, AP_CAMERA_TRIGGER_TYPE_RELAY, - AP_CAMERA_TRIGGER_TYPE_UAVCAN, - AP_CAMERA_TRIGGER_TYPE_UAVCAN_ATT + AP_CAMERA_TRIGGER_TYPE_UAVCAN_ATT, + AP_CAMERA_TRIGGER_TYPE_UAVCAN }; @@ -214,6 +214,7 @@ private: float _camera_pitch; // 航线传入相机姿态pitch float _camera_yaw; // 航线传入相机姿态yaw // void send_can_msg(void); + bool new_att; }; namespace AP { AP_Camera *camera();