Skip to content

Commit

Permalink
AP_Proximity: add RPLidarC1 support
Browse files Browse the repository at this point in the history
Co-authored-by: Randy Mackay <[email protected]>
  • Loading branch information
FoxSuzuran and rmackay9 committed Mar 11, 2024
1 parent 6534a96 commit e3881ed
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 1 deletion.
8 changes: 7 additions & 1 deletion libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -108,6 +108,8 @@ float AP_Proximity_RPLidarA2::distance_max() const
return 8.0f;
case Model::A2:
return 16.0f;
case Model::C1:
return 10.0f;
case Model::S1:
return 40.0f;
}
Expand All @@ -121,8 +123,8 @@ float AP_Proximity_RPLidarA2::distance_min() const
case Model::UNKNOWN:
return 0.0f;
case Model::A1:
return 0.2f;
case Model::A2:
case Model::C1:
case Model::S1:
return 0.2f;
}
Expand Down Expand Up @@ -334,6 +336,10 @@ void AP_Proximity_RPLidarA2::parse_response_device_info()
model = Model::A2;
device_type = "A2";
break;
case 0x41:
model=Model::C1;
device_type="C1";
break;
case 0x61:
model = Model::S1;
device_type = "S1";
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_Proximity/AP_Proximity_RPLidarA2.h
Original file line number Diff line number Diff line change
Expand Up @@ -151,6 +151,7 @@ class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial
UNKNOWN,
A1,
A2,
C1,
S1,
} model = Model::UNKNOWN;

Expand Down

0 comments on commit e3881ed

Please sign in to comment.