Difference between revisions of "F17: Optimus"
Proj user5 (talk | contribs) (→Algorithm for interfacing LIDAR to SJOne board and obtaining the obstacle info) |
Proj user5 (talk | contribs) (→Sensor Controller) |
||
| Line 725: | Line 725: | ||
The entire flowchart for communicating with the LIDAR and receiving data from it is shown in the figure below: | The entire flowchart for communicating with the LIDAR and receiving data from it is shown in the figure below: | ||
[[File:CmpE243_F17_Optimus_LIDARflowchart.png|700px|thumb|center|Flowchart for communicating with the LIDAR and receiving obstacle information from it]] | [[File:CmpE243_F17_Optimus_LIDARflowchart.png|700px|thumb|center|Flowchart for communicating with the LIDAR and receiving obstacle information from it]] | ||
| + | |||
| + | =====Testing the data obtained from the LIDAR===== | ||
| + | To perform the initial testing of the LIDAR and to check if we are getting the correct obstacle info, we have made a setup enclosing the LIDAR on all four sides. So, by plotting the distance info given by the LIDAR in Microsoft Excel we can visualize a map of the obstacles as detected by the LIDAR. The map plotted in Excel after closing almost all four sides of the LIDAR can be shown in the figure shown below. | ||
| + | |||
| + | [Figure of a plot of the LIDAR readings (done in Excel) will be added] | ||
=====CAN DBC messages sent from the Sensor Controller===== | =====CAN DBC messages sent from the Sensor Controller===== | ||
Revision as of 05:21, 18 December 2017
Contents
- 1 Optimus
- 2 Abstract
- 3 Objectives & Introduction
- 4 Team Members & Responsibilities
- 5 Project Schedule
- 6 Parts List & Cost
- 7 CAN Communication
- 8 DBC File
- 9 Hardware & Software Architecture
- 10 Motor Controller
- 10.1 Design & Implementation
- 10.2 Hardware Design
- 10.3 Hardware Interface
- 10.4 Software Design
- 10.5 Implementation
- 10.6 Testing & Technical Challenges
- 10.7 Sensor Controller
- 10.7.1 Introduction
- 10.7.2 Hardware Implementation
- 10.7.3 Software Implementation
- 10.7.3.1 Approach for obtaining the data from the LIDAR
- 10.7.3.2 Algorithm for interfacing LIDAR to SJOne board and obtaining the obstacle info
- 10.7.3.3 Flowchart for Communicating with the LIDAR and receiving obstacle information
- 10.7.3.4 Testing the data obtained from the LIDAR
- 10.7.3.5 CAN DBC messages sent from the Sensor Controller
- 10.8 Android Application
- 10.9 Bluetooth Controller
- 10.10 Geographical Controller
- 10.11 Software Design
- 10.12 Implementation
- 10.13 Testing & Technical Challenges
- 10.14 Git Project Management
- 11 Common Technical Challenges
- 12 Project Videos
- 13 Conclusion
- 14 Project Source Code
- 15 References
- 16 Acknowledgement
Optimus
Optimus - Self Navigating R/C Car powered by SJOne(LPC1758) micro controller.
Abstract
Objectives & Introduction
Team Members & Responsibilities
- Motor Controller
- Unnikrishnan
- Rajul
- Android and Communication Bridge
- Parimal
- Kripanand Jha
- Unnikrishnan
- Geographical Controller:
- Master Controller:
- Revathy
- Sensor and I/O Controller:
- Sushma
- Supradeep
- Harshitha
- Integration Testing
- Unnikrishnan
- Sneha
- Kripanand Jha
Project Schedule
Legend:
Major Feature milestone , CAN Master Controller , Sensor & IO Controller , Android Controller, Motor Controller , Geo , Testing, Ble controller, Team Goal
| Week# | Date | Planned Task | Actual | Status |
|---|---|---|---|---|
| 1 | 9/23/2017 |
|
|
Complete. |
| 2 | 9/30/2017 |
|
|
Complete |
| 3 | 10/14/2017 |
|
|
Complete |
| 4 | 10/21/2017 |
|
|
complete |
| 5 | 10/28/2017 |
|
|
Complete |
| 6 | 11/07/2017 |
|
|
Complete |
| 7 | 11/14/2017 |
|
|
Complete. |
| 8 | 11/21/2017 |
|
|
Complete. |
| 9 | 11/28/2017 |
|
|
Complete. |
| 10 | 12/5/2017 |
|
|
Complete |
| 11 | 12/12/2017 |
|
|
On Track |
Parts List & Cost
The Project bill of materials is as listed in the table below.
| SNo. | Component | Units | Total Cost |
|---|---|---|---|
| General System Components | |||
| 1 | SJ One Board (LPC 1758) | 5 | $400 |
| 2 | Traxaas RC Car | 1 | From Prof. Kaikai Liu |
| 3 | CAN Transceivers | 15 | $55 |
| 4 | PCAN dongle | 1 | From Preet |
| 5 | PCB Manufacturing | 5 | $70 |
| 6 | 3D printing | 2 | From Marvin |
| 6 | General Hardware components( Connectors,standoffs,Soldering Kits) | 1 | $40 |
| 7 | Power Bank | 1 | $41.50 |
| 8 | LED Digital Display | 1 | From Preet |
| 9 | Acrylic Board | 1 | $12.53 |
| Sensor/IO Controller Components | |||
| 10 | RP Lidar | 1 | $449 |
| Geo Controller Components | |||
| 11 | GPS Module | 1 | $50 |
| Bluetooth Bridge Controller Component | |||
| 12 | Bluetooth Module | 1 | $34.95 |
| Drive Controller Component | |||
| 13 | RPM Sensor | 1 | $20 |
CAN Communication
System Nodes : MASTER , MOTOR , BLE , SENSOR , GEO
| SNo. | Message ID | Message from Source Node | Receivers |
|---|---|---|---|
| Master Controller Message | |||
| 1 | 2 | System Start command to start motor | Motor |
| 2 | 17 | Target Speed-Steer Signal to Motor | Motor |
| 3 | 194 | Telemetry Message to Display it on Android | BLE |
| Sensor Controller Message | |||
| 4 | 3 | Lidar Detections of obstacles in 360 degree grouped as sectors | Master,BLE |
| 5 | 36 | Heartbeat | Master |
| Geo Controller Message | |||
| 6 | 195 | Compass, Destination Reached flag, Checkpoint id signals | Master,BLE |
| 7 | 4 | Turning Angle | Master,BLE |
| 8 | 4 | Heartbeat | Master |
| Bluetooth Bridge Controller Message | |||
| 9 | 1 | Send start command received from Android app to master | Master |
| 10 | 38 | Heartbeat | Master |
| 11 | 213 | Checkpoint Count from AndroidApp | Geo |
| 12 | 212 | Checkpoints (Lat, Long) from Android App | Geo |
DBC File
https://gitlab.com/optimus_prime/optimus/blob/master/_can_dbc/243.dbc
Hardware & Software Architecture
Master Controller Design & Implementation
Software Architecture Design
The Master Controller Integrates the functionality of all other controllers and it acts as the Central Control Unit of the R/C car. Two of the major functionalities handled by Mater Controller is Obstacle avoidance and Route Manuevering.
The overview of Master Controller Software Architecture is to shown in the below figure.
As an analogy to Human driving, it receives the imputs from sensors to determine the surrounding of the R/C car and take decisions based on the environments and commands from the user. The input data it receives includes the following. The output of the Master controller is to direct the Motor with target Speed and Steering direction.
1. Lidar Object Detections - To determine if there is any obstacle in the path of navigation
2. GPS and Compass Reading - To understand the Heading and Bearing angle to decide the direction of movement
3. User command from Android - To stop or Navigate to the Destination
Software Implementation
To Add:
Obstacle Detection design
Obstacle Avoidance Algorithm
Overall Control Flow
Debug Leds map
GPIO interface to led, CAN txrx
Description of control flow(periodic tasks)
Handling of Can messages
Unit testing
Issues
Motor Controller
Design & Implementation
The Motor Controller is responsible for the Movement and Steering action of the Car. It includes two types of motors, DC motor for movement and DC Servo motor for Steering. The Motor has an inbuilt driver called ESC (Electronic Speed Control) Circuit used the manipulate the speed and steering of the Car. It has a PWM input for both Servo Motor and DC Motor. We are using RPM sensor to take the feedback from the motor to monitor the speed.
Hardware Design
| SJOne Pin Diagram | ||
|---|---|---|
| Sr.No | Pin Number | Pin Function |
| 1 | P0.0 | CAN RX |
| 2 | P0.1 | CAN TX |
| 3 | P2.0 | Servo motor |
| 4 | P2.1 | DC motor |
| 5 | P2.5 | Speed Encoder |
- Hardware Specifications
1. DC Motor
Our car came with Titan 12T 550 brushed motor and waterproof ESC. The ESC drives the DC motor based on the Pulse Width modulation (PWM) applied to it. The power supply required for this motor is 8.4 V. Maximum speed of upto 30mph can be achieved. The rotational speed is proportional to the EMF generated in its coil and the torque is proportional to the current.The main connection pins driving the motor are VCC,GND and the Control pin (PWM). The pin P2.1 of SJ-one board is connected to supply the required PWM to the motor.
The basic working principle of DC motor is illustrated in the following figure :
Since the preprogrammed controller has to be replaced by using our design ,the DC motor is then tested with Digital Oscilloscope for getting the frequency of operation and equivalent PWM values for full throttle condition in the forward as well as backward condition.
It was observed from the waveform that the frequency of operation is 100Hz. The range of operational duty cycle is 10% to 20% with 15% being the neutral value or the stop condition.
In order to accelerate the car a PWM value in the range of 15.6%-20.0% is applied. The 15.6 is the minimum pickup PWM that should be supplied in order to get the car moving at full load.
2. Servo Motor
The servomotor used in the car is #2056 a waterproof all weather-action and double the steering power as compared to standard servos. The servo motor is responsible for controlling the steering action of right or left by applying a suitable PWM pulse. The servo motor can be driven with 3.3 V power supply. The pin P2.0 of SJ-one board is connected to supply the required PWM to the motor. After testing the servo motor, we found that the frequency of operation is 100Hz and the operational duty cycle range is 10.0%-20.0% with 15% being the neutral value. For a full right deflection, we provide input PWM pulse ranging from 15.0-20.0% and for full deflection to the left we apply 10.0-15.0% of PWM.
3. Speed Sensor
The speed feedback is monitored through the speed encoder which works on the Hall-effect principle.
The Hall-effect speed sensor works as a transducer whose output voltage varies in response to the magnetic field.
The sensor is mounted on the Spur gear instead of the wheel. The sensor would detect the rotation of axle. The motor controller would detect whenever the magnet is aligned with the sensor. This would generate a pulse. The pulse is detected in the form of rising-edge interrupt.
This gives the wheel rotation count. The wheel rotates for every 1/4th rotation of the spur gear. The rotation count can then be converted to rpm to calculate the speed of the car.
Hardware Interface
The CAN bus is used to send and receive messages to and from the Master Controller. The motor controller receives driving and steering signals from the master. The speed calculation is performed using the speed sensor and is sent on the bus, which will be received by the IO controller for display purposes.
Software Design
The following diagram describes the flow of the software implementation for the motor driver and speed feedback mechanism.
Implementation
The motor controller receives all its signals from Master controller from the CAN bus. The motor controller receives the steer and drive command from the master. The motor controller receives the System start command which boots and decodes further drive signals to the motor controller. Upon receiving the drive command the motor controller decodes the steering action. Upon receiving suitable data about the obstacle from sensor controller the master controller relays appropriate steering action. To achieve better performance in steering, the turn is categorized as FULL and HALF. This gives better precision in turning.
- Speed Regulation:
Upon detection of uphill the pulse received from the speed encoder reduces. This is detected and the motor feedback is designed such that the speed is increased by providing higher value of PWM value to drive the DC motor. Similarly, for downhill the pulse count received increases which is detected by the speed encoder and the speed is reduced by applying reduced PWM.
Testing & Technical Challenges
- Wheel Alignment Error
Though the neutral value of PWM is 15% at which the servo is supposed to be aligned straight. In practice, however when we tested the car for straight run slight deflection towards right was observed when the PWM pulse width was set to 15.0 %. Thus, to correct this, we provided correction value of -0.98 giving a resultant PWM pulse width of 14.02%. Thus, we fixed the wheel alignment and obtained the desired straight path traversal.
- Speed Sensor Assembly
The speed encoder was assembled on the spur gear of the car. The installation at first was such that outer fitting was large and was avoiding the pulse trigger by the magnet.As a result of which we were unable to modulate speed.Issue was resolved by using the correct outer assembly of the gear which generated the speed feedback.
Sensor Controller
The Sensor is for detecting and avoiding obstacles. For this purpose we have used RPLIDAR by SLAMTEC.
Introduction
The RPLIDAR A2 is a 360 degree 2D laser scanner (LIDAR) solution developed by SLAMTEC. It can take up to 4000 samples of laser ranging per second with high rotation speed. And equipped with SLAMTEC patented OPTMAG technology, it breakouts the life limitation of traditional LIDAR system so as to work stably for a long time. The system can perform 2D 360-degree scan within a 6-meter range. The generated 2D point cloud data can be used in mapping, localization and object/environment modeling. The typical scanning frequency of the RPLIDAR A2 is 10hz (600rpm).
Under this condition, the resolution will be 0.9°. And the actual scanning frequency can be freely adjusted within the 5-15hz range according to the requirements of users. The RPLIDAR A2 adopts the low cost laser triangulation measurement system developed by SLAMTEC, which makes the RPLIDAR A2 has excellent performance in all kinds of indoor environment and outdoor environment without direct sunlight exposure.
This LIDAR consists of a range scanner core and the mechanical powering part which makes the core rotate at a high speed. When it functions normally, the scanner will rotate and scan clockwise. And users can get the range scan data via the communication interface of the RPLIDAR (UART) and control the start, stop and rotating speed of the rotate motor via PWM.
A laser beam is sent out by the transmitter and the reflected laser beam is received back. Depending on the time taken to receive the beam back, the distance of the obstacle is calculated. I f there is no obstacle, the beam will not be reflected back.
Hardware Implementation
Specifications of the LIDAR
The specifications of the LIDAR as mentioned in the datasheet are as follows:
Power Supply: 5V
Serial Communication interface: UART
Baud Rate for the UART: 115200
Working mode of the UART: 8N1
PWM Maximum Voltage: 5V (Typical 3.3V)
PWM frequency: 25KHz
Duty Cycle of the PWM wave: 60% - 100%
Connections to the SJOne Board
The LIDAR works with a UART interface and hence has been connected to the UART3 pins of of the SJOne board i.e. P4.28 and P4.29. As the LIDAR needs a 5V supply, it is provided from the PCB (which is powered through a power bank) instead of the SJOne board which can supply only 3.3V. The connections can be seen in the figure below.
Software Implementation
Approach for obtaining the data from the LIDAR
The LIDAR senses all the obstacles around it (360 degrees upto a range of 6000cm) one degree at a time. This means that for one rotation of the LIDAR WE GET 360 values i.e. 360 angles with their corresponding obstacle information. It takes 180ms for the LIDAR to complete one 360 degree scan. Since we do not need obstacle information for each and every angle, we group a few angles together into "sectors" and consider the nearest object present in a sector as an obstacle. To identify how far an obstacle is located, the distance values are grouped into "tracks" i.e 0cm to 25cm is track 1 and 25cm to 50cm is track 2 and so on. The motor will take action depending on the track in which an obstacle is present.
Algorithm for interfacing LIDAR to SJOne board and obtaining the obstacle info
Step 1: Send a GET_HEALTH (0XA5 0X52) Request. If the receive times out it is a communication error.
Step 2: Check if a ‘protection stop’ is happening. If it is happening then send a RESET (0XA5 0X40). Again check for ‘protection stop’ and if it it still set, send a RESET again. If ‘protection stop’ is set even after sending RESET multiple times it means there is a hardware defect. If there is no hardware defect, proceed to the next step.
Step 3: Send a START_SCAN (0XA5 0X20) request. Wait for the response header. If there is no timeout, read the measurement sample. Otherwise check HEALTH_STATUS and MOTOR_STATUS again. Send START_SCAN again.
Step 4: Continuously read the measurement samples.The data sent from the LIDAR will contain the start bit, angle, distance and quality. The start bit is set to 1 after every single 360 degree scan. The angle and distance represent to the motor angle and the obstacle in that corresponding angle. The quality represents the strength of the reflected beam. If the quality is zero it means that there is no obstacle in that direction. This data is processed to be group the information into sectors and tracks.
Step 5: If we wish to stop the readings, send a STOP (0XA5 0X25) request. This is the end of operation.
Flowchart for Communicating with the LIDAR and receiving obstacle information
The entire flowchart for communicating with the LIDAR and receiving data from it is shown in the figure below:
Testing the data obtained from the LIDAR
To perform the initial testing of the LIDAR and to check if we are getting the correct obstacle info, we have made a setup enclosing the LIDAR on all four sides. So, by plotting the distance info given by the LIDAR in Microsoft Excel we can visualize a map of the obstacles as detected by the LIDAR. The map plotted in Excel after closing almost all four sides of the LIDAR can be shown in the figure shown below.
[Figure of a plot of the LIDAR readings (done in Excel) will be added]
CAN DBC messages sent from the Sensor Controller
The data received from the LIDAR is grouped into sectors and tracks and is sent over the CAN bus. The CAN DBC messages in the DBC file will be as follows
BO_ 3 SENSOR_LIDAR_OBSTACLE_INFO: 6 SENSOR
SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR0 : 0|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR1 : 4|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR2 : 8|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR3 : 12|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR4 : 16|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR5 : 20|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR6 : 24|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR7 : 28|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR8 : 32|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR9 : 36|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR10 : 40|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR11 : 44|4@1+ (1,0) [0|12] "" MASTER,BLE
Android Application
Bluetooth Controller
Hardware Implementation
Bluetooth Module: We are using HC-05 Bluetooth module to send and receive the data from our android application.
Pin Configuration:
The Bridge controller is connected to the bluetooth module through the uart serial interface (Uart3) with 9600 baud rate 8-bit data and 1 stop bit.
Software Implementation
Pseudo code of Bridge controller:
1. Turn on bridge controller.
2. Initialise Bluetooth controller with Uart3 settings.
3. Initialise CAN-BUS with 100 kbps speed.
4. Handle Incoming IO messages it received from the Geo and the Sensor over CAN Bus.
5. Send the received CAN message to the Android over Bluetooth each second.
6. Send the heartbeat message every second to the Master controller.
7. Read Bluetooth message it received from the Android app.
8. Forward the Android message to GEO controller if it received checkpoints otherwise forward it to Master.
Periodic Task implementation:
1 Hz task
1. Send Heartbeat message to Master.
2. Send IO messages which are already received from Geo and Sensor controller to the Android app.
3. Read Data from the Bluetooth module.
100 Hz task
1. Read CAN messages.
DBC messages:
Messages sent from Bluetooth controller :
BO_ 1 BLE_START_STOP_CMD: 1 BLE
SG_ BLE_START_STOP_CMD_start : 0|4@1+ (1,0) [0|1] "" MASTER SG_ BLE_START_STOP_CMD_reset : 4|4@1+ (1,0) [0|1] "" MASTER
BO_ 2 MASTER_SYS_STOP_CMD: 1 MASTER
SG_ MASTER_SYS_STOP_CMD_stop : 0|8@1+ (1,0) [0|1] "" MOTOR
BO_ 38 BLE_HEARTBEAT: 1 BLE
SG_ BLE_HEARTBEAT_signal : 0|8@1+ (1,0) [0|255] "" MASTER
BO_ 212 BLE_GPS_DATA: 8 BLE
SG_ BLE_GPS_long : 0|32@1- (0.000001,0) [0|0] "" GEO SG_ BLE_GPS_lat : 32|32@1- (0.000001,0) [0|0] "" GEO
BO_ 213 BLE_GPS_DATA_CNT: 1 BLE
SG_ BLE_GPS_COUNT : 0|8@1+ (1,0) [0|0] "" GEO,SENSOR
Messages received By Bluetooth controller:
BO_ 214 GEO_CURRENT_COORD: 8 GEO
SG_ GEO_CURRENT_COORD_LONG : 0|32@1- (0.000001,0) [0|0] "" MASTER,BLE SG_ GEO_CURRENT_COORD_LAT : 32|32@1- (0.000001,0) [0|0] "" MASTER,BLE
BO_ 194 MASTER_TELEMETRY: 3 MASTER
SG_ MASTER_TELEMETRY_gps_mia : 0|1@1+ (1,0) [0|1] "" BLE SG_ MASTER_TELEMETRY_sensor_mia : 1|1@1+ (1,0) [0|1] "" BLE SG_ MASTER_TELEMETRY_sensor_heartbeat : 2|1@1+ (1,0) [0|1] "" BLE SG_ MASTER_TELEMETRY_ble_heartbeat : 3|1@1+ (1,0) [0|1] "" BLE SG_ MASTER_TELEMETRY_motor_heartbeat : 4|1@1+ (1,0) [0|1] "" BLE SG_ MASTER_TELEMETRY_geo_heartbeat : 5|1@1+ (1,0) [0|1] "" BLE SG_ MASTER_TELEMETRY_sys_status : 6|2@1+ (1,0) [0|3] "" BLE SG_ MASTER_TELEMETRY_gps_tele_mia : 8|1@1+ (1,0) [0|1] "" BLE
BO_ 195 GEO_TELECOMPASS: 6 GEO
SG_ GEO_TELECOMPASS_compass : 0|12@1+ (0.1,0) [0|360.0] "" MASTER,BLE SG_ GEO_TELECOMPASS_bearing_angle : 12|12@1+ (0.1,0) [0|360.0] "" MASTER,BLE SG_ GEO_TELECOMPASS_distance : 24|12@1+ (0.1,0) [0|0] "" MASTER,BLE SG_ GEO_TELECOMPASS_destination_reached : 36|1@1+ (1,0) [0|1] "" MASTER,BLE SG_ GEO_TELECOMPASS_checkpoint_id : 37|8@1+ (1,0) [0|0] "" MASTER,BLE,SENSOR
BO_ 196 GEO_TELEMETRY_LOCK: 1 GEO
SG_ GEO_TELEMETRY_lock : 0|8@1+ (1,0) [0|0] "" MASTER,SENSOR,BLE
BO_ 3 SENSOR_LIDAR_OBSTACLE_INFO: 6 SENSOR
SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR0 : 0|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR1 : 4|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR2 : 8|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR3 : 12|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR4 : 16|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR5 : 20|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR6 : 24|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR7 : 28|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR8 : 32|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR9 : 36|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR10 : 40|4@1+ (1,0) [0|12] "" MASTER,BLE SG_ SENSOR_LIDAR_OBSTACLE_INFO_SECTOR11 : 44|4@1+ (1,0) [0|12] "" MASTER,BLE
BO_ 4 GEO_TURNING_ANGLE: 2 GEO
SG_ GEO_TURNING_ANGLE_degree : 0|9@1- (1,0) [-180|180] "" MASTER,BLE
Geographical Controller
Design & Implementation
GPS and Compass Module:
GPS:
The Global Positioning System (GPS), originally Navstar GPS, is a space-based radio navigation system owned by the United States government and operated by the United States Air Force. It is a global navigation satellite system that provides geo location and time information to a GPS receiver anywhere on or near the Earth where there is an unobstructed line of sight to four or more GPS satellites.
Compass:
A compass is an instrument used for navigation and orientation that shows direction relative to the geographic cardinal directions (or points). Usually, a diagram called a compass rose shows the directions north, south, east, and west on the compass face as abbreviated initials. When the compass is used, the rose can be aligned with the corresponding geographic directions; for example, the "N" mark on the rose really points northward. Compasses often display markings for angles in degrees in addition to (or sometimes instead of) the rose. North corresponds to 0°, and the angles increase clockwise, so east is 90° degrees, south is 180°, and west is 270°. These numbers allow the compass to show azimuths or bearings, which are commonly stated in this notation.
We are using DJI’s NAZA GPS/COMPASS to get the GPS coordinates and Heading angle. The diagram of the module is as follows:
Message Structure:
GPS:
The 0x10 message contains GPS data. The message structure is as follows:
55 AA 10 3A DT DT DT DT LO LO LO LO LA LA LA LA
The payload is XORed with a mask that changes over time.
Values in the message are stored in little endian.
HEADER
BYTE 1-2: message header - always 55 AA BYTE 3: message id (0x10 for GPS message) BYTE 4: length of the payload (0x3A or 58 decimal for 0x10 message)
PAYLOAD
BYTE 5-8 (DT): date and time BYTE 9-12 (LO): longitude (x10^7, degree decimal) BYTE 13-16 (LA): latitude (x10^7, degree decimal)
CHECKSUM
BYTE 63-64 (CS): checksum
XOR mask
All bytes of the payload except 53rd (NS), 54th, 61st (SN LSB) and 62nd (SN MSB) are XORed with a mask. Mask is calculated based on the value of byte 53rd (NS) and 61st (SN LSB).
Compass:
The 0x20 message contains compass data. The structure of the message is as follows:
55 AA 20 06 CX CX CY CY CZ CZ CS CS
Values in the message are stored in little endian format.
HEADER
BYTE 1-2: message header - always 55 AA BYTE 3 : message id (0x20 for compass message) BYTE 4 : length of the payload (0x06 or 6 decimal for 0x20 message)
PAYLOAD
BYTE 5-6 (CX): compass X axis data (signed) BYTE 7-8 (CY): compass Y axis data (signed) BYTE 9-10 (CZ): compass Z axis data (signed)
XOR
All the bytes of the payload except 9th are XORed with a mask. Mask is calculated based on the value of the 9th byte.
If we index bits from LSB to MSB as 0-7 we have:
mask[0] = 9thByte[0] xor 9thByte[4]
mask[1] = 9thByte[1] xor 9thByte[5]
mask[2] = 9thByte[2] xor 9thByte[6]
mask[3] = 9thByte[3] xor 9thByte[7] xor 9thByte[0];
mask[4] = 9thByte[1];
mask[5] = 9thByte[2];
mask[6] = 9thByte[3];
mask[7] = 9thByte[4] xor 9thByte[0];
Calibration:
Why calibrate the compass?
Ferromagnetic substances placed on multi-rotor or around its working environment affect the reading of earth’s magnetic field for the digital compass. It also reduces the accuracy of the multi-rotor control, or even reads an incorrect heading. Calibration will eliminate such influences, and ensure MC system performs well in a non-ideal magnetic environment.
When to do it?
• The first time you install Naza compass.
• When the multi-rotor mechanical setup has changed.
• If the GPS/Compass module is re-positioned.
• If electronic devices are added/removed/re-positioned.
Pin Configuration:
The Pin Configuration is as follows:
Software Design
Algorithm:
Distance calculation:
We are using the ‘haversine’ formula to calculate the great-circle distance between two points – that is, the shortest distance over the earth’s surface
Haversine formula:
a = sin²(Δφ/2) + cos φ1 ⋅ cos φ2 ⋅ sin²(Δλ/2)
c = 2 ⋅ atan2( √a, √(1−a) )
d = R ⋅ c
where φ is latitude, λ is longitude, R is earth’s radius (mean radius = 6,371km)
Bearing Angle calculation:
The bearing of a point is the number of degrees in the angle measured in a clockwise direction from the north line to the line joining the centre of the compass with the point. A bearing is used to represent the direction of one point relative to another point.
Formula:
θ = atan2( sin Δλ ⋅ cos φ2 , cos φ1 ⋅ sin φ2 − sin φ1 ⋅ cos φ2 ⋅ cos Δλ )
where φ1,λ1 is the start point, φ2,λ2 the end point (Δλ is the difference in longitude)
Since atan2 returns values in the range –π to +π (that is, -180° to +180°), to normalize the result to a compass bearing (in the range 0° to 360°), negative values are transformed into the range 180° to 360°) by converting to degrees first and then using (θ+360) % 360, where % is (floating point) modulo.
DBC Messages:
Implementation
Periodic callback function description:
1 Hz Function:
a. Check CAN bus
b. Send Heartbeat to Master module
c. Send GPS lock to Master and Sensor modules
100 Hz Function:
a. Get GPS coordinates
b. Get Heading angle
c. Get Checkpoints count
d. Calculate Bearing angle
e. Calculate Turning angle
f. Calculate Distance remaining
g. Send Heading angle
h. Send Bearing angle
i. Send Turning angle
j. Send Distance remaining
k. Check Destination reached
Testing & Technical Challenges
Git Project Management
Common Technical Challenges
Project Videos
Conclusion
Project Source Code
References
Acknowledgement
== References Used ==
