aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Camera: fix MAV_CMD_START_CAPTURE handling

Browse Source
This commit is contained in:
Asif Khan 2023-09-07 15:35:40 +05:30 committed by Peter Barker
parent 5298187aaa
commit 2cc080620c
2 changed files with 66 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
libraries/AP_Camera/AP_Camera.cpp
View File

@ -81,10 +81,24 @@ void AP_Camera::take_picture()
{
WITH_SEMAPHORE(_rsem);
if (primary == nullptr) {
return;
// call for each instance
for (uint8_t i = 0; i < AP_CAMERA_MAX_INSTANCES; i++) {
if (_backends[i] != nullptr) {
_backends[i]->take_picture();
}
}
primary->take_picture();
}
bool AP_Camera::take_picture(uint8_t instance)
{
WITH_SEMAPHORE(_rsem);
auto *backend = get_instance(instance);
if (backend == nullptr) {
return false;
}
backend->take_picture();
return true;
}
// take multiple pictures, time_interval between two consecutive pictures is in miliseconds
@ -93,10 +107,24 @@ void AP_Camera::take_multiple_pictures(uint32_t time_interval_ms, int16_t total_
{
WITH_SEMAPHORE(_rsem);
if (primary == nullptr) {
return;
// call for all instances
for (uint8_t i = 0; i < AP_CAMERA_MAX_INSTANCES; i++) {
if (_backends[i] != nullptr) {
_backends[i]->take_multiple_pictures(time_interval_ms, total_num);
}
}
primary->take_multiple_pictures(time_interval_ms, total_num);
}
bool AP_Camera::take_multiple_pictures(uint8_t instance, uint32_t time_interval_ms, int16_t total_num)
{
WITH_SEMAPHORE(_rsem);
auto *backend = get_instance(instance);
if (backend == nullptr) {
return false;
}
backend->take_multiple_pictures(time_interval_ms, total_num);
return true;
}
// start/stop recording video
@ -251,17 +279,38 @@ MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet)
}
return MAV_RESULT_DENIED;
case MAV_CMD_IMAGE_START_CAPTURE:
if (!is_zero(packet.param2) || !is_equal(packet.param3, 1.0f) || !is_zero(packet.param4)) {
// Its a multiple picture request
if (is_equal(packet.param3, 0.0f)) {
take_multiple_pictures(packet.param2*1000, -1);
} else {
take_multiple_pictures(packet.param2*1000, packet.param3);
// check if this is a single picture request
if (is_equal(packet.param3, 1.0f)) {
if (is_zero(packet.param1)) {
// take pictures for every backend
take_picture();
return MAV_RESULT_ACCEPTED;
}
if (take_picture(packet.param1-1)) {
return MAV_RESULT_ACCEPTED;
}
} else if (is_zero(packet.param3)) {
// multiple picture request, take pictures forever
if (is_zero(packet.param1)) {
// take pictures for every backend
take_multiple_pictures(packet.param2*1000, -1);
return MAV_RESULT_ACCEPTED;
}
if (take_multiple_pictures(packet.param1-1,packet.param2*1000, -1)) {
return MAV_RESULT_ACCEPTED;
}
} else {
// take multiple pictures equal to the number specified in param3
if (is_zero(packet.param1)) {
// take pictures for every backend
take_multiple_pictures(packet.param2*1000, packet.param3);
return MAV_RESULT_ACCEPTED;
}
if (take_multiple_pictures(packet.param1-1,packet.param2*1000, packet.param3)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_ACCEPTED;
}
take_picture();
return MAV_RESULT_ACCEPTED;
return MAV_RESULT_UNSUPPORTED;
case MAV_CMD_CAMERA_TRACK_POINT:
if (set_tracking(TrackingType::TRK_POINT, Vector2f{packet.param1, packet.param2}, Vector2f{})) {
return MAV_RESULT_ACCEPTED;
@ -433,20 +482,6 @@ void AP_Camera::update()
}
}
// take_picture - take a picture
void AP_Camera::take_picture(uint8_t instance)
{
WITH_SEMAPHORE(_rsem);
auto *backend = get_instance(instance);
if (backend == nullptr) {
return;
}
// call backend
backend->take_picture();
}
// start/stop recording video. returns true on success
// start_recording should be true to start recording, false to stop recording
bool AP_Camera::record_video(uint8_t instance, bool start_recording)

3
libraries/AP_Camera/AP_Camera.h
View File

@ -113,11 +113,12 @@ public:
// take a picture
void take_picture();
void take_picture(uint8_t instance);
bool take_picture(uint8_t instance);
// take multiple pictures, time_interval between two consecutive pictures is in miliseconds
// total_num is number of pictures to be taken, -1 means capture forever
void take_multiple_pictures(uint32_t time_interval_ms, int16_t total_num);
bool take_multiple_pictures(uint8_t instance, uint32_t time_interval_ms, int16_t total_num);
// start/stop recording video
// start_recording should be true to start recording, false to stop recording