Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: Change from method reference to macro

Browse Source
This commit is contained in:
murata 2019-03-27 20:10:57 +09:00 committed by Randy Mackay
parent 7340502f18
commit fb544cab78
4 changed files with 16 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp
View File

@ -98,23 +98,14 @@ bool AP_Proximity_TeraRangerTower::read_sensor_data()
// check if message has right CRC // check if message has right CRC
if (crc_crc8(buffer, 18) == buffer[18]){ if (crc_crc8(buffer, 18) == buffer[18]){
uint16_t d1 = process_distance(buffer[2], buffer[3]); update_sector_data(0, UINT16_VALUE(buffer[2], buffer[3])); // d1
uint16_t d2 = process_distance(buffer[4], buffer[5]); update_sector_data(45, UINT16_VALUE(buffer[16], buffer[17])); // d8
uint16_t d3 = process_distance(buffer[6], buffer[7]); update_sector_data(90, UINT16_VALUE(buffer[14], buffer[15])); // d7
uint16_t d4 = process_distance(buffer[8], buffer[9]); update_sector_data(135, UINT16_VALUE(buffer[12], buffer[13])); // d6
uint16_t d5 = process_distance(buffer[10], buffer[11]); update_sector_data(180, UINT16_VALUE(buffer[10], buffer[11])); // d5
uint16_t d6 = process_distance(buffer[12], buffer[13]); update_sector_data(225, UINT16_VALUE(buffer[8], buffer[9])); // d4
uint16_t d7 = process_distance(buffer[14], buffer[15]); update_sector_data(270, UINT16_VALUE(buffer[6], buffer[7])); // d3
uint16_t d8 = process_distance(buffer[16], buffer[17]); update_sector_data(315, UINT16_VALUE(buffer[4], buffer[5])); // d2
update_sector_data(0, d1);
update_sector_data(45, d8);
update_sector_data(90, d7);
update_sector_data(135, d6);
update_sector_data(180, d5);
update_sector_data(225, d4);
update_sector_data(270, d3);
update_sector_data(315, d2);
message_count++; message_count++;
} }
@ -123,11 +114,6 @@ bool AP_Proximity_TeraRangerTower::read_sensor_data()
return (message_count > 0); return (message_count > 0);
} }
uint16_t AP_Proximity_TeraRangerTower::process_distance(uint8_t buf1, uint8_t buf2)
{
return (buf1 << 8) + buf2;
}
// process reply // process reply
void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_cm) void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
{ {

1
libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h
View File

@ -27,7 +27,6 @@ private:
// check and process replies from sensor // check and process replies from sensor
bool read_sensor_data(); bool read_sensor_data();
void update_sector_data(int16_t angle_deg, uint16_t distance_cm); void update_sector_data(int16_t angle_deg, uint16_t distance_cm);
uint16_t process_distance(uint8_t buf1, uint8_t buf2);
// reply related variables // reply related variables
AP_HAL::UARTDriver *uart = nullptr; AP_HAL::UARTDriver *uart = nullptr;

31
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp
View File

@ -150,24 +150,14 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
//check if message has right CRC //check if message has right CRC
if (crc_crc8(buffer, 19) == buffer[19]){ if (crc_crc8(buffer, 19) == buffer[19]){
update_sector_data(0, UINT16_VALUE(buffer[2], buffer[3])); // d1
uint16_t d1 = process_distance(buffer[2], buffer[3]); update_sector_data(45, UINT16_VALUE(buffer[4], buffer[5])); // d2
uint16_t d2 = process_distance(buffer[4], buffer[5]); update_sector_data(90, UINT16_VALUE(buffer[6], buffer[7])); // d3
uint16_t d3 = process_distance(buffer[6], buffer[7]); update_sector_data(135, UINT16_VALUE(buffer[8], buffer[9])); // d4
uint16_t d4 = process_distance(buffer[8], buffer[9]); update_sector_data(180, UINT16_VALUE(buffer[10], buffer[11])); // d5
uint16_t d5 = process_distance(buffer[10], buffer[11]); update_sector_data(225, UINT16_VALUE(buffer[12], buffer[13])); // d6
uint16_t d6 = process_distance(buffer[12], buffer[13]); update_sector_data(270, UINT16_VALUE(buffer[14], buffer[15])); // d7
uint16_t d7 = process_distance(buffer[14], buffer[15]); update_sector_data(315, UINT16_VALUE(buffer[16], buffer[17])); // d8
uint16_t d8 = process_distance(buffer[16], buffer[17]);
update_sector_data(0, d1);
update_sector_data(45, d2);
update_sector_data(90, d3);
update_sector_data(135, d4);
update_sector_data(180, d5);
update_sector_data(225, d6);
update_sector_data(270, d7);
update_sector_data(315, d8);
message_count++; message_count++;
} }
@ -176,11 +166,6 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
return (message_count > 0); return (message_count > 0);
} }
uint16_t AP_Proximity_TeraRangerTowerEvo::process_distance(uint8_t buf1, uint8_t buf2)
{
return (buf1 << 8) + buf2;
}
// process reply // process reply
void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_cm) void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
{ {

1
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.h
View File

@ -27,7 +27,6 @@ private:
void initialise_modes(); void initialise_modes();
bool read_sensor_data(); bool read_sensor_data();
void update_sector_data(int16_t angle_deg, uint16_t distance_cm); void update_sector_data(int16_t angle_deg, uint16_t distance_cm);
uint16_t process_distance(uint8_t buf1, uint8_t buf2);
void set_mode(const uint8_t *c, int length); void set_mode(const uint8_t *c, int length);
enum InitState { enum InitState {