Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix camera definition loading #1234

Merged
merged 2 commits into from
Oct 23, 2020
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 1 addition & 16 deletions src/plugins/camera/camera_definition.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -529,20 +529,6 @@ bool CameraDefinition::set_setting(
return false;
}

// FIXME: this is a hack because some params have the wrong type
// when they come from the camera.
MAVLinkParameters::ParamValue changed_value = value;
if (name.compare("CAM_MODE") == 0 || name.compare("CAM_COLORMODE") == 0 ||
name.compare("CAM_EXPMODE") == 0 || name.compare("CAM_METERING") == 0 ||
name.compare("CAM_PHOTOFMT") == 0 || name.compare("CAM_PHOTOQUAL") == 0 ||
name.compare("CAM_VIDRES") == 0 || name.compare("CAM_WBMODE") == 0 ||
name.compare("CAM_COLORENCODE") == 0 || name.compare("CAM_FLICKER") == 0) {
if (changed_value.is_uint8()) {
uint8_t temp = changed_value.get_uint8();
changed_value.set_uint32(uint32_t(temp));
}
}

// For range params, we need to verify the range.
if (_parameter_map[name]->is_range) {
// Check against the minimum
Expand All @@ -559,8 +545,7 @@ bool CameraDefinition::set_setting(
// TODO: Check step as well, until now we have only seen steps of 1 in the wild though.
}

// LogDebug() << "Setting " << name << " of type: " << changed_value.typestr();
_current_settings[name].value = changed_value;
_current_settings[name].value = value;
_current_settings[name].needs_updating = false;

// Some param changes cause other params to change, so they need to be updated.
Expand Down
87 changes: 54 additions & 33 deletions src/plugins/camera/camera_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -813,43 +813,76 @@ void CameraImpl::process_camera_information(const mavlink_message_t& message)
}
}

std::string content{};
bool found_content = false;
if (!_camera_definition) {
std::string content{};
auto succeeded = fetch_camera_definition(camera_information, content);

if (succeeded) {
_camera_definition.reset(new CameraDefinition());
_camera_definition->load_string(content);
refresh_params();
LogDebug() << "Successfully loaded camera definition";
} else {
LogDebug() << "Failed to fetch camera definition!";
}
}
}

bool CameraImpl::fetch_camera_definition(
const mavlink_camera_information_t& camera_information, std::string& camera_definition_out)
{
auto download_succeeded =
download_definition_file(camera_information.cam_definition_uri, camera_definition_out);

if (download_succeeded) {
return true;
}

return load_stored_definition(camera_information, camera_definition_out);
}

bool CameraImpl::download_definition_file(
const std::string& uri, std::string& camera_definition_out)
{
HttpLoader http_loader;
LogInfo() << "Downloading camera definition from: " << uri;
if (!http_loader.download_text_sync(uri, camera_definition_out)) {
LogErr() << "Failed to download camera definition.";
return false;
}

return true;
}

bool CameraImpl::load_stored_definition(
const mavlink_camera_information_t& camera_information, std::string& camera_definition_out)
{
// TODO: we might also try to support the correct version of the xml files.
if (strcmp((const char*)(camera_information.vendor_name), "Yuneec") == 0) {
if (strcmp((const char*)(camera_information.model_name), "E90") == 0) {
LogInfo() << "Using cached file for Yuneec E90.";
content = e90xml;
found_content = true;
camera_definition_out = e90xml;
return true;
} else if (strcmp((const char*)(camera_information.model_name), "E50") == 0) {
LogInfo() << "Using cached file for Yuneec E50.";
content = e50xml;
found_content = true;
camera_definition_out = e50xml;
return true;
} else if (strcmp((const char*)(camera_information.model_name), "CGOET") == 0) {
LogInfo() << "Using cached file for Yuneec ET.";
content = cgoetxml;
found_content = true;
camera_definition_out = cgoetxml;
return true;
} else if (strcmp((const char*)(camera_information.model_name), "E10T") == 0) {
LogInfo() << "Using cached file for Yuneec E10T.";
content = e10txml;
found_content = true;
camera_definition_out = e10txml;
return true;
} else if (strcmp((const char*)(camera_information.model_name), "E30Z") == 0) {
LogInfo() << "Using cached file for Yuneec E30Z.";
content = e30zxml;
found_content = true;
camera_definition_out = e30zxml;
return true;
}
} else {
content = camera_information.cam_definition_uri;
LogDebug() << "downloading camera definition file from: " << content;
found_content = load_definition_file(camera_information.cam_definition_uri, content);
}

if (found_content) {
_camera_definition.reset(new CameraDefinition());
_camera_definition->load_string(content);
refresh_params();
}
return false;
}

void CameraImpl::process_video_information(const mavlink_message_t& message)
Expand Down Expand Up @@ -992,18 +1025,6 @@ void CameraImpl::notify_mode()
_parent->call_user_callback([mode, temp_callback]() { temp_callback(mode); });
}

bool CameraImpl::load_definition_file(const std::string& uri, std::string& content)
{
HttpLoader http_loader;
LogInfo() << "Downloading camera definition from: " << uri;
if (!http_loader.download_text_sync(uri, content)) {
LogErr() << "Failed to download camera definition.";
return false;
}

return true;
}

bool CameraImpl::get_possible_setting_options(std::vector<std::string>& settings)
{
settings.clear();
Expand Down
6 changes: 5 additions & 1 deletion src/plugins/camera/camera_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -129,7 +129,11 @@ class CameraImpl : public PluginImplBase {

void check_status();

bool load_definition_file(const std::string& uri, std::string& content);
bool fetch_camera_definition(
const mavlink_camera_information_t& camera_information, std::string& camera_definition_out);
bool download_definition_file(const std::string& uri, std::string& camera_definition_out);
bool
load_stored_definition(const mavlink_camera_information_t&, std::string& camera_definition_out);

void refresh_params();
void invalidate_params();
Expand Down