Skip to content

Commit

Permalink
AP_Mount: xacti config cx-gb250 for abs temp
Browse files Browse the repository at this point in the history 
configures cx-gb250 for absolute temp readings
  • Loading branch information
@rmackay9
rmackay9 committed Sep 25, 2024
1 parent f13356a commit 4df3fc5
Show file tree
Hide file tree
Showing 2 changed files with 186 additions and 4 deletions.
155 changes: 152 additions & 3 deletions libraries/AP_Mount/AP_Mount_Xacti.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,12 @@ const char* AP_Mount_Xacti::sensor_mode_str[] = { "RGB", "IR", "PIP", "NDVI" };
const char* AP_Mount_Xacti::_param_names[] = {"SingleShot", "Recording", "FocusMode",
"SensorMode", "DigitalZoomMagnification",
"FirmwareVersion", "Status", "DateTime",
"OpticalZoomMagnification"};
"OpticalZoomMagnification",
"IRThroughColorPalette", "IRTemperatureRange",
"IRTemperatureThreshold_S", "IRColorControl_S",
"IRColorInfo_S"};
const char* AP_Mount_Xacti::_ir_temp_thresh_config_str = "96009600960096009600"; // IR temperature thresholds (5 thresholds all set to 150C)
const char* AP_Mount_Xacti::_ir_color_control_config_str = "000100000100"; // IR color control config string

// Constructor
AP_Mount_Xacti::AP_Mount_Xacti(class AP_Mount &frontend, class AP_Mount_Params &params, uint8_t instance) :
Expand Down Expand Up @@ -82,10 +87,17 @@ void AP_Mount_Xacti::update()
if (set_datetime(now_ms)) {
return;
}
// request camera capabilities
// request camera capabilities
if (request_capabilities(now_ms)) {
return;
}

#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
// configure CX-GB250 to take absolute thermal images
if (configure_cxgb250(now_ms)) {
return;
}
#endif
}

// request status
Expand Down Expand Up @@ -644,7 +656,20 @@ bool AP_Mount_Xacti::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan,
}
return false;
}


#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
// IRThroughColorPalette
if (strcmp(name, get_param_name_str(Param::IRThroughColorPalette)) == 0) {
_thermal.ir_through_color_palette = value;
return false;
}
// IRTemperatureRange
if (strcmp(name, get_param_name_str(Param::IRTemperatureRange)) == 0) {
_thermal.ir_temperate_range = value;
return false;
}
#endif

// unhandled parameter get or set
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s get/set %s res:%ld", send_text_prefix, name, (long int)value);
return false;
Expand Down Expand Up @@ -721,6 +746,14 @@ bool AP_Mount_Xacti::handle_param_get_set_response_string(AP_DroneCAN* ap_dronec
_motor_error = _status.error_status & ((uint32_t)ErrorStatus::MOTOR_INIT_ERROR | (uint32_t)ErrorStatus::MOTOR_OPERATION_ERROR | (uint32_t)ErrorStatus::GIMBAL_CONTROL_ERROR);
_camera_error = _status.error_status & ((uint32_t)ErrorStatus::LENS_ERROR | (uint32_t)ErrorStatus::MEDIA_ERROR);
return false;

#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
} else if (strcmp(name, get_param_name_str(Param::IRTemperatureThreshold_S)) == 0) {
// IRTemperatureThreshold_S, check for expected config
_thermal.got_ir_temp_thresh = true;
_thermal.ir_temp_thresh_ok = (strcmp((const char *)value.data, (const char *)_ir_temp_thresh_config_str) == 0);
return false;
#endif
}

// unhandled parameter get or set
Expand Down Expand Up @@ -759,6 +792,22 @@ bool AP_Mount_Xacti::set_param_int32(Param param, int32_t param_value)
return _set_param_int32_queue->push(SetParamQueueItem{param, param_value});
}

// helper function to set integer parameters
bool AP_Mount_Xacti::get_param_int32(Param param)
{
if (_detected_modules[_instance].ap_dronecan == nullptr) {
return false;
}

// convert param enum to param name string
const char* param_name_str = get_param_name_str(param);
if (_detected_modules[_instance].ap_dronecan->get_parameter_on_node(_detected_modules[_instance].node_id, param_name_str, &param_int_cb)) {
last_send_getset_param_ms = AP_HAL::millis();
return true;
}
return false;
}

// helper function to set string parameters
bool AP_Mount_Xacti::set_param_string(Param param, const AP_DroneCAN::string& param_value)
{
Expand Down Expand Up @@ -1029,6 +1078,106 @@ bool AP_Mount_Xacti::request_status(uint32_t now_ms)
return get_param_string(Param::Status);
}

#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
// configure CX-GB250 to take absolute thermal images
// this is the main function to configure the camera for thermal imaging
bool AP_Mount_Xacti::configure_cxgb250(uint32_t now_ms)
{
// return immediately if configuration attempted within last 3 seconds
if (now_ms - _thermal.last_config_ms < 3000) {
return false;
}
_thermal.last_config_ms = now_ms;

// check IRThroughColorPalette is 10:Manual Radiometry Colorize
if (_thermal.ir_through_color_palette < 0) {
return get_param_int32(Param::IRThroughColorPalette);
} else if (_thermal.ir_through_color_palette != 10) {
// set IRThroughColorPalette to 10:Manual(Radiometry Colorize)
return set_param_int32(Param::IRThroughColorPalette, 10);
}

// check IRTemperatureRange is 0:HighGain
if (_thermal.ir_temperate_range < 0) {
return get_param_int32(Param::IRTemperatureRange);
} else if (_thermal.ir_temperate_range != 0) {
// set IRTemperatureRange to 0:HighGain
return set_param_int32(Param::IRTemperatureRange, 0);
}

// check IRTemperatureThreshold_S is set correctly
if (!_thermal.got_ir_temp_thresh) {
return get_param_string(Param::IRTemperatureThreshold_S);
}
if (!_thermal.ir_temp_thresh_ok) {
if (set_ir_temp_thesholds()) {
// re-fetch temp thresh
_thermal.got_ir_temp_thresh = false;
return true;
}
return false;
}

// send IRColorColor
if (!_thermal.sent_ir_color_control) {
_thermal.sent_ir_color_control = set_ir_color_control();
return _thermal.sent_ir_color_control;
}

// if we got this far then configuration is complete
if (!_thermal.config_complete_msg) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s CX-GB250 config complete", send_text_prefix);
_thermal.config_complete_msg = true;
}

return false;
}

// set IR temperature thresholds
bool AP_Mount_Xacti::set_ir_temp_thesholds()
{
// there are 5 temperature thresholds of 4 characters each
// these are hex numbers in Little Endian format (e.g. 150C = "9600") with the lowest threshold first
// with high gain the range is -50 ~ +150, with low gain the range is -50 ~ +550
AP_DroneCAN::string thresh_config_str {};
const int num_bytes = snprintf((char *)thresh_config_str.data, sizeof(AP_DroneCAN::string::data), "%s", _ir_temp_thresh_config_str);

// sanity check bytes to be sent
if (num_bytes != 20) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s IR temp thesholds bad len:%d", send_text_prefix, num_bytes);
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return false;
}
thresh_config_str.len = num_bytes;

return set_param_string(Param::IRTemperatureThreshold_S, thresh_config_str);
}

// set IR color control which controls the colours used for an "area" (aka temperature range)
bool AP_Mount_Xacti::set_ir_color_control()
{
// create IRColorControl_S parameter string using the below format
// area number: '00' to '05'
// color type: '00': color1 only, '01': graadation from color1 to color2
// color1: '0000':White '0100':Black '0200':Dark Red '0300':Red '0400':Orange '0500':Yellow '0600':Light Green
// '0700':Green '0800':Light Blue '0900':Blue '0A00':Dark Blue '0B00':Purple
// color2: same as above
AP_DroneCAN::string ir_color_control_str {};
const int num_bytes = snprintf((char *)ir_color_control_str.data, sizeof(AP_DroneCAN::string::data), "%s", _ir_color_control_config_str);

// sanity check lenght of string
if (num_bytes != 12) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s color controls bad len:%d", send_text_prefix, num_bytes);
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
return false;
}
ir_color_control_str.len = num_bytes;

// set IRColorControl_S parameter
return set_param_string(Param::IRColorControl_S, ir_color_control_str);
}
#endif

// check if safe to send message (if messages sent too often camera will not respond)
// now_ms should be current system time (reduces calls to AP_HAL::millis)
bool AP_Mount_Xacti::is_safe_to_send(uint32_t now_ms) const
Expand Down
35 changes: 34 additions & 1 deletion libraries/AP_Mount/AP_Mount_Xacti.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,12 @@ class AP_Mount_Xacti : public AP_Mount_Backend
Status,
DateTime,
OpticalZoomMagnification,
LAST = OpticalZoomMagnification, // this should be equal to the final parameter enum
IRThroughColorPalette,
IRTemperatureRange,
IRTemperatureThreshold_S,
IRColorControl_S,
IRColorInfo_S,
LAST = IRColorInfo_S, // this should be equal to the final parameter enum
};
static const char* _param_names[]; // array of Xacti parameter strings

Expand All @@ -144,6 +149,7 @@ class AP_Mount_Xacti : public AP_Mount_Backend

// helper function to get and set parameters
bool set_param_int32(Param param, int32_t param_value);
bool get_param_int32(Param param);
bool set_param_string(Param param, const AP_DroneCAN::string& param_value);
bool get_param_string(Param param);

Expand Down Expand Up @@ -179,6 +185,18 @@ class AP_Mount_Xacti : public AP_Mount_Backend
// returns true if sent so that we avoid immediately trying to also send other messages
bool request_status(uint32_t now_ms);

#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
// configure CX-GB250 to take absolute thermal images
// this is the main function to configure the camera for thermal imaging
bool configure_cxgb250(uint32_t now_ms);

// set IR temperature thresholds
bool set_ir_temp_thesholds();

// set IR color control
bool set_ir_color_control();
#endif

// check if safe to send message (if messages sent too often camera will not respond)
// now_ms is current system time
bool is_safe_to_send(uint32_t now_ms) const;
Expand Down Expand Up @@ -264,6 +282,21 @@ class AP_Mount_Xacti : public AP_Mount_Backend
bool _motor_error; // true if status reports motor or control error (used for health reporting)
bool _camera_error; // true if status reports camera error

#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
// thermal related variables
struct {
uint32_t last_config_ms; // system time that thermal configuration was last sent
int8_t ir_through_color_palette = -1; // latest IRThroughColorPalette param value received from camera (used to confirm correct setting)
int8_t ir_temperate_range = -1; // latest IRTemperatureRange param value received from camera (used to confirm correct setting)
bool got_ir_temp_thresh; // true once IRTemperatureThreshold_S value has been received from camera
bool ir_temp_thresh_ok; // true once IRTemperatureThreshold_S value has been set correctly
bool sent_ir_color_control; // true once IRColorControl_S value has been sent
bool config_complete_msg; // true once config complete message has been sent to user
} _thermal;
static const char* _ir_temp_thresh_config_str; // IR temperature thresholds configuration string
static const char* _ir_color_control_config_str; // IR color control config string
#endif

// DroneCAN related variables
static bool _subscribed; // true once subscribed to receive DroneCAN messages
static struct DetectedModules {
Expand Down

0 comments on commit 4df3fc5

Please sign in to comment.