zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

þMerge branch 'zr-v4.0.5-cam_new1' of gitee.com:xmzrzk/zr-v4 into zr-v4.0.5-cam_new1

zr-sdk-4.1.16
zbr 4 years ago
parent
4b514bbd98 a956e3b219
commit
ec5cc90b92
6 changed files with 204 additions and 53 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      ArduCopter/zr_flight.cpp
  2. 28
      libraries/AP_Camera/AP_Camera.cpp
  3. 6
      libraries/AP_Camera/AP_Camera.h
  4. 4
      libraries/AP_Notify/UAVCAN_RGB_LED.cpp
  5. 155
      libraries/AP_UAVCAN/AP_UAVCAN.cpp
  6. 59
      libraries/AP_UAVCAN/AP_UAVCAN.h

5
ArduCopter/zr_flight.cpp
Unescape View File

@ -70,7 +70,10 @@ void Copter::zr_SuperSlowLoop(){ @@ -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{

28
libraries/AP_Camera/AP_Camera.cpp
Unescape View File

@ -495,15 +495,15 @@ void AP_Camera::send_can_msg(void) @@ -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 @@ -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){ @@ -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) @@ -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;

6
libraries/AP_Camera/AP_Camera.h
Unescape View File

@ -74,8 +74,10 @@ public: @@ -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: @@ -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

4
libraries/AP_Notify/UAVCAN_RGB_LED.cpp
Unescape View File

@ -77,8 +77,8 @@ bool UAVCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) @@ -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;

155
libraries/AP_UAVCAN/AP_UAVCAN.cpp
Unescape View File

@ -296,15 +296,15 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) @@ -296,15 +296,15 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
zr_camera_command[driver_index] = new uavcan::Publisher<zrzk::equipment::camera::Command>(*_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<uavcan::equipment::camera_gimbal::AngularCommand>(*_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<uavcan::equipment::camera_gimbal::GEOPOICommand>(*_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<ardupilot::indication::Button, ButtonCb>(*_node);
@ -399,11 +399,15 @@ void AP_UAVCAN::loop(void) @@ -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 @@ -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)

59
libraries/AP_UAVCAN/AP_UAVCAN.h
Unescape View File

@ -95,11 +95,10 @@ public: @@ -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 <typename DataType_>
class RegistryBinder {
@ -175,6 +174,11 @@ private: @@ -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<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
// UAVCAN parameters
@ -229,6 +233,51 @@ private: @@ -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;

Write Preview
Loading…
Cancel
Save