Browse Source

基本通信可行

zr-sdk-4.1.16
yaozb 4 years ago
parent
commit
af6b7c0751
  1. 167
      libraries/AP_Camera/AP_Camera.cpp
  2. 57
      libraries/AP_Camera/AP_Camera.h
  3. 2
      libraries/AP_HAL/AP_HAL_Boards.h
  4. 1079
      libraries/AP_Logger/AP_Logger.h
  5. 2043
      libraries/AP_Logger/LogFile.cpp
  6. 3587
      libraries/AP_Logger/LogStructure.h
  7. 204
      libraries/AP_Logger/README.md
  8. 166
      libraries/AP_UAVCAN/AP_UAVCAN.cpp
  9. 11
      libraries/AP_UAVCAN/AP_UAVCAN.h
  10. 27
      libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30010.Status.uavcan
  11. 17
      libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30011.Version.uavcan
  12. 27
      libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30012.Command.uavcan
  13. 21
      libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30014.CamFeedBack.uavcan
  14. 4
      libraries/GCS_MAVLink/GCS.h
  15. 47
      libraries/GCS_MAVLink/GCS_Common.cpp
  16. 1
      libraries/GCS_MAVLink/ap_message.h
  17. 2
      modules/mavlink

167
libraries/AP_Camera/AP_Camera.cpp

@ -1,5 +1,4 @@
#include "AP_Camera.h" #include "AP_Camera.h"
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_Relay/AP_Relay.h> #include <AP_Relay/AP_Relay.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
@ -10,7 +9,11 @@
#include <SRV_Channel/SRV_Channel.h> #include <SRV_Channel/SRV_Channel.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
#if HAL_WITH_UAVCAN
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
//#include<uavcan/equipment/camera_gimbal/Status.hpp>
#endif
// ------------------------------ // ------------------------------
#define CAM_DEBUG DISABLED #define CAM_DEBUG DISABLED
@ -122,8 +125,7 @@ AP_Camera::servo_pic()
} }
/// basic relay activation /// basic relay activation
void void AP_Camera::relay_pic()
AP_Camera::relay_pic()
{ {
AP_Relay *_apm_relay = AP::relay(); AP_Relay *_apm_relay = AP::relay();
if (_apm_relay == nullptr) { if (_apm_relay == nullptr) {
@ -153,6 +155,9 @@ void AP_Camera::trigger_pic()
case AP_CAMERA_TRIGGER_TYPE_RELAY: case AP_CAMERA_TRIGGER_TYPE_RELAY:
relay_pic(); // basic relay activation relay_pic(); // basic relay activation
break; break;
case AP_CAMERA_TRIGGER_TYPE_UAVCAN:
uavcan_pic();
break;
} }
log_picture(); log_picture();
@ -460,7 +465,161 @@ void AP_Camera::update_trigger()
} }
} }
} }
send_can_msg();
}
void AP_Camera::uavcan_pic()
{
#if HAL_WITH_UAVCAN
const AP_AHRS &ahrs = AP::ahrs();
camera_can_msg.mode = 12;
camera_can_msg.lng = current_loc.lng;
camera_can_msg.lat = current_loc.lat;
camera_can_msg.alt = current_loc.alt;
camera_can_msg.pitch = ahrs.pitch_sensor*1e-2f;
camera_can_msg.roll = ahrs.roll_sensor*1e-2f;
camera_can_msg.yaw = ahrs.yaw_sensor*1e-2f;
send_msg_step = 1;
_trigger_counter = constrain_int16(_trigger_duration*5,0,255);
#endif
}
void AP_Camera::send_can_msg(void)
{
#if HAL_WITH_UAVCAN
if(send_msg_step != 0){
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) {
switch (send_msg_step)
{
case 1:
uavcan->send_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);
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);
send_msg_step = 0;
break;
default:
break;
}
}
}
}
#endif
}
void AP_Camera::send_uavcan_camera_zr_control(const mavlink_camera_zr_control_t &packet){
#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->send_zr_cam_cmd(camera_state.id,packet.mode, packet.cmd1, packet.cmd2, packet.cmd3, packet.cmd4);
}
}
#endif
}
const char* AP_Camera::get_camera_sn(void)
{
return &sn[0];
}
const char* AP_Camera::get_camrea_ver(void )
{
return &sver[0];
}
void AP_Camera::set_camera_sn(char* sn_temp)
{
memcpy(&sn[0],&sn_temp[0],14);
sn[14] ='\0';
} }
void AP_Camera::set_camera_ver(char *ver_temp)
{
memcpy(&sver[0],&ver_temp[0],11);
sn[11] ='\0';
}
void AP_Camera::request_camera_sn(void){
#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->send_zr_cam_cmd(camera_state.id, 19, 1, 0, 0, 0);
}
}
#endif
}
void AP_Camera::update_zr_camera_status()
{
notice_send_camera_status_to_gcs();
}
void AP_Camera::update_zr_camera_version(void)
{
if ((strcmp(&sn[0], "\0")==0)||(strcmp(&sver[0],"\0")==0))
{
request_camera_sn();
}
else
{
gcs().send_text(MAV_SEVERITY_INFO, "CAMSN:%s", &sn[0]);
gcs().send_text(MAV_SEVERITY_INFO, "CAMVer:%s", &sver[0]);
}
}
void AP_Camera::send_camera_zr_status(mavlink_channel_t chan)
{
uint8_t feedback_shutter_speed = 0;
uint8_t feedback_gimbal = camera_state.feedback_code;
uint8_t feedback_error_fixed = 0;
uint8_t feedback_format = 0;
// struct AP_Camera::Cam_ZR_Info status = {};
mavlink_msg_camera_zr_status_send(
chan,
0,
0,
camera_state.id,
camera_state.shutter_speed,
camera_state.err_code,
camera_state.connect_state,
camera_state.usb_enable,
feedback_shutter_speed,
feedback_gimbal,
feedback_format,
feedback_error_fixed,
camera_state.num1,
camera_state.num2,
camera_state.num3,
camera_state.num4,
camera_state.num5,
camera_state.trigger_num,
camera_state.temperature);
}
// struct Camera_State & AP_Camera::get_camera_state()
// {
// return camera_state;
// }
// singleton instance // singleton instance
AP_Camera *AP_Camera::_singleton; AP_Camera *AP_Camera::_singleton;

57
libraries/AP_Camera/AP_Camera.h

@ -6,7 +6,8 @@
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#define AP_CAMERA_TRIGGER_TYPE_SERVO 0 #define AP_CAMERA_TRIGGER_TYPE_SERVO 0
#define AP_CAMERA_TRIGGER_TYPE_RELAY 1 #define AP_CAMERA_TRIGGER_TYPE_RELAY 1
#define AP_CAMERA_TRIGGER_TYPE_UAVCAN 2
#define AP_CAMERA_TRIGGER_DEFAULT_TRIGGER_TYPE AP_CAMERA_TRIGGER_TYPE_RELAY // default is to use servo to trigger camera #define AP_CAMERA_TRIGGER_DEFAULT_TRIGGER_TYPE AP_CAMERA_TRIGGER_TYPE_RELAY // default is to use servo to trigger camera
@ -56,13 +57,23 @@ public:
} }
void take_picture(); void take_picture();
void uavcan_pic();
uint8_t send_msg_step;
// Update - to be called periodically @at least 10Hz // Update - to be called periodically @at least 10Hz
void update(); void update();
// update camera trigger - 50Hz // update camera trigger - 50Hz
void update_trigger(); void update_trigger();
const char* get_camera_sn(void);
const char* get_camrea_ver(void);
void set_camera_sn(char * sn);
void set_camera_ver(char * ver);
void request_camera_sn(void);
void update_zr_camera_status();
void send_uavcan_camera_zr_control(const mavlink_camera_zr_control_t &packet);
void notice_send_camera_status_to_gcs(void);
void update_zr_camera_version(void);
void send_camera_zr_status(mavlink_channel_t chan);
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
// set if vehicle is in AUTO mode // set if vehicle is in AUTO mode
@ -84,10 +95,43 @@ public:
uint16_t _image_index_log; // number of pictures taken to log @Brown uint16_t _image_index_log; // number of pictures taken to log @Brown
struct Camera_State
{
float temperature;
uint16_t num1;
uint16_t num2;
uint16_t num3;
uint16_t num4;
uint16_t num5;
uint16_t trigger_num;
uint8_t instance; // the instance number of this Camera
uint8_t id;
uint8_t mode;
uint8_t shutter_speed;
uint8_t err_code;
uint8_t connect_state;
uint8_t uavcan_node_id;
uint8_t feedback_code;
uint8_t usb_enable;
};
struct Camera_Can_Msg
{
int32_t lat; // @Brown,for get camera pos
int32_t lng;
int32_t alt;
float roll;
float pitch;
float yaw;
uint8_t mode;
};
struct Camera_Can_Msg camera_can_msg;
struct Camera_State &get_camera_state(){
return camera_state;
}
private: private:
static AP_Camera *_singleton; static AP_Camera *_singleton;
struct Camera_State camera_state;
AP_Int8 _trigger_type; // 0:Servo,1:Relay AP_Int8 _trigger_type; // 0:Servo,1:Relay
AP_Int8 _trigger_duration; // duration in 10ths of a second that the camera shutter is held open AP_Int8 _trigger_duration; // duration in 10ths of a second that the camera shutter is held open
AP_Int8 _relay_on; // relay value to trigger camera AP_Int8 _relay_on; // relay value to trigger camera
@ -144,8 +188,11 @@ private:
int32_t last_pos_lat; // @Brown,for get camera pos int32_t last_pos_lat; // @Brown,for get camera pos
int32_t last_pos_lng; int32_t last_pos_lng;
}; char sn[15]; //sn 10 byte str 11byte give 15byte for tomorrow used
char sver[12]; //software version
void send_can_msg(void);
};
namespace AP { namespace AP {
AP_Camera *camera(); AP_Camera *camera();
}; };

2
libraries/AP_HAL/AP_HAL_Boards.h

@ -162,7 +162,7 @@
#endif #endif
#ifndef HAL_WITH_UAVCAN #ifndef HAL_WITH_UAVCAN
#define HAL_WITH_UAVCAN 0 #define HAL_WITH_UAVCAN 1
#endif #endif
#ifndef HAL_RCINPUT_WITH_AP_RADIO #ifndef HAL_RCINPUT_WITH_AP_RADIO

1079
libraries/AP_Logger/AP_Logger.h

File diff suppressed because it is too large Load Diff

2043
libraries/AP_Logger/LogFile.cpp

File diff suppressed because it is too large Load Diff

3587
libraries/AP_Logger/LogStructure.h

File diff suppressed because it is too large Load Diff

204
libraries/AP_Logger/README.md

@ -1,102 +1,102 @@
# Logger Notes # Logger Notes
## Format Types ## Format Types
The format type specifies the amount of storage required for the entry The format type specifies the amount of storage required for the entry
and how the content should be interpreted. and how the content should be interpreted.
| Char | C Type | | Char | C Type |
|------|--------| |------|--------|
|a | int16_t[32]| |a | int16_t[32]|
|b | int8_t| |b | int8_t|
|B | uint8_t| |B | uint8_t|
|h | int16_t| |h | int16_t|
|H | uint16_t| |H | uint16_t|
|i | int32_t| |i | int32_t|
|I | uint32_t| |I | uint32_t|
|f | float| |f | float|
|d | double| |d | double|
|n | char[4]| |n | char[4]|
|N | char[16]| |N | char[16]|
|Z | char[64]| |Z | char[64]|
|L | int32_t latitude/longitude (so -35.1332423 becomes -351332423)| |L | int32_t latitude/longitude (so -35.1332423 becomes -351332423)|
|M | uint8_t flight mode| |M | uint8_t flight mode|
|q | int64_t| |q | int64_t|
|Q | uint64_t| |Q | uint64_t|
Legacy field types - do not use. These have been replaced by using the base C type and an appropriate multiplier column entry. Legacy field types - do not use. These have been replaced by using the base C type and an appropriate multiplier column entry.
| Char | CType+Mult | | Char | CType+Mult |
|------|--------------| |------|--------------|
| c | int16_t * 100| | c | int16_t * 100|
| C | uint16_t * 100| | C | uint16_t * 100|
| e | int32_t * 100| | e | int32_t * 100|
| E | uint32_t * 100| | E | uint32_t * 100|
## Units ## Units
All units here should be base units All units here should be base units
This does mean battery capacity is here as "amp*second" This does mean battery capacity is here as "amp*second"
Please keep the names consistent with Tools/autotest/param_metadata/param.py:33 Please keep the names consistent with Tools/autotest/param_metadata/param.py:33
| Char | Unit Abbrev. | Description | | Char | Unit Abbrev. | Description |
|-----|---|---| |-----|---|---|
| '-' | "" | no units e.g. Pi | or a string| | '-' | "" | no units e.g. Pi | or a string|
| '?' | "UNKNOWN" | Units which haven't been worked out yet....| | '?' | "UNKNOWN" | Units which haven't been worked out yet....|
| 'A' | "A" | Ampere| | 'A' | "A" | Ampere|
| 'd' | "deg" | of the angular variety | -180 to 180| | 'd' | "deg" | of the angular variety | -180 to 180|
| 'b' | "B" | bytes| | 'b' | "B" | bytes|
| 'k' | "deg/s" | degrees per second. Degrees are NOT SI | but is some situations more user-friendly than radians| | 'k' | "deg/s" | degrees per second. Degrees are NOT SI | but is some situations more user-friendly than radians|
| 'D' | "deglatitude" | degrees of latitude| | 'D' | "deglatitude" | degrees of latitude|
| 'e' | "deg/s/s" | degrees per second per second. Degrees are NOT SI | but is some situations more user-friendly than radians| | 'e' | "deg/s/s" | degrees per second per second. Degrees are NOT SI | but is some situations more user-friendly than radians|
| 'E' | "rad/s" | radians per second| | 'E' | "rad/s" | radians per second|
| 'G' | "Gauss" | Gauss is not an SI unit | but 1 tesla = 10000 gauss so a simple replacement is not possible here| | 'G' | "Gauss" | Gauss is not an SI unit | but 1 tesla = 10000 gauss so a simple replacement is not possible here|
| 'h' | "degheading" | 0.? to 359.?| | 'h' | "degheading" | 0.? to 359.?|
| 'i' | "A.s" | Ampere second| | 'i' | "A.s" | Ampere second|
| 'J' | "W.s" | Joule (Watt second)| | 'J' | "W.s" | Joule (Watt second)|
| 'l' | "l" | litres| | 'l' | "l" | litres|
| 'L' | "rad/s/s" | radians per second per second| | 'L' | "rad/s/s" | radians per second per second|
| 'm' | "m" | metres| | 'm' | "m" | metres|
| 'n' | "m/s" | metres per second| | 'n' | "m/s" | metres per second|
| 'N' | "N" | Newton| | 'N' | "N" | Newton|
| 'o' | "m/s/s" | metres per second per second| | 'o' | "m/s/s" | metres per second per second|
| 'O' | "degC" | degrees Celsius. Not SI | but Kelvin is too cumbersome for most users| | 'O' | "degC" | degrees Celsius. Not SI | but Kelvin is too cumbersome for most users|
| '%' | "%" | percent| | '%' | "%" | percent|
| 'S' | "satellites" | number of satellites| | 'S' | "satellites" | number of satellites|
| 's' | "s" | seconds| | 's' | "s" | seconds|
| 'q' | "rpm" | rounds per minute. Not SI | but sometimes more intuitive than Hertz| | 'q' | "rpm" | rounds per minute. Not SI | but sometimes more intuitive than Hertz|
| 'r' | "rad" | radians| | 'r' | "rad" | radians|
| 'U' | "deglongitude" | degrees of longitude| | 'U' | "deglongitude" | degrees of longitude|
| 'u' | "ppm" | pulses per minute| | 'u' | "ppm" | pulses per minute|
| 'v' | "V" | Volt| | 'v' | "V" | Volt|
| 'P' | "Pa" | Pascal| | 'P' | "Pa" | Pascal|
| 'w' | "Ohm" | Ohm| | 'w' | "Ohm" | Ohm|
| 'Y' | "us" | pulse width modulation in microseconds| | 'Y' | "us" | pulse width modulation in microseconds|
| 'z' | "Hz" | Hertz| | 'z' | "Hz" | Hertz|
| '#' | "instance" | (e.g.)Sensor instance number| | '#' | "instance" | (e.g.)Sensor instance number|
## Multipliers ## Multipliers
This multiplier information applies to the raw value present in the This multiplier information applies to the raw value present in the
log. Any adjustment implied by the format field (e.g. the "centi" log. Any adjustment implied by the format field (e.g. the "centi"
in "centidegrees" is *IGNORED* for the purposes of scaling. in "centidegrees" is *IGNORED* for the purposes of scaling.
Essentially "format" simply tells you the C-type, and format-type h Essentially "format" simply tells you the C-type, and format-type h
(int16_t) is equivalent to format-type c (int16_t*100) (int16_t) is equivalent to format-type c (int16_t*100)
tl;dr a GCS shouldn't/mustn't infer any scaling from the unit name tl;dr a GCS shouldn't/mustn't infer any scaling from the unit name
| Char | Multiplier | | Char | Multiplier |
|------|------------| |------|------------|
| '-' | 0 | // no multiplier e.g. a string| | '-' | 0 | // no multiplier e.g. a string|
| '?' | 1 | // multipliers which haven't been worked out yet....| | '?' | 1 | // multipliers which haven't been worked out yet....|
| '2' | 1e2 || | '2' | 1e2 ||
| '1' | 1e1 || | '1' | 1e1 ||
| '0' | 1e0 || | '0' | 1e0 ||
| 'A' | 1e-1 || | 'A' | 1e-1 ||
| 'B' | 1e-2 || | 'B' | 1e-2 ||
| 'C' | 1e-3 || | 'C' | 1e-3 ||
| 'D' | 1e-4 || | 'D' | 1e-4 ||
| 'E' | 1e-5 || | 'E' | 1e-5 ||
| 'F' | 1e-6 || | 'F' | 1e-6 ||
| 'G' | 1e-7 || | 'G' | 1e-7 ||
| '!' | 3.6 | // (ampere*second => milliampere*hour) and (km/h => m/s)| | '!' | 3.6 | // (ampere*second => milliampere*hour) and (km/h => m/s)|
| '/' | 3600 | // (ampere*second => ampere*hour)| | '/' | 3600 | // (ampere*second => ampere*hour)|

166
libraries/AP_UAVCAN/AP_UAVCAN.cpp

@ -41,7 +41,15 @@
#include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp> #include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp>
#include <uavcan/equipment/gnss/RTCMStream.hpp> #include <uavcan/equipment/gnss/RTCMStream.hpp>
#include<zrzk/equipment/indication/ZrRGB.hpp> #include <zrzk/equipment/indication/ZrRGB.hpp>
#include <zrzk/equipment/camera/Version.hpp>
#include <zrzk/equipment/camera/Status.hpp>
#include <zrzk/equipment/camera/CamFeedBack.hpp>
#include <zrzk/equipment/camera/Command.hpp>
#include <uavcan/equipment/camera_gimbal/GEOPOICommand.hpp>
#include <uavcan/equipment/camera_gimbal/AngularCommand.hpp>
#include <AP_Baro/AP_Baro_UAVCAN.h> #include <AP_Baro/AP_Baro_UAVCAN.h>
#include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h> #include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
@ -53,6 +61,8 @@
#include <AP_OpticalFlow/AP_OpticalFlow_HereFlow.h> #include <AP_OpticalFlow/AP_OpticalFlow_HereFlow.h>
#include <AP_ADSB/AP_ADSB.h> #include <AP_ADSB/AP_ADSB.h>
#include "AP_UAVCAN_Server.h" #include "AP_UAVCAN_Server.h"
#include <AP_Logger/AP_Logger.h>
#include <AP_Camera/AP_Camera.h>
#define LED_DELAY_US 50000 #define LED_DELAY_US 50000
@ -113,6 +123,10 @@ static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[MAX_N
static uavcan::Publisher<zrzk::equipment::indication::ZrRGB>* zr_rgb_led[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher<zrzk::equipment::indication::ZrRGB>* zr_rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<zrzk::equipment::camera::Command>* zr_camera_command[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::camera_gimbal::AngularCommand>* cam_angular[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::camera_gimbal::GEOPOICommand>* cam_geopoi[MAX_NUMBER_OF_CAN_DRIVERS];
// subscribers // subscribers
// handler SafteyButton // handler SafteyButton
@ -123,6 +137,15 @@ static uavcan::Subscriber<ardupilot::indication::Button, ButtonCb> *safety_butto
UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport); UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport);
static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb> *traffic_report_listener[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb> *traffic_report_listener[MAX_NUMBER_OF_CAN_DRIVERS];
//handler cameras status @binsir
UC_REGISTRY_BINDER(ZrCamStatusCb,zrzk::equipment::camera::Status);
static uavcan::Subscriber<zrzk::equipment::camera::Status,ZrCamStatusCb> *zr_camera_status_listener[MAX_NUMBER_OF_CAN_DRIVERS];
UC_REGISTRY_BINDER(ZrCamVersionCb,zrzk::equipment::camera::Version);
static uavcan::Subscriber<zrzk::equipment::camera::Version,ZrCamVersionCb> *zr_camera_version_listener[MAX_NUMBER_OF_CAN_DRIVERS];
UC_REGISTRY_BINDER(ZrCamFeedBackCb,zrzk::equipment::camera::CamFeedBack);
static uavcan::Subscriber<zrzk::equipment::camera::CamFeedBack,ZrCamFeedBackCb> *zr_camera_feedback_listener[MAX_NUMBER_OF_CAN_DRIVERS];
AP_UAVCAN::AP_UAVCAN() : AP_UAVCAN::AP_UAVCAN() :
_node_allocator() _node_allocator()
@ -211,7 +234,6 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
uint8_t uid_len = uid_buf_len; uint8_t uid_len = uid_buf_len;
uint8_t unique_id[uid_buf_len]; uint8_t unique_id[uid_buf_len];
if (hal.util->get_system_id_unformatted(unique_id, uid_len)) { if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
//This is because we are maintaining a common Server Record for all UAVCAN Instances. //This is because we are maintaining a common Server Record for all UAVCAN Instances.
//In case the node IDs are different, and unique id same, it will create //In case the node IDs are different, and unique id same, it will create
@ -273,6 +295,18 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
zr_rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); zr_rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
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]->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]->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]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node); safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
if (safety_button_listener[driver_index]) { if (safety_button_listener[driver_index]) {
safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button)); safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button));
@ -282,7 +316,22 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
if (traffic_report_listener[driver_index]) { if (traffic_report_listener[driver_index]) {
traffic_report_listener[driver_index]->start(TrafficReportCb(this, &handle_traffic_report)); traffic_report_listener[driver_index]->start(TrafficReportCb(this, &handle_traffic_report));
} }
zr_camera_status_listener[driver_index] = new uavcan::Subscriber<zrzk::equipment::camera::Status,ZrCamStatusCb>(*_node);
if(zr_camera_status_listener[driver_index]){
zr_camera_status_listener[driver_index]->start(ZrCamStatusCb(this, &handle_zr_camera_status));
}
zr_camera_version_listener[driver_index] = new uavcan::Subscriber<zrzk::equipment::camera::Version,ZrCamVersionCb>(*_node);
if(zr_camera_version_listener[driver_index]){
zr_camera_version_listener[driver_index]->start(ZrCamVersionCb(this, &handle_zr_camera_version));
}
zr_camera_feedback_listener[driver_index] = new uavcan::Subscriber<zrzk::equipment::camera::CamFeedBack,ZrCamFeedBackCb>(*_node);
if(zr_camera_feedback_listener[driver_index]){
zr_camera_feedback_listener[driver_index]->start(ZrCamFeedBackCb(this, &handle_zr_camera_feedback));
}
_led_conf.devices_count = 0; _led_conf.devices_count = 0;
if (enable_filters) { if (enable_filters) {
configureCanAcceptanceFilters(*_node); configureCanAcceptanceFilters(*_node);
@ -359,9 +408,7 @@ void AP_UAVCAN::loop(void)
} }
} }
///// SRV output ///// ///// SRV output /////
void AP_UAVCAN::SRV_send_actuator(void) void AP_UAVCAN::SRV_send_actuator(void)
{ {
uint8_t starting_servo = 0; uint8_t starting_servo = 0;
@ -732,5 +779,114 @@ bool AP_UAVCAN::send_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_
return 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)
{
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);
}
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)
{
uavcan::equipment::camera_gimbal::GEOPOICommand msg;
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;
cam_geopoi[_driver_index]->broadcast(msg);
}
void AP_UAVCAN:: send_cam_angular(uint8_t id,uint8_t mode,float pitch,float roll,float yaw)
{
uavcan::equipment::camera_gimbal::AngularCommand msg;
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;
cam_angular[_driver_index]->broadcast(msg);
}
void AP_UAVCAN::handle_zr_camera_feedback(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ZrCamFeedBackCb &cb)
{
AP_Logger *logger = AP_Logger::get_singleton();
if ( logger!=nullptr )
{
AP::logger().Write_Camera_Feedback(
cb.msg->index,
cb.msg->camera_photo_number[0],
cb.msg->camera_photo_number[1],
cb.msg->camera_photo_number[2],
cb.msg->camera_photo_number[3],
cb.msg->camera_photo_number[4],
cb.msg->camera_photoed);
}
}
void AP_UAVCAN::handle_zr_camera_status(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ZrCamStatusCb &cb)
{
// if(cb.msg->gimbal_id ==0xC5){ //根据id 判断是五镜头数据 197 ==C5
AP_Camera *camera = AP::camera();
if (camera == nullptr)
{
return;
}
AP_Camera::Camera_State &interim_state = AP::camera()->get_camera_state();
interim_state.mode = cb.msg->mode;
interim_state.id = cb.msg->id;
interim_state.num1 = (uint16_t)cb.msg->camera_photo_number[0];
interim_state.num2 = (uint16_t)cb.msg->camera_photo_number[1];
interim_state.num3 = (uint16_t)cb.msg->camera_photo_number[2];
interim_state.num4 = (uint16_t)cb.msg->camera_photo_number[3];
interim_state.num5 = (uint16_t)cb.msg->camera_photo_number[4];
interim_state.temperature = cb.msg->temperature;
interim_state.shutter_speed = cb.msg->shutter_speed;
interim_state.err_code = cb.msg->error_code;
interim_state.feedback_code = cb.msg->feedback_code;
interim_state.usb_enable = cb.msg->usb_enable;
camera->update_zr_camera_status();
}
void AP_UAVCAN::handle_zr_camera_version(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ZrCamVersionCb &cb)
{
AP_Camera *camera = AP::camera();
if (camera == nullptr)
{
return;
}
char sn[15];
char ver[12];
memcpy(&sn[0],&cb.msg->camera_sn[0],15);
memcpy(&ver[0],&cb.msg->software_version[0],12);
camera->set_camera_sn(&sn[0]);
camera->set_camera_ver(&ver[0]);
camera->update_zr_camera_version( );
}
void AP_Camera::notice_send_camera_status_to_gcs(void)
{
//add control send time
gcs().send_message(MSG_CAMERA_ZR_STATUS);
}
#endif // HAL_WITH_UAVCAN #endif // HAL_WITH_UAVCAN

11
libraries/AP_UAVCAN/AP_UAVCAN.h

@ -50,6 +50,10 @@
class ButtonCb; class ButtonCb;
class TrafficReportCb; class TrafficReportCb;
class ZrCamStatusCb;
class ZrCamVersionCb;
class ZrCamFeedBackCb;
/* /*
Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received, Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
the Callback will invoke registery to register the node as separate backend. the Callback will invoke registery to register the node as separate backend.
@ -93,6 +97,10 @@ public:
bool send_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue); 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);
template <typename DataType_> template <typename DataType_>
class RegistryBinder { class RegistryBinder {
protected: protected:
@ -228,6 +236,9 @@ private:
// safety button handling // safety button handling
static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb); static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb); static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
static void handle_zr_camera_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamStatusCb &cb);
static void handle_zr_camera_version(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamVersionCb &cb);
static void handle_zr_camera_feedback(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamFeedBackCb &cb);
}; };
#endif /* AP_UAVCAN_H_ */ #endif /* AP_UAVCAN_H_ */

27
libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30010.Status.uavcan

@ -0,0 +1,27 @@
#
# Generic zrzk camera status.
#
uint8 id
uint6 mode #delete
uint2 usb_enable
uint8 shutter_speed
uint8 error_code
uint8 NONE = 0
uint8 SHUTTER_SPEED_DONE = 1
uint8 FORMAT_DONE = 2
uint8 GIMBAL_DONE = 4
uint8 ERROR_FIX_DONE = 8
uint8 feedback_code
float16 temperature
uint16 trigger_num
uint16[<=5] camera_photo_number
#uint8[<40] message

17
libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30011.Version.uavcan

@ -0,0 +1,17 @@
#
# Generic zr camera status.
#
uint8 id
uint8 mode
#
# Camera axis orientation in body frame (not in fixed frame).
# Please refer to the UAVCAN coordinate frame conventions.
#
uint8[12] software_version
uint8[12] hardware_version
uint8[15] camera_sn
uint8[15] camera_name

27
libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30012.Command.uavcan

@ -0,0 +1,27 @@
#
# Generic zr camera commond.
#
uint8 id
uint8 CAM_RECOVER = 11
uint8 TAKE_PHOTO = 12
uint8 SHATTER_SPEED = 13
uint8 GIMBAL_CONTROL = 15
uint8 ERROR_FIX = 16
uint8 FORMAT = 17
uint8 GET_PHOTO_NUMBER = 18
uint8 GET_CAMERA_VERSION = 19
uint8 command_mode
#
# Camera axis orientation in body frame (not in fixed frame).
# Please refer to the UAVCAN coordinate frame conventions.
#
uint8 command_1
uint8 command_2
uint8 command_3
uint8 command_4

21
libraries/AP_UAVCAN/dsdl/zrzk/equipment/camera/30014.CamFeedBack.uavcan

@ -0,0 +1,21 @@
#
# Generic zr camera commond.
#
uint8 id
#
#0x01 cam1
#0x02 cam2
#0x04 cam3
#0x08 cam4
#0x10 cam5
#
uint8 camera_photoed
uint16 index
uint16[<=5] camera_photo_number
#
# Camera axis orientation in body frame (not in fixed frame).
# Please refer to the UAVCAN coordinate frame conventions.
#

4
libraries/GCS_MAVLink/GCS.h

@ -371,7 +371,7 @@ protected:
MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_long_t &packet); MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_long_t &packet);
virtual void send_banner(); virtual void send_banner();
virtual void send_camera_banner();
void handle_device_op_read(const mavlink_message_t &msg); void handle_device_op_read(const mavlink_message_t &msg);
void handle_device_op_write(const mavlink_message_t &msg); void handle_device_op_write(const mavlink_message_t &msg);
@ -421,7 +421,7 @@ protected:
MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &packet); MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &packet);
void handle_optical_flow(const mavlink_message_t &msg); void handle_optical_flow(const mavlink_message_t &msg);
void handle_camera_zr_control(const mavlink_message_t &msg);
// vehicle-overridable message send function // vehicle-overridable message send function
virtual bool try_send_message(enum ap_message id); virtual bool try_send_message(enum ap_message id);
virtual void send_global_position_int(); virtual void send_global_position_int();

47
libraries/GCS_MAVLink/GCS_Common.cpp

@ -796,6 +796,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
{ MAVLINK_MSG_ID_EXTENDED_SYS_STATE, MSG_EXTENDED_SYS_STATE}, { MAVLINK_MSG_ID_EXTENDED_SYS_STATE, MSG_EXTENDED_SYS_STATE},
{ MAVLINK_MSG_ID_AUTOPILOT_VERSION, MSG_AUTOPILOT_VERSION}, { MAVLINK_MSG_ID_AUTOPILOT_VERSION, MSG_AUTOPILOT_VERSION},
{ MAVLINK_MSG_ID_ZR_FLYING_STATUS, MSG_ZR_FLYING_STATUS}, { MAVLINK_MSG_ID_ZR_FLYING_STATUS, MSG_ZR_FLYING_STATUS},
{MAVLINK_MSG_ID_CAMERA_ZR_STATUS, MSG_CAMERA_ZR_STATUS},
}; };
for (uint8_t i=0; i<ARRAY_SIZE(map); i++) { for (uint8_t i=0; i<ARRAY_SIZE(map); i++) {
@ -3042,6 +3043,17 @@ void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg)
optflow->handle_msg(msg); optflow->handle_msg(msg);
} }
void GCS_MAVLINK::handle_camera_zr_control(const mavlink_message_t &msg)
{
mavlink_camera_zr_control_t packet;
mavlink_msg_camera_zr_control_decode(&msg, &packet);
AP_Camera *camera = AP::camera();
if (camera != nullptr)
{
camera->send_uavcan_camera_zr_control(packet);
}
}
/* /*
handle messages which don't require vehicle specific data handle messages which don't require vehicle specific data
*/ */
@ -3217,6 +3229,9 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
handle_optical_flow(msg); handle_optical_flow(msg);
break; break;
case MAVLINK_MSG_ID_CAMERA_ZR_CONTROL:
handle_camera_zr_control(msg);
break;
} }
} }
@ -4323,6 +4338,17 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
} }
break; break;
case MSG_CAMERA_ZR_STATUS:
{
AP_Camera *camera = AP::camera();
if(camera ==nullptr ){
break;
}
CHECK_PAYLOAD_SIZE(CAMERA_ZR_STATUS);
camera->send_camera_zr_status(chan) ;
}
break;
case MSG_SYSTEM_TIME: case MSG_SYSTEM_TIME:
CHECK_PAYLOAD_SIZE(SYSTEM_TIME); CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
send_system_time(); send_system_time();
@ -4821,6 +4847,27 @@ void GCS_MAVLINK::manual_override(RC_Channel *c, int16_t value_in, const uint16_
c->set_override(override_value, tnow); c->set_override(override_value, tnow);
} }
void GCS_MAVLINK::send_camera_banner(void)
{
#if HAL_WITH_UAVCAN
AP_Camera *camera = AP::camera();
if (camera == nullptr)
return;
if ((strcmp(camera->get_camera_sn(), "\0") == 0) || (strcmp(camera->get_camrea_ver(), "\0") == 0))
{
camera->request_camera_sn();
}
else
{
send_text(MAV_SEVERITY_INFO, "CAMSN:%s", camera->get_camera_sn());
//send_text(MAV_SEVERITY_INFO, "CAMId: %d", camera->interim_state.id);
send_text(MAV_SEVERITY_INFO, "CAMVer:%s", camera->get_camrea_ver());
}
#endif
}
/* /*
handle a SET_MODE MAVLink message handle a SET_MODE MAVLink message
*/ */

1
libraries/GCS_MAVLink/ap_message.h

@ -77,5 +77,6 @@ enum ap_message : uint8_t {
MSG_ZR_CAMERA_STATUS, MSG_ZR_CAMERA_STATUS,
MSG_BATTGO_INFO, MSG_BATTGO_INFO,
MSG_BATTGO_HISTORY, MSG_BATTGO_HISTORY,
MSG_CAMERA_ZR_STATUS,
MSG_LAST // MSG_LAST must be the last entry in this enum MSG_LAST // MSG_LAST must be the last entry in this enum
}; };

2
modules/mavlink

@ -1 +1 @@
Subproject commit 18cab6950986cd7b9b7a507ec35f3501d4a7e019 Subproject commit 4a6577557847fcb13f8bd9b7ebbe54f4a5a9f00e
Loading…
Cancel
Save