AP_Mount: add set_rangefinder_enable for ViewPro

This commit is contained in:
Randy Mackay 2024-03-15 09:58:24 +09:00
parent b9da931059
commit bd8f7bfaac
5 changed files with 39 additions and 4 deletions

View File

@ -898,6 +898,16 @@ bool AP_Mount::get_rangefinder_distance(uint8_t instance, float& distance_m) con
return backend->get_rangefinder_distance(distance_m);
}
// enable/disable rangefinder. Returns true on success
bool AP_Mount::set_rangefinder_enable(uint8_t instance, bool enable)
{
auto *backend = get_instance(instance);
if (backend == nullptr) {
return false;
}
return backend->set_rangefinder_enable(enable);
}
AP_Mount_Backend *AP_Mount::get_primary() const
{
return get_instance(_primary);

View File

@ -272,6 +272,9 @@ public:
// get rangefinder distance. Returns true on success
bool get_rangefinder_distance(uint8_t instance, float& distance_m) const;
// enable/disable rangefinder. Returns true on success
bool set_rangefinder_enable(uint8_t instance, bool enable);
// parameter var table
static const struct AP_Param::GroupInfo var_info[];

View File

@ -209,6 +209,9 @@ public:
// get rangefinder distance. Returns true on success
virtual bool get_rangefinder_distance(float& distance_m) const { return false; }
// enable/disable rangefinder. Returns true on success
virtual bool set_rangefinder_enable(bool enable) { return false; }
protected:
enum class MountTargetType {

View File

@ -573,22 +573,23 @@ bool AP_Mount_Viewpro::send_target_angles(float pitch_rad, float yaw_rad, bool y
}
// send camera command, affected image sensor and value (e.g. zoom speed)
bool AP_Mount_Viewpro::send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value)
bool AP_Mount_Viewpro::send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value, LRFCommand lrf_cmd)
{
// fill in 2 bytes containing sensor, zoom speed, operation command and LRF
// bit0~2: sensor
// bit3~5: zoom speed
// bit6~12: operation command no
// bit13~15: LRF command (unused)
// bit13~15: LRF command
const uint16_t sensor_id = (uint16_t)img_sensor;
const uint16_t zoom_speed = ((uint16_t)value & 0x07) << 3;
const uint16_t operation_cmd = ((uint16_t)cmd & 0x7F) << 6;
const uint16_t rangefinder_cmd = ((uint16_t)lrf_cmd & 0x07) << 13;
// fill in packet
const C1Packet c1_packet {
.content = {
frame_id: FrameId::C1,
sensor_zoom_cmd_be: htobe16(sensor_id | zoom_speed | operation_cmd)
sensor_zoom_cmd_be: htobe16(sensor_id | zoom_speed | operation_cmd | rangefinder_cmd)
}
};
@ -980,4 +981,10 @@ bool AP_Mount_Viewpro::get_rangefinder_distance(float& distance_m) const
return true;
}
// enable/disable rangefinder. Returns true on success
bool AP_Mount_Viewpro::set_rangefinder_enable(bool enable)
{
return send_camera_command(ImageSensor::NO_ACTION, CameraCommand::NO_ACTION, 0, enable ? LRFCommand::CONTINUOUS_RANGING_START : LRFCommand::STOP_RANGING);
}
#endif // HAL_MOUNT_VIEWPRO_ENABLED

View File

@ -95,6 +95,9 @@ public:
// get rangefinder distance. Returns true on success
bool get_rangefinder_distance(float& distance_m) const override;
// enable/disable rangefinder. Returns true on success
bool set_rangefinder_enable(bool enable) override;
protected:
// get attitude as a quaternion. returns true on success
@ -161,6 +164,15 @@ private:
MANUAL_FOCUS = 0x1A
};
// C1 rangefinder commands
enum class LRFCommand : uint8_t {
NO_ACTION = 0x00,
SINGLE_RANGING = 0x01,
CONTINUOUS_RANGING_START = 0x02,
LPCL_CONTINUOUS_RANGING_START = 0x03,
STOP_RANGING = 0x05
};
// C2 camera commands
enum class CameraCommand2 : uint8_t {
SET_EO_ZOOM = 0x53
@ -351,7 +363,7 @@ private:
bool send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef);
// send camera command, affected image sensor and value (e.g. zoom speed)
bool send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value);
bool send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value, LRFCommand lrf_cmd = LRFCommand::NO_ACTION);
// send camera command2 and corresponding value (e.g. zoom as absolute value)
bool send_camera_command2(CameraCommand2 cmd, uint16_t value);