mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
378 lines
15 KiB
C++
378 lines
15 KiB
C++
/*
|
|
* This file 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 file 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 <http://www.gnu.org/licenses/>.
|
|
*
|
|
* Code by Michael Oborne and Siddharth Bharat Purohit, Cubepilot Pty.
|
|
*/
|
|
|
|
#include "AP_ONVIF.h"
|
|
#include <AP_ONVIF/MediaBinding.nsmap>
|
|
|
|
#include "onvifhelpers.h"
|
|
// For ChibiOS we will use HW RND # generator
|
|
#include <stdlib.h> //rand()
|
|
|
|
|
|
#ifndef USERNAME
|
|
#define USERNAME "admin"
|
|
#endif
|
|
|
|
#ifndef PASSWORD
|
|
#define PASSWORD "admin"
|
|
#endif
|
|
|
|
#ifndef ONVIF_HOSTNAME
|
|
#define ONVIF_HOSTNAME "http://10.211.55.3:10000"
|
|
#endif
|
|
|
|
#define DEVICE_ENDPOINT ONVIF_HOSTNAME"/onvif/device_service"
|
|
#define MEDIA_ENDPOINT ONVIF_HOSTNAME"/onvif/media_service"
|
|
#define PTZ_ENDPOINT ONVIF_HOSTNAME"/onvif/ptz_service"
|
|
|
|
#ifndef PRINT
|
|
#define PRINT(fmt,args...) do {printf(fmt "\n", ## args); } while(0)
|
|
#endif
|
|
|
|
const char *wsse_PasswordDigestURI = "http://docs.oasis-open.org/wss/2004/01/oasis-200401-wss-username-token-profile-1.0#PasswordDigest";
|
|
const char *wsse_Base64BinaryURI = "http://docs.oasis-open.org/wss/2004/01/oasis-200401-wss-soap-message-security-1.0#Base64Binary";
|
|
|
|
// static AP _ONVIF *_singleton;
|
|
extern const AP_HAL::HAL &hal;
|
|
|
|
bool AP_ONVIF::init()
|
|
{
|
|
srand ((time_t)(hal.util->get_hw_rtc()/1000000ULL));
|
|
soap = soap_new1(SOAP_XML_CANONICAL | SOAP_C_UTFSTRING);
|
|
soap->connect_timeout = soap->recv_timeout = soap->send_timeout = 30; // 30 sec
|
|
|
|
proxy_device = new DeviceBindingProxy(soap);
|
|
proxy_media = new MediaBindingProxy(soap);
|
|
proxy_ptz = new PTZBindingProxy(soap);
|
|
|
|
if (proxy_device == nullptr ||
|
|
proxy_media == nullptr ||
|
|
proxy_ptz == nullptr) {
|
|
AP_HAL::panic("AP_ONVIF: Failed to allocate gSOAP Proxy objects.");
|
|
return false;
|
|
}
|
|
/// TODO: Need to find a way to store this in parameter system
|
|
// or it could be just storage, we will see
|
|
proxy_device->soap_endpoint = DEVICE_ENDPOINT;
|
|
if (!probe_onvif_server()) {
|
|
PRINT("Failed to probe onvif server.");
|
|
return false;
|
|
}
|
|
|
|
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ONVIF::update, void), "onvif",
|
|
4096, AP_HAL::Scheduler::PRIORITY_IO, 0)) {
|
|
PRINT("Failed to create onvif thread");
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void AP_ONVIF::update()
|
|
{
|
|
while(true) {
|
|
float pan = pan_norm;
|
|
float tilt = tilt_norm;
|
|
|
|
// translate them into actual commands using cmd limits
|
|
pan = ((pan + 1) * (pan_tilt_limit_max.x - pan_tilt_limit_min.x)/2.0) + pan_tilt_limit_min.x;
|
|
tilt = ((tilt + 1) * (pan_tilt_limit_max.y - pan_tilt_limit_min.y)/2.0) + pan_tilt_limit_min.y;
|
|
PRINT("PAN: %f TILT: %f", pan, tilt);
|
|
// don't send the same request again
|
|
if (uint32_t(pan*100.0) != uint32_t(last_pan_cmd*100.0) || uint32_t(tilt*100.0) != uint32_t(last_tilt_cmd*100.0)) {
|
|
// actually send the command
|
|
set_absolutemove(pan, tilt, 0.0);
|
|
last_pan_cmd = pan;
|
|
last_tilt_cmd = tilt;
|
|
}
|
|
hal.scheduler->delay(100);
|
|
}
|
|
}
|
|
|
|
void AP_ONVIF::report_error()
|
|
{
|
|
PRINT("ONVIF ERROR:\n");
|
|
if (soap_check_state(soap)) {
|
|
PRINT("Error: soap struct state not initialized");
|
|
} else if (soap->error) {
|
|
const char **c, *v = NULL, *s, *d;
|
|
c = soap_faultcode(soap);
|
|
if (!*c) {
|
|
soap_set_fault(soap);
|
|
c = soap_faultcode(soap);
|
|
}
|
|
if (soap->version == 2) {
|
|
v = soap_fault_subcode(soap);
|
|
}
|
|
s = soap_fault_string(soap);
|
|
d = soap_fault_detail(soap);
|
|
PRINT("%s%d fault %s [%s]\n%s\nDetail: %s",(soap->version ? "SOAP 1." : "Error "),
|
|
(soap->version ? (int)soap->version : soap->error),
|
|
*c, (v ? v : "no subcode"), (s ? s : "[no reason]"),
|
|
(d ? d : "[no detail]"));
|
|
}
|
|
}
|
|
|
|
bool AP_ONVIF::probe_onvif_server()
|
|
{
|
|
_tds__GetDeviceInformation GetDeviceInformation;
|
|
_tds__GetDeviceInformationResponse GetDeviceInformationResponse;
|
|
set_credentials();
|
|
if (proxy_device->GetDeviceInformation(&GetDeviceInformation, GetDeviceInformationResponse)) {
|
|
PRINT("Failed to fetch Device Information");
|
|
report_error();
|
|
return false;
|
|
}
|
|
|
|
PRINT("Manufacturer: %s",GetDeviceInformationResponse.Manufacturer.c_str());
|
|
PRINT("Model: %s",GetDeviceInformationResponse.Model.c_str());
|
|
PRINT("FirmwareVersion: %s",GetDeviceInformationResponse.FirmwareVersion.c_str());
|
|
PRINT("SerialNumber: %s",GetDeviceInformationResponse.SerialNumber.c_str());
|
|
PRINT("HardwareId: %s",GetDeviceInformationResponse.HardwareId.c_str());
|
|
|
|
// get device capabilities and print media
|
|
_tds__GetCapabilities GetCapabilities;
|
|
_tds__GetCapabilitiesResponse GetCapabilitiesResponse;
|
|
set_credentials();
|
|
if (proxy_device->GetCapabilities(&GetCapabilities, GetCapabilitiesResponse)) {
|
|
PRINT("Failed to fetch Device Capabilities");
|
|
report_error();
|
|
return false;
|
|
}
|
|
|
|
if (!GetCapabilitiesResponse.Capabilities || !GetCapabilitiesResponse.Capabilities->Media) {
|
|
PRINT("Missing device capabilities info");
|
|
} else {
|
|
PRINT("XAddr: %s", GetCapabilitiesResponse.Capabilities->Media->XAddr.c_str());
|
|
if (GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities) {
|
|
if (GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTPMulticast) {
|
|
PRINT("RTPMulticast: %s",(*GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTPMulticast ? "yes" : "no"));
|
|
}
|
|
if (GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTP_USCORETCP) {
|
|
PRINT("RTP_TCP: %s", (*GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTP_USCORETCP ? "yes" : "no"));
|
|
}
|
|
if (GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTP_USCORERTSP_USCORETCP) {
|
|
PRINT("RTP_RTSP_TCP: %s", (*GetCapabilitiesResponse.Capabilities->Media->StreamingCapabilities->RTP_USCORERTSP_USCORETCP ? "yes" : "no"));
|
|
}
|
|
}
|
|
}
|
|
|
|
// set the Media proxy endpoint to XAddr
|
|
proxy_media->soap_endpoint = MEDIA_ENDPOINT;
|
|
|
|
// get device profiles
|
|
_trt__GetProfiles GetProfiles;
|
|
_trt__GetProfilesResponse GetProfilesResponse;
|
|
set_credentials();
|
|
if (proxy_media->GetProfiles(&GetProfiles, GetProfilesResponse)){
|
|
PRINT("Failed to fetch profiles");
|
|
report_error();
|
|
return false;
|
|
}
|
|
|
|
if (GetProfilesResponse.Profiles.size()) {
|
|
PRINT("Profiles Received %lu", GetProfilesResponse.Profiles.size());
|
|
} else {
|
|
PRINT("Error: No Profiles Received");
|
|
return false;
|
|
}
|
|
|
|
// for each profile get snapshot
|
|
for (uint32_t i = 0; i < GetProfilesResponse.Profiles.size(); i++) {
|
|
PRINT("Profile name: %s", GetProfilesResponse.Profiles[i]->Name.c_str());
|
|
}
|
|
|
|
// Just use first one for now
|
|
profile_token = GetProfilesResponse.Profiles[0]->token;
|
|
|
|
proxy_ptz->soap_endpoint = PTZ_ENDPOINT;
|
|
|
|
// _tptz__GetServiceCapabilities GetCapabilities;
|
|
// _tptz__GetServiceCapabilitiesResponse GetCapabilitiesResponse;
|
|
// GetCapabilities.Category = tt__CapabilityCategory__PTZ;
|
|
// set_credentials();
|
|
// if (proxy_ptz->GetServiceCapabilities(&GetCapabilities, GetCapabilitiesResponse)) {
|
|
// PRINT("Failed to fetch PTZ Capabilities");
|
|
// report_error();
|
|
// return false;
|
|
// }
|
|
// if (!GetCapabilitiesResponse.Capabilities) {
|
|
|
|
// }
|
|
// GetCapabilitiesResponse.Capabilities->PTZ
|
|
|
|
// get PTZ Token
|
|
_tptz__GetConfigurations GetConfigurations;
|
|
_tptz__GetConfigurationsResponse GetConfigurationsResponse;
|
|
set_credentials();
|
|
if (proxy_ptz->GetConfigurations(&GetConfigurations, GetConfigurationsResponse)) {
|
|
PRINT("Failed to fetch Configurations");
|
|
report_error();
|
|
return false;
|
|
}
|
|
|
|
if (GetConfigurationsResponse.PTZConfiguration.size()) {
|
|
PRINT("PTZ Tokens Received");
|
|
} else {
|
|
PRINT("Error: No Profiles Received");
|
|
return false;
|
|
}
|
|
|
|
for (uint32_t i = 0; i < GetConfigurationsResponse.PTZConfiguration.size(); i++) {
|
|
PRINT("PTZ: %s", GetConfigurationsResponse.PTZConfiguration[i]->Name.c_str());
|
|
}
|
|
//GetConfigurationsResponse.PTZConfiguration[0]->token
|
|
pan_tilt_limit_max = Vector2f(GetConfigurationsResponse.PTZConfiguration[0]->PanTiltLimits->Range->XRange->Max,
|
|
GetConfigurationsResponse.PTZConfiguration[0]->PanTiltLimits->Range->YRange->Max);
|
|
pan_tilt_limit_min = Vector2f(GetConfigurationsResponse.PTZConfiguration[0]->PanTiltLimits->Range->XRange->Min,
|
|
GetConfigurationsResponse.PTZConfiguration[0]->PanTiltLimits->Range->YRange->Min);
|
|
zoom_min = GetConfigurationsResponse.PTZConfiguration[0]->ZoomLimits->Range->XRange->Min;
|
|
zoom_max = GetConfigurationsResponse.PTZConfiguration[0]->ZoomLimits->Range->XRange->Max;
|
|
|
|
PRINT("Pan: %f %f Tilt: %f %f", pan_tilt_limit_min.x, pan_tilt_limit_max.x,
|
|
pan_tilt_limit_min.y, pan_tilt_limit_max.y);
|
|
// Get PTZ status
|
|
_tptz__GetStatus GetStatus;
|
|
_tptz__GetStatusResponse GetStatusResponse;
|
|
GetStatus.ProfileToken = profile_token;
|
|
set_credentials();
|
|
if (proxy_ptz->GetStatus(&GetStatus, GetStatusResponse)) {
|
|
PRINT("Failed to recieve PTZ status");
|
|
return false;
|
|
}
|
|
|
|
if (GetStatusResponse.PTZStatus->Error) {
|
|
PRINT("ErrorStatus: %s", GetStatusResponse.PTZStatus->Error->c_str());
|
|
}
|
|
if (GetStatusResponse.PTZStatus->MoveStatus->PanTilt) {
|
|
PRINT("PTStatus: %d", *GetStatusResponse.PTZStatus->MoveStatus->PanTilt);
|
|
}
|
|
if (GetStatusResponse.PTZStatus->MoveStatus->Zoom) {
|
|
PRINT("ZoomStatus: %d", *GetStatusResponse.PTZStatus->MoveStatus->Zoom);
|
|
}
|
|
PRINT("Pan: %f Tilt: %f", GetStatusResponse.PTZStatus->Position->PanTilt->x,
|
|
GetStatusResponse.PTZStatus->Position->PanTilt->y);
|
|
PRINT("Zoom: %f", GetStatusResponse.PTZStatus->Position->Zoom->x);
|
|
|
|
soap_destroy(soap);
|
|
soap_end(soap);
|
|
return true;
|
|
}
|
|
|
|
void AP_ONVIF::rand_nonce(char *nonce, size_t noncelen)
|
|
{
|
|
size_t i;
|
|
uint32_t r = (uint32_t)(hal.util->get_hw_rtc()/1000000ULL);
|
|
(void)memcpy((void *)nonce, (const void *)&r, 4);
|
|
for (i = 4; i < noncelen; i += 4)
|
|
{
|
|
r = rand();
|
|
(void)memcpy((void *)(nonce + i), (const void *)&r, 4);
|
|
}
|
|
}
|
|
#define TEST_NONCE "LKqI6G/AikKCQrN0zqZFlg=="
|
|
#define TEST_TIME "2010-09-16T07:50:45Z"
|
|
#define TEST_PASS "userpassword"
|
|
#define TEST_RESULT "tuOSpGlFlIXsozq4HFNeeGeFLEI="
|
|
#define TEST 0
|
|
void AP_ONVIF::set_credentials()
|
|
{
|
|
soap_wsse_delete_Security(soap);
|
|
soap_wsse_add_Timestamp(soap, "Time", 60);
|
|
|
|
_wsse__Security *security = soap_wsse_add_Security(soap);
|
|
const char *created = soap_dateTime2s(soap, (time_t)(hal.util->get_hw_rtc()/1000000ULL));
|
|
char HA[SHA1_DIGEST_SIZE] {};
|
|
char HABase64fin[29] {};
|
|
char nonce[16], *nonceBase64;
|
|
sha1_ctx ctx;
|
|
uint16_t HABase64len;
|
|
char *HABase64enc;
|
|
uint16_t noncelen;
|
|
|
|
/* generate a nonce */
|
|
rand_nonce(nonce, 16);
|
|
|
|
sha1_begin(&ctx);
|
|
#if TEST
|
|
char* test_nonce = (char*)base64_decode((const unsigned char*)TEST_NONCE, strlen(TEST_NONCE), &noncelen);
|
|
sha1_hash((const unsigned char*)test_nonce, noncelen, &ctx);
|
|
sha1_hash((const unsigned char*)TEST_TIME, strlen(TEST_TIME), &ctx);
|
|
sha1_hash((const unsigned char*)TEST_PASS, strlen(TEST_PASS), &ctx);
|
|
nonceBase64 = (char*)base64_encode((unsigned char*)test_nonce, 16, &noncelen);
|
|
PRINT("Created:%s Hash64:%s", TEST_TIME, HABase64fin);
|
|
#else
|
|
sha1_hash((const unsigned char*)nonce, 16, &ctx);
|
|
sha1_hash((const unsigned char*)created, strlen(created), &ctx);
|
|
sha1_hash((const unsigned char*)PASSWORD, strlen(PASSWORD), &ctx);
|
|
nonceBase64 = (char*)base64_encode((unsigned char*)nonce, 16, &noncelen);
|
|
#endif
|
|
sha1_end((unsigned char*)HA, &ctx);
|
|
HABase64enc = (char*)base64_encode((unsigned char*)HA, SHA1_DIGEST_SIZE, &HABase64len);
|
|
if (HABase64len > 29) {
|
|
//things have gone truly bad time to panic
|
|
PRINT("Error: Invalid Base64 Encode!");
|
|
free(HABase64enc);
|
|
return;
|
|
}
|
|
|
|
memcpy(HABase64fin, HABase64enc, HABase64len);
|
|
|
|
if (soap_wsse_add_UsernameTokenText(soap, "Auth", USERNAME, HABase64fin)) {
|
|
report_error();
|
|
}
|
|
/* populate the remainder of the password, nonce, and created */
|
|
security->UsernameToken->Password->Type = (char*)wsse_PasswordDigestURI;
|
|
security->UsernameToken->Nonce = (struct wsse__EncodedString*)soap_malloc(soap, sizeof(struct wsse__EncodedString));
|
|
security->UsernameToken->Salt = NULL;
|
|
security->UsernameToken->Iteration = NULL;
|
|
if (!security->UsernameToken->Nonce) {
|
|
return;
|
|
}
|
|
soap_default_wsse__EncodedString(soap, security->UsernameToken->Nonce);
|
|
security->UsernameToken->Nonce->__item = nonceBase64;
|
|
security->UsernameToken->Nonce->EncodingType = (char*)wsse_Base64BinaryURI;
|
|
security->UsernameToken->wsu__Created = soap_strdup(soap, created);
|
|
}
|
|
|
|
bool AP_ONVIF::set_absolutemove(float x, float y, float z)
|
|
{
|
|
_tptz__AbsoluteMove AbsoluteMove;
|
|
_tptz__AbsoluteMoveResponse AbsoluteMoveResponse;
|
|
AbsoluteMove.Position = soap_new_tt__PTZVector(soap);
|
|
AbsoluteMove.Position->PanTilt = soap_new_tt__Vector2D(soap);
|
|
AbsoluteMove.Position->Zoom = soap_new_tt__Vector1D(soap);
|
|
AbsoluteMove.Position->PanTilt->x = constrain_float(x, pan_tilt_limit_min.x, pan_tilt_limit_max.x);
|
|
AbsoluteMove.Position->PanTilt->y = constrain_float(y, pan_tilt_limit_min.y, pan_tilt_limit_max.y);
|
|
AbsoluteMove.Position->Zoom->x = constrain_float(z, zoom_min, zoom_max);
|
|
AbsoluteMove.Speed = NULL;
|
|
AbsoluteMove.ProfileToken = profile_token;
|
|
// PRINT("Setting AbsoluteMove: %f %f %f", AbsoluteMove.Position->PanTilt->x,
|
|
// AbsoluteMove.Position->PanTilt->y,
|
|
// AbsoluteMove.Position->Zoom->x);
|
|
set_credentials();
|
|
if (proxy_ptz->AbsoluteMove(&AbsoluteMove, AbsoluteMoveResponse)) {
|
|
PRINT("Failed to sent AbsoluteMove cmd");
|
|
report_error();
|
|
soap_destroy(soap);
|
|
soap_end(soap);
|
|
return false;
|
|
}
|
|
soap_destroy(soap);
|
|
soap_end(soap);
|
|
return true;
|
|
}
|