You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
138 lines
3.6 KiB
138 lines
3.6 KiB
#include "Underwater_Sonar.h" |
|
|
|
#include <AP_Logger/AP_Logger.h> |
|
|
|
#define MY_USE_DEBUG 1 |
|
#if MY_USE_DEBUG |
|
|
|
# define Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_CRITICAL,fmt, ## args) |
|
#else |
|
// |
|
# define Debug(fmt, args ...) |
|
#endif |
|
|
|
Underwater_Sonar::Underwater_Sonar(void): |
|
ucCRCHi(0), |
|
ucCRCLo(0), |
|
iIndex(0) |
|
{ |
|
_singleton = this; |
|
} |
|
|
|
bool Underwater_Sonar::init(const AP_SerialManager& serial_manager) |
|
{ |
|
_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Underwater_Sonar,0); |
|
if (_uart != nullptr) { |
|
_uart->begin(4800U); |
|
|
|
return true; |
|
// _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); |
|
} |
|
return false; |
|
} |
|
|
|
bool Underwater_Sonar::read(void) |
|
{ |
|
if (_uart == nullptr) { |
|
return false; |
|
} |
|
// Debug("read"); |
|
bool ret = false; |
|
|
|
|
|
uint32_t nbytes = _uart->available(); |
|
uint32_t data_len = nbytes; |
|
while(nbytes-->0) |
|
{ |
|
uint8_t temp = _uart->read(); |
|
sonar_data[data_len-nbytes] = temp; |
|
// Debug("read %d:%d",nbytes,temp); |
|
if(_decode(temp)){ |
|
|
|
// write(&temp,1); |
|
for(int j = 0; j < 8; j++){ |
|
// radar_data = _sonar[j]; |
|
// memcpy(*radar_data,_sonar[j],sizeof(_sonar[j])); |
|
radar_data[j] = atof(_term[j]); |
|
|
|
// cout << j << " : " << radar_data[j] << endl; |
|
} |
|
// data_index +=1; |
|
// Debug("%d: %d,%.2f,%d,%d,%d,%.2f,%.d,%d ",data_index,(int)radar_data[0],radar_data[1],(int)radar_data[2],(int)radar_data[3], \ |
|
// (int)radar_data[4],radar_data[5],(int)radar_data[6],(int)radar_data[7]); |
|
ret = true; |
|
} |
|
} |
|
// Debug("read %s",sonar_data); |
|
return ret; |
|
} |
|
|
|
bool Underwater_Sonar::_decode(char c) |
|
{ |
|
bool valid_sentence = false; |
|
// _term[_term_num][_term_offset]; |
|
switch (c) { |
|
case '\r': |
|
case '\n': |
|
_term_offset = 0; |
|
_term_num = 0; |
|
valid_sentence = true; |
|
return valid_sentence; |
|
case ' ': |
|
_term_num+=1; |
|
_term_offset = 0; |
|
return valid_sentence; |
|
|
|
} |
|
if (_term_offset < sizeof(_term[_term_num]) - 1) |
|
_term[_term_num][_term_offset++] = c; |
|
return valid_sentence; |
|
} |
|
|
|
|
|
void Underwater_Sonar::write(const uint8_t *data, uint16_t len) |
|
{ |
|
|
|
if (_uart == nullptr) { |
|
return; |
|
} |
|
if (_uart->txspace() > len) { |
|
_uart->write(data, len); |
|
} else { |
|
// Debug("FLOW: Not enough TXSPACE"); |
|
} |
|
} |
|
|
|
void Underwater_Sonar::update(const AP_SerialManager& serial_manager, const struct Location &_loc){ |
|
if (_uart == nullptr) { |
|
_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Underwater_Sonar,0); |
|
if (_uart != nullptr) { |
|
_uart->begin(4800U); |
|
Debug("---------Underwater_Sonar: ok\n"); |
|
} |
|
} |
|
if(read()){ |
|
AP_Logger *logger = AP_Logger::get_singleton(); |
|
if (logger == nullptr) { |
|
Debug("log error"); |
|
return; |
|
} |
|
logger->Write_Underwater_Sonar(_loc); |
|
|
|
Debug("%d: %d,%.2f,%d,%d,%d,%.2f,%.d,%d ",data_index,(int)radar_data[0],radar_data[1],(int)radar_data[2],(int)radar_data[3], \ |
|
(int)radar_data[4],radar_data[5],(int)radar_data[6],(int)radar_data[7]); |
|
|
|
} |
|
|
|
} |
|
// singleton instance |
|
Underwater_Sonar *Underwater_Sonar::_singleton; |
|
|
|
namespace AP { |
|
|
|
Underwater_Sonar &underwater_sonar() |
|
{ |
|
return *Underwater_Sonar::get_singleton(); |
|
} |
|
|
|
}
|
|
|