fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 03:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ll40ls: astyle

Browse Source
This commit is contained in:
Ban Siesta
2015-05-24 09:44:10 +01:00
parent 3efaeabd5b
commit e67f681935
7 changed files with 667 additions and 628 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ll40ls/LidarLitePWM.cpp
+52 -51
View File
@@ -48,40 +48,40 @@ static const int ERROR = -1;
#endif
LidarLitePWM::LidarLitePWM() :
_terminateRequested(false),
_pwmSub(-1),
_pwm{},
_rangePub(-1),
_range{}
_terminateRequested(false),
_pwmSub(-1),
_pwm{},
_rangePub(-1),
_range{}
{
}
int LidarLitePWM::init()
{
_pwmSub = orb_subscribe(ORB_ID(pwm_input));
_pwmSub = orb_subscribe(ORB_ID(pwm_input));
if(_pwmSub == -1) {
return ERROR;
}
if (_pwmSub == -1) {
return ERROR;
}
_range.type = RANGE_FINDER_TYPE_LASER;
_range.valid = false;
_rangePub = orb_advertise(ORB_ID(sensor_range_finder), &_range);
_range.type = RANGE_FINDER_TYPE_LASER;
_range.valid = false;
_rangePub = orb_advertise(ORB_ID(sensor_range_finder), &_range);
return OK;
return OK;
}
void LidarLitePWM::print_info()
{
printf("poll interval: %u ticks\n", getMeasureTicks());
printf("distance: %ucm (0x%04x)\n",
(unsigned)_range.distance, (unsigned)_range.distance);
printf("poll interval: %u ticks\n", getMeasureTicks());
printf("distance: %ucm (0x%04x)\n",
(unsigned)_range.distance, (unsigned)_range.distance);
}
void LidarLitePWM::print_registers()
{
printf("Not supported in PWM mode\n");
printf("Not supported in PWM mode\n");
}
void LidarLitePWM::start()
@@ -91,52 +91,53 @@ void LidarLitePWM::start()
void LidarLitePWM::stop()
{
//TODO: stop measurement task
_terminateRequested = true;
//TODO: stop measurement task
_terminateRequested = true;
}
int LidarLitePWM::measure()
{
int result = OK;
int result = OK;
_range.error_count = _pwm.error_count;
_range.maximum_distance = get_maximum_distance();
_range.minimum_distance = get_minimum_distance();
_range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite
_range.distance_vector[0] = _range.distance;
_range.just_updated = 0;
_range.valid = true;
_range.error_count = _pwm.error_count;
_range.maximum_distance = get_maximum_distance();
_range.minimum_distance = get_minimum_distance();
_range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite
_range.distance_vector[0] = _range.distance;
_range.just_updated = 0;
_range.valid = true;
//TODO: due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0)
if(_range.distance <= 0.0f) {
_range.valid = false;
_range.error_count++;
result = ERROR;
}
//TODO: due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0)
if (_range.distance <= 0.0f) {
_range.valid = false;
_range.error_count++;
result = ERROR;
}
orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range);
orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range);
return result;
return result;
}
int LidarLitePWM::collect()
{
//Check PWM
bool update;
orb_check(_pwmSub, &update);
if(update) {
orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm);
_range.timestamp = hrt_absolute_time();
return OK;
}
//Check PWM
bool update;
orb_check(_pwmSub, &update);
//Timeout readings after 0.2 seconds and mark as invalid
if(hrt_absolute_time() - _range.timestamp > LL40LS_CONVERSION_TIMEOUT*2) {
_range.timestamp = hrt_absolute_time();
_range.valid = false;
orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range);
return ERROR;
}
if (update) {
orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm);
_range.timestamp = hrt_absolute_time();
return OK;
}
return EAGAIN;
//Timeout readings after 0.2 seconds and mark as invalid
if (hrt_absolute_time() - _range.timestamp > LL40LS_CONVERSION_TIMEOUT * 2) {
_range.timestamp = hrt_absolute_time();
_range.valid = false;
orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range);
return ERROR;
}
return EAGAIN;
}