diff --git a/ArduCopter/zr_flight.cpp b/ArduCopter/zr_flight.cpp index 7a296ac010..20713567f3 100644 --- a/ArduCopter/zr_flight.cpp +++ b/ArduCopter/zr_flight.cpp @@ -70,7 +70,10 @@ void Copter::zr_SuperSlowLoop(){ }; if(motors->armed()){ - before_fly = false; + if(before_fly){ + before_fly = false; + camera.create_new_folder(); + } relay.on(3); }else{ diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 6216ea316e..3fc197fee7 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -495,15 +495,15 @@ void AP_Camera::send_can_msg(void) switch (send_msg_step) { case 1: - uavcan->send_zr_cam_cmd(camera_state.id,camera_can_msg.mode,1,_image_index,0,0); + uavcan->set_zr_cam_cmd(camera_state.id,camera_can_msg.mode,1,_image_index,0,0); send_msg_step = 2; break; case 2: - uavcan->send_cam_geopoi(camera_state.id,camera_can_msg.mode,camera_can_msg.lng,camera_can_msg.lat,camera_can_msg.alt,0); + uavcan->set_cam_geopoi(camera_state.id,camera_can_msg.mode,camera_can_msg.lng,camera_can_msg.lat,camera_can_msg.alt,0); send_msg_step = 3; break; case 3: - uavcan->send_cam_angular(camera_state.id, camera_can_msg.mode,camera_can_msg.pitch,camera_can_msg.roll,camera_can_msg.yaw); + uavcan->set_cam_angular(camera_state.id, camera_can_msg.mode,camera_can_msg.pitch,camera_can_msg.roll,camera_can_msg.yaw); send_msg_step = 0; break; @@ -525,7 +525,7 @@ void AP_Camera::send_uavcan_camera_zr_control(const mavlink_camera_zr_control_t { AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); if (uavcan != nullptr) { - uavcan->send_zr_cam_cmd(camera_state.id,packet.mode, packet.cmd1, packet.cmd2, packet.cmd3, packet.cmd4); + uavcan->set_zr_cam_cmd(camera_state.id,packet.mode, packet.cmd1, packet.cmd2, packet.cmd3, packet.cmd4); } } #endif @@ -560,7 +560,7 @@ void AP_Camera::request_camera_sn(void){ AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); if (uavcan != nullptr) { - uavcan->send_zr_cam_cmd(camera_state.id, 19, 1, 0, 0, 0); + uavcan->set_zr_cam_cmd(camera_state.id, 19, 1, 0, 0, 0); } } #endif @@ -616,6 +616,24 @@ void AP_Camera::send_camera_zr_status(mavlink_channel_t chan) camera_state.temperature); } + +void AP_Camera::create_new_folder() +{ +#if HAL_WITH_UAVCAN + 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_zr_cam_cmd(camera_state.id, 10, 1, 0, 0, 0); + } + + } +#endif + +} + // struct Camera_State & AP_Camera::get_camera_state() // { // return camera_state; diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 88627cd490..67f5f3ef22 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -74,8 +74,10 @@ public: void notice_send_camera_status_to_gcs(void); void update_zr_camera_version(void); void send_camera_zr_status(mavlink_channel_t chan); + void create_new_folder(); + static const struct AP_Param::GroupInfo var_info[]; - + // set if vehicle is in AUTO mode void set_is_auto_mode(bool enable) { @@ -92,7 +94,7 @@ public: // 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 struct Camera_State diff --git a/libraries/AP_Notify/UAVCAN_RGB_LED.cpp b/libraries/AP_Notify/UAVCAN_RGB_LED.cpp index c505e5ef61..bcfe7d4b27 100644 --- a/libraries/AP_Notify/UAVCAN_RGB_LED.cpp +++ b/libraries/AP_Notify/UAVCAN_RGB_LED.cpp @@ -77,8 +77,8 @@ bool UAVCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); if (uavcan != nullptr) { // success = uavcan->led_write(_led_index, red, green, blue) || success; - // success = uavcan->send_zr_rgb_led(rgb_mode,red, green, blue) || success;//(_led_index, red, green, blue) || success - success = uavcan->send_zr_rgb_led(rgb_mode,red, green, blue) || success;//(_led_index, red, green, blue) || success + // success = uavcan->set_zr_rgb_led(rgb_mode,red, green, blue) || success;//(_led_index, red, green, blue) || success + success = uavcan->set_zr_rgb_led(rgb_mode,red, green, blue) || success;//(_led_index, red, green, blue) || success } } return success; diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 8104188c49..8fce8109c1 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -296,15 +296,15 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) zr_camera_command[driver_index] = new uavcan::Publisher(*_node); - zr_camera_command[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); + zr_camera_command[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); zr_camera_command[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); cam_angular[driver_index] = new uavcan::Publisher(*_node); - cam_angular[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); + cam_angular[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); cam_angular[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); cam_geopoi[driver_index] = new uavcan::Publisher(*_node); - cam_geopoi[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); + cam_geopoi[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); cam_geopoi[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); safety_button_listener[driver_index] = new uavcan::Subscriber(*_node); @@ -399,11 +399,15 @@ void AP_UAVCAN::loop(void) _SRV_conf[i].esc_pending = false; } } - + zr_cam_cmd_send(); + cam_aular_send(); + cam_geopoi_send(); + zr_rgb_led_send(); led_out_send(); buzzer_send(); rtcm_stream_send(); safety_state_send(); + AP::uavcan_server().verify_nodes(this); } } @@ -769,55 +773,130 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con adsb->handle_adsb_vehicle(vehicle); } -bool AP_UAVCAN::send_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue){ - zrzk::equipment::indication::ZrRGB msg; - msg.rgb_mode = mode; - msg.rgb_r = red; - msg.rgb_g = green; - msg.rgb_b = blue; - zr_rgb_led[_driver_index]->broadcast(msg); - return true; +bool AP_UAVCAN::set_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue) +{ + WITH_SEMAPHORE(_zr_rgb_led.sem); + + _zr_rgb_led.mode = mode; + _zr_rgb_led.red = red; + _zr_rgb_led.green = green; + _zr_rgb_led.blue = blue; + _zr_rgb_led.is_send = true; + return true; } -void AP_UAVCAN::send_zr_cam_cmd(uint8_t gimbal_id, uint8_t mode,uint8_t v1, uint8_t v2,uint8_t v3,uint8_t v4) +void AP_UAVCAN::set_zr_cam_cmd(uint8_t gimbal_id, uint8_t mode, uint8_t v1, uint8_t v2, uint8_t v3, uint8_t v4) { - zrzk::equipment::camera::Command msg; - - msg.id = gimbal_id; - msg.command_mode = mode; - msg.command_1 =v1; - msg.command_2 =v2; - msg.command_3 =v3; - msg.command_4 =v4; - zr_camera_command[_driver_index]->broadcast(msg); + + WITH_SEMAPHORE(_zr_cam_cmd.sem); + + _zr_cam_cmd.gimbal_id = gimbal_id; + _zr_cam_cmd.mode = mode; + _zr_cam_cmd.v1 = v1; + _zr_cam_cmd.v2 = v2; + _zr_cam_cmd.v3 = v3; + _zr_cam_cmd.v4 = v4; + _zr_cam_cmd.is_send = true; } -void AP_UAVCAN::send_cam_geopoi(uint8_t id,uint8_t mode,int32_t longitude_deg_1e7,int32_t latitude_deg_1e7,int32_t height_cm,int8_t height_type) -{ +void AP_UAVCAN::set_cam_geopoi(uint8_t id, uint8_t mode, int32_t longitude_deg_1e7, int32_t latitude_deg_1e7, int32_t height_cm, int8_t height_type) +{ + WITH_SEMAPHORE(_cam_geopoi.sem); + _cam_geopoi.id = id; + _cam_geopoi.mode = mode; + _cam_geopoi.latitude_deg_1e7 = latitude_deg_1e7; + _cam_geopoi.longitude_deg_1e7 = longitude_deg_1e7; + _cam_geopoi.height_cm = height_cm; + _cam_geopoi.height_type = height_type; + _cam_geopoi.is_send =true; +} + +void AP_UAVCAN:: set_cam_angular(uint8_t id,uint8_t mode,float pitch,float roll,float yaw) +{ + WITH_SEMAPHORE(_cam_angular.sem); + + _cam_angular.id = id; + _cam_angular.mode= mode; + _cam_angular.pitch= pitch;//ahrs.pitch_sensor*1e-2f; + _cam_angular.roll =roll; + _cam_angular.yaw= yaw; + _cam_angular.is_send =true; + +} + +void AP_UAVCAN::cam_geopoi_send() +{ + if(!_cam_geopoi.is_send) + return; + uavcan::equipment::camera_gimbal::GEOPOICommand msg; + WITH_SEMAPHORE(_cam_geopoi.sem); - uint8_t gimbal_id = id; - msg.gimbal_id = gimbal_id; - msg.mode.command_mode = mode; - msg.latitude_deg_1e7 =latitude_deg_1e7; - msg.longitude_deg_1e7 = longitude_deg_1e7; - msg.height_cm = height_cm; + msg.gimbal_id = _cam_geopoi.id; + msg.mode.command_mode = _cam_geopoi.mode; + msg.latitude_deg_1e7 = _cam_geopoi.latitude_deg_1e7; + msg.longitude_deg_1e7 = _cam_geopoi.longitude_deg_1e7; + msg.height_cm = _cam_geopoi.height_cm; + msg.height_reference = _cam_geopoi.height_type; cam_geopoi[_driver_index]->broadcast(msg); + _cam_geopoi.is_send = false; } -void AP_UAVCAN:: send_cam_angular(uint8_t id,uint8_t mode,float pitch,float roll,float yaw) +void AP_UAVCAN::cam_aular_send() { - uavcan::equipment::camera_gimbal::AngularCommand msg; + if(!_cam_angular.is_send) + return; - uint8_t gimbal_id = id; - msg.gimbal_id = gimbal_id; - msg.mode.command_mode = mode; - msg.quaternion_xyzw[0] = pitch;//ahrs.pitch_sensor*1e-2f; - msg.quaternion_xyzw[1] =roll; - msg.quaternion_xyzw[2] = yaw; + uavcan::equipment::camera_gimbal::AngularCommand msg; + WITH_SEMAPHORE(_cam_angular.sem); + + msg.gimbal_id = _cam_angular.id; + msg.mode.command_mode = _cam_angular.mode; + msg.quaternion_xyzw[0] = _cam_angular.pitch; //ahrs.pitch_sensor*1e-2f; + msg.quaternion_xyzw[1] = _cam_angular.roll; + msg.quaternion_xyzw[2] = _cam_angular.yaw; cam_angular[_driver_index]->broadcast(msg); + _cam_angular.is_send =false; + +} + + +void AP_UAVCAN::zr_cam_cmd_send() +{ + if(!_zr_cam_cmd.is_send) + return; + zrzk::equipment::camera::Command msg; + WITH_SEMAPHORE(_zr_cam_cmd.sem); + msg.id =_zr_cam_cmd.gimbal_id ; + msg.command_mode =_zr_cam_cmd. mode; + msg.command_1 =_zr_cam_cmd.v1; + msg.command_2 =_zr_cam_cmd.v2; + msg.command_3 =_zr_cam_cmd.v3; + msg.command_4 =_zr_cam_cmd.v4; + + zr_camera_command[_driver_index]->broadcast(msg); + _zr_cam_cmd.is_send =false; + +} + +void AP_UAVCAN::zr_rgb_led_send() +{ + if(!_zr_rgb_led.is_send) + { + return; + } + + zrzk::equipment::indication::ZrRGB msg; + WITH_SEMAPHORE(_zr_rgb_led.sem); + + msg.rgb_mode = _zr_rgb_led.mode; + msg.rgb_r =_zr_rgb_led. red; + msg.rgb_g = _zr_rgb_led.green; + msg.rgb_b = _zr_rgb_led.blue; + zr_rgb_led[_driver_index]->broadcast(msg); + _zr_rgb_led.is_send =false; } void AP_UAVCAN::handle_zr_camera_feedback(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ZrCamFeedBackCb &cb) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index c955dac3a7..80c11853b3 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -95,11 +95,10 @@ public: // send RTCMStream packets void send_RTCMStream(const uint8_t *data, uint32_t len); - bool send_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue); - - void send_zr_cam_cmd(uint8_t gimbal_id, uint8_t mode,uint8_t v1, uint8_t v2,uint8_t v3,uint8_t v4); - void send_cam_geopoi(uint8_t id,uint8_t mode,int32_t longitude_deg_1e7,int32_t latitude_deg_1e7,int32_t height_cm,int8_t height_type); - void send_cam_angular(uint8_t id,uint8_t mode,float pitch,float roll,float yaw); + bool set_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue); + void set_zr_cam_cmd(uint8_t gimbal_id, uint8_t mode,uint8_t v1, uint8_t v2,uint8_t v3,uint8_t v4); + void set_cam_geopoi(uint8_t id,uint8_t mode,int32_t longitude_deg_1e7,int32_t latitude_deg_1e7,int32_t height_cm,int8_t height_type); + void set_cam_angular(uint8_t id,uint8_t mode,float pitch,float roll,float yaw); template class RegistryBinder { @@ -175,6 +174,11 @@ private: // send GNSS injection void rtcm_stream_send(); + void cam_geopoi_send(); + void cam_aular_send(); + void zr_cam_cmd_send(); + void zr_rgb_led_send(); + uavcan::PoolAllocator _node_allocator; // UAVCAN parameters @@ -229,6 +233,51 @@ private: uint32_t last_send_ms; ByteBuffer *buf; } _rtcm_stream; + + + struct{ + HAL_Semaphore sem; + bool is_send; + uint8_t gimbal_id; + uint8_t mode; + uint8_t v1; + uint8_t v2; + uint8_t v3; + uint8_t v4; + }_zr_cam_cmd; + + struct + { + HAL_Semaphore sem; + bool is_send; + uint8_t id; + uint8_t mode; + int32_t longitude_deg_1e7; + int32_t latitude_deg_1e7; + int32_t height_cm; + int8_t height_type; + } _cam_geopoi; + + struct + { + HAL_Semaphore sem; + bool is_send; + uint8_t id; + uint8_t mode; + float pitch; + float roll; + float yaw; + }_cam_angular; + + struct + { + HAL_Semaphore sem; + bool is_send; + uint8_t mode; + uint8_t red; + uint8_t green; + uint8_t blue; + }_zr_rgb_led; // safety status send state uint32_t _last_safety_state_ms;