mirror of https://github.com/ArduPilot/ardupilot
93 lines
3.2 KiB
C++
93 lines
3.2 KiB
C++
#pragma once
|
|
|
|
#include "AP_Proximity_Backend_Serial.h"
|
|
|
|
#if HAL_PROXIMITY_ENABLED
|
|
#define PROXIMITY_SF40C_TIMEOUT_MS 200 // requests timeout after 0.2 seconds
|
|
|
|
class AP_Proximity_LightWareSF40C_v09 : public AP_Proximity_Backend_Serial
|
|
{
|
|
|
|
public:
|
|
|
|
using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial;
|
|
|
|
// update state
|
|
void update(void) override;
|
|
|
|
// get maximum and minimum distances (in meters) of sensor
|
|
float distance_max() const override;
|
|
float distance_min() const override;
|
|
|
|
private:
|
|
|
|
enum RequestType {
|
|
RequestType_None = 0,
|
|
RequestType_Health,
|
|
RequestType_MotorSpeed,
|
|
RequestType_MotorDirection,
|
|
RequestType_ForwardDirection,
|
|
RequestType_DistanceMeasurement
|
|
};
|
|
|
|
// initialise sensor (returns true if sensor is successfully initialised)
|
|
bool initialise();
|
|
void set_motor_speed(bool on_off);
|
|
void set_motor_direction();
|
|
void set_forward_direction();
|
|
|
|
// send request for something from sensor
|
|
void request_new_data();
|
|
void send_request_for_health();
|
|
bool send_request_for_distance();
|
|
|
|
// check and process replies from sensor
|
|
bool check_for_reply();
|
|
bool process_reply();
|
|
void clear_buffers();
|
|
|
|
// reply related variables
|
|
char element_buf[2][10];
|
|
uint8_t element_len[2];
|
|
uint8_t element_num;
|
|
bool ignore_reply; // true if we should ignore the incoming message (because it is just echoing our command)
|
|
bool wait_for_space; // space marks the start of returned data
|
|
|
|
// request related variables
|
|
enum RequestType _last_request_type; // last request made to sensor
|
|
uint8_t _last_sector; // last sector requested
|
|
uint32_t _last_request_ms; // system time of last request
|
|
uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor
|
|
uint8_t _request_count; // counter used to interleave requests for distance with health requests
|
|
|
|
// sensor health register
|
|
union {
|
|
struct PACKED {
|
|
uint16_t motor_stopped : 1;
|
|
uint16_t motor_dir : 1; // 0 = clockwise, 1 = counter-clockwise
|
|
uint16_t motor_fault : 1;
|
|
uint16_t torque_control : 1; // 0 = automatic, 1 = manual
|
|
uint16_t laser_fault : 1;
|
|
uint16_t low_battery : 1;
|
|
uint16_t flat_battery : 1;
|
|
uint16_t system_restarting : 1;
|
|
uint16_t no_results_available : 1;
|
|
uint16_t power_saving : 1;
|
|
uint16_t user_flag1 : 1;
|
|
uint16_t user_flag2 : 1;
|
|
uint16_t unused1 : 1;
|
|
uint16_t unused2 : 1;
|
|
uint16_t spare_input : 1;
|
|
uint16_t major_system_abnormal : 1;
|
|
} _flags;
|
|
uint16_t value;
|
|
} _sensor_status;
|
|
|
|
// sensor config
|
|
uint8_t _motor_speed; // motor speed as reported by lidar
|
|
uint8_t _motor_direction = 99; // motor direction as reported by lidar
|
|
int16_t _forward_direction = 999; // forward direction as reported by lidar
|
|
};
|
|
|
|
#endif // HAL_PROXIMITY_ENABLED
|