Skip to content

Commit 856755b

Browse files
gabryelreyesgabryelreyes
committed
Refactored Leader application
1 parent 9e9e1ce commit 856755b

12 files changed

+1170
-772
lines changed

‎lib/ConvoyLeader/src/App.cpp

+202-117
Large diffs are not rendered by default.

‎lib/ConvoyLeader/src/App.h

+58-44
Original file line numberDiff line numberDiff line change
@@ -46,10 +46,10 @@
4646
#include <Arduino.h>
4747
#include <Board.h>
4848
#include <MqttClient.h>
49-
#include <SerialMuxProtServer.hpp>
50-
#include "SerialMuxChannels.h"
51-
#include <PlatoonController.h>
49+
#include <SimpleTimer.hpp>
50+
#include <StateMachine.h>
5251
#include <V2VClient.h>
52+
#include "SerialMuxChannels.h"
5353

5454
/******************************************************************************
5555
* Macros
@@ -67,11 +67,12 @@ class App
6767
* Construct the convoy leader application.
6868
*/
6969
App() :
70+
m_serialMuxProtChannelIdRemoteCtrl(0U),
71+
m_serialMuxProtChannelIdMotorSpeeds(0U),
7072
m_smpServer(Board::getInstance().getDevice().getStream(), this),
71-
m_serialMuxProtChannelIdMotorSpeedSetpoints(0U),
7273
m_mqttClient(),
73-
m_platoonController(),
74-
m_v2vClient(m_mqttClient)
74+
m_v2vClient(m_mqttClient),
75+
m_sendWaypointTimer()
7576
{
7677
}
7778

@@ -93,62 +94,48 @@ class App
9394
void loop();
9495

9596
/**
96-
* Callback for the current vehicle data.
97+
* Set latest vehicle data from RU.
9798
*
98-
* @param[in] vehicleData Current vehicle data.
99+
* @param[in] waypoint Latest vehicle data from RU.
99100
*/
100-
void currentVehicleChannelCallback(const VehicleData& vehicleData);
101+
void setLatestVehicleData(const Waypoint& waypoint);
101102

102103
/**
103-
* Input waypoint callback.
104-
* Called in order to get the next waypoint into the platoon controller.
105-
*
106-
* @param[out] waypoint Next waypoint.
107-
*
108-
* @return If a waypoint is available, it returns true. Otherwise, false.
109-
*/
110-
bool inputWaypointCallback(Waypoint& waypoint);
111-
112-
/**
113-
* Output waypoint callback.
114-
* Called in order to send the last waypoint to the next platoon participant.
115-
*
116-
* @param[in] waypoint Last waypoint.
117-
*
118-
* @return If the waypoint was sent successfully, returns true. Otherwise, false.
104+
* Set error state.
119105
*/
120-
bool outputWaypointCallback(const Waypoint& waypoint);
121-
122-
/**
123-
* Motor setpoint callback.
124-
* Called in order to send the motor speeds using SerialMuxProt to the robot.
125-
*
126-
* @param[in] left Left motor speed [steps/s].
127-
* @param[in] right Right motor speed [steps/s].
128-
*
129-
* @return If the motor speeds were sent successfully, returns true. Otherwise, false.
130-
*/
131-
bool motorSetpointCallback(const int16_t left, const int16_t right);
106+
void setErrorState();
132107

133108
private:
134109
/** Minimum battery level in percent. */
135110
static const uint8_t MIN_BATTERY_LEVEL = 10U;
136111

112+
/** Send waypoint timer interval in ms. */
113+
static const uint32_t SEND_WAYPOINT_TIMER_INTERVAL = 500U;
114+
115+
/** Send commands timer interval in ms. */
116+
static const uint32_t SEND_COMMANDS_TIMER_INTERVAL = 100U;
117+
137118
/** MQTT topic name for birth messages. */
138119
static const char* TOPIC_NAME_BIRTH;
139120

140121
/** MQTT topic name for will messages. */
141122
static const char* TOPIC_NAME_WILL;
142123

143-
/** SerialMuxProt Channel id sending sending motor speed setpoints. */
144-
uint8_t m_serialMuxProtChannelIdMotorSpeedSetpoints;
124+
/** MQTT topic name for release messages. */
125+
static const char* TOPIC_NAME_RELEASE;
126+
127+
/** SerialMuxProt Channel id for sending remote control commands. */
128+
uint8_t m_serialMuxProtChannelIdRemoteCtrl;
129+
130+
/** SerialMuxProt Channel id for sending motor speeds. */
131+
uint8_t m_serialMuxProtChannelIdMotorSpeeds;
145132

146133
/**
147134
* SerialMuxProt Server Instance
148135
*
149136
* @tparam tMaxChannels set to MAX_CHANNELS, defined in SerialMuxChannels.h.
150137
*/
151-
SerialMuxProtServer<MAX_CHANNELS> m_smpServer;
138+
SMPServer m_smpServer;
152139

153140
/**
154141
* MQTTClient Instance
@@ -160,10 +147,23 @@ class App
160147
*/
161148
V2VClient m_v2vClient;
162149

150+
/** The system state machine. */
151+
StateMachine m_systemStateMachine;
152+
153+
/**
154+
* Latest vehicle data from RU.
155+
*/
156+
Waypoint m_latestVehicleData;
157+
158+
/**
159+
* Send waypoint timer.
160+
*/
161+
SimpleTimer m_sendWaypointTimer;
162+
163163
/**
164-
* Platoon controller instance.
164+
* Timer for sending initial commands.
165165
*/
166-
PlatoonController m_platoonController;
166+
SimpleTimer m_commandTimer;
167167

168168
private:
169169
/**
@@ -172,9 +172,23 @@ class App
172172
void fatalErrorHandler();
173173

174174
/**
175-
* Send speed setpoints using SerialMuxProt.
175+
* Setup the MQTT client.
176+
*
177+
* @return If successful returns true, otherwise false.
178+
*/
179+
bool setupMqttClient();
180+
181+
/**
182+
* Setup the SerialMuxProt channels.
183+
*
184+
* @return If successful returns true, otherwise false.
185+
*/
186+
bool setupSerialMuxProt();
187+
188+
/**
189+
* Process periodic tasks.
176190
*/
177-
void sendSpeedSetpoints();
191+
void processPeriodicTasks();
178192

179193
private:
180194
/* Not allowed. */

‎lib/ConvoyLeader/src/LongitudinalController.cpp ‎lib/ConvoyLeader/src/DrivingState.cpp

+127-106
Original file line numberDiff line numberDiff line change
@@ -1,106 +1,127 @@
1-
/* MIT License
2-
*
3-
* Copyright (c) 2023 - 2024 Andreas Merkle <web@blue-andi.de>
4-
*
5-
* Permission is hereby granted, free of charge, to any person obtaining a copy
6-
* of this software and associated documentation files (the "Software"), to deal
7-
* in the Software without restriction, including without limitation the rights
8-
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9-
* copies of the Software, and to permit persons to whom the Software is
10-
* furnished to do so, subject to the following conditions:
11-
*
12-
* The above copyright notice and this permission notice shall be included in all
13-
* copies or substantial portions of the Software.
14-
*
15-
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16-
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17-
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18-
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19-
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20-
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21-
* SOFTWARE.
22-
*/
23-
24-
/*******************************************************************************
25-
DESCRIPTION
26-
*******************************************************************************/
27-
/**
28-
* @brief Concrete Longitudinal Controller.
29-
* @author Gabryel Reyes <gabryelrdiaz@gmail.com>
30-
*/
31-
32-
/******************************************************************************
33-
* Includes
34-
*****************************************************************************/
35-
36-
#include "LongitudinalController.h"
37-
#include "PlatoonUtils.h"
38-
#include <Arduino.h>
39-
40-
/******************************************************************************
41-
* Compiler Switches
42-
*****************************************************************************/
43-
44-
/******************************************************************************
45-
* Macros
46-
*****************************************************************************/
47-
48-
/******************************************************************************
49-
* Types and classes
50-
*****************************************************************************/
51-
52-
/******************************************************************************
53-
* Prototypes
54-
*****************************************************************************/
55-
56-
/******************************************************************************
57-
* Local Variables
58-
*****************************************************************************/
59-
60-
/******************************************************************************
61-
* Public Methods
62-
*****************************************************************************/
63-
64-
LongitudinalController::LongitudinalController() : ILongitudinalController()
65-
{
66-
}
67-
68-
LongitudinalController::~LongitudinalController()
69-
{
70-
}
71-
72-
bool LongitudinalController::calculateLongitudinalMovement(const Waypoint& currentWaypoint,
73-
const Waypoint& targetWaypoint, int16_t& centerSpeedSetpoint)
74-
{
75-
bool isSuccessful = true;
76-
int32_t distance = PlatoonUtils::calculateAbsoluteDistance(targetWaypoint, currentWaypoint);
77-
78-
if (0 == distance)
79-
{
80-
/* Target reached. */
81-
centerSpeedSetpoint = 0;
82-
}
83-
else
84-
{
85-
/* Calculate speed using ramp factor. */
86-
centerSpeedSetpoint = constrain(((distance * RAMP_FACTOR) + OFFSET_SPEED), -MAX_MOTOR_SPEED, MAX_MOTOR_SPEED);
87-
}
88-
89-
return isSuccessful;
90-
}
91-
92-
/******************************************************************************
93-
* Protected Methods
94-
*****************************************************************************/
95-
96-
/******************************************************************************
97-
* Private Methods
98-
*****************************************************************************/
99-
100-
/******************************************************************************
101-
* External Functions
102-
*****************************************************************************/
103-
104-
/******************************************************************************
105-
* Local Functions
106-
*****************************************************************************/
1+
/* MIT License
2+
*
3+
* Copyright (c) 2023 - 2024 Andreas Merkle <web@blue-andi.de>
4+
*
5+
* Permission is hereby granted, free of charge, to any person obtaining a copy
6+
* of this software and associated documentation files (the "Software"), to deal
7+
* in the Software without restriction, including without limitation the rights
8+
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9+
* copies of the Software, and to permit persons to whom the Software is
10+
* furnished to do so, subject to the following conditions:
11+
*
12+
* The above copyright notice and this permission notice shall be included in all
13+
* copies or substantial portions of the Software.
14+
*
15+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16+
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17+
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18+
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19+
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20+
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21+
* SOFTWARE.
22+
*/
23+
24+
/*******************************************************************************
25+
DESCRIPTION
26+
*******************************************************************************/
27+
/**
28+
* @brief Driving state.
29+
* @author Gabryel Reyes <gabryelrdiaz@gmail.com>
30+
*/
31+
32+
/******************************************************************************
33+
* Includes
34+
*****************************************************************************/
35+
36+
#include "DrivingState.h"
37+
38+
/******************************************************************************
39+
* Compiler Switches
40+
*****************************************************************************/
41+
42+
/******************************************************************************
43+
* Macros
44+
*****************************************************************************/
45+
46+
/******************************************************************************
47+
* Types and classes
48+
*****************************************************************************/
49+
50+
/******************************************************************************
51+
* Prototypes
52+
*****************************************************************************/
53+
54+
/******************************************************************************
55+
* Local Variables
56+
*****************************************************************************/
57+
58+
/******************************************************************************
59+
* Public Methods
60+
*****************************************************************************/
61+
62+
void DrivingState::entry()
63+
{
64+
m_isActive = true;
65+
}
66+
67+
void DrivingState::process(StateMachine& sm)
68+
{
69+
/* Check if the state is active. */
70+
if (false == m_isActive)
71+
{
72+
m_topMotorSpeed = 0;
73+
}
74+
else
75+
{
76+
/* Check limits. */
77+
78+
/* Check follower feedback. Calculate platoon length and react accordingly */
79+
80+
/* Calculate top motor speed. */
81+
m_topMotorSpeed = m_maxMotorSpeed;
82+
}
83+
}
84+
85+
void DrivingState::exit()
86+
{
87+
m_isActive = false;
88+
}
89+
90+
void DrivingState::setMaxMotorSpeed(int16_t maxSpeed)
91+
{
92+
m_maxMotorSpeed = maxSpeed;
93+
}
94+
95+
bool DrivingState::getTopMotorSpeed(int16_t& topMotorSpeed) const
96+
{
97+
topMotorSpeed = m_topMotorSpeed;
98+
99+
/* Only valid if the state is active. */
100+
return m_isActive;
101+
}
102+
103+
void DrivingState::setVehicleData(const VehicleData& vehicleData)
104+
{
105+
m_vehicleData = vehicleData;
106+
}
107+
108+
void DrivingState::setLastFollowerFeedback(const VehicleData& feedback)
109+
{
110+
m_followerFeedback = feedback;
111+
}
112+
113+
/******************************************************************************
114+
* Protected Methods
115+
*****************************************************************************/
116+
117+
/******************************************************************************
118+
* Private Methods
119+
*****************************************************************************/
120+
121+
/******************************************************************************
122+
* External Functions
123+
*****************************************************************************/
124+
125+
/******************************************************************************
126+
* Local Functions
127+
*****************************************************************************/

0 commit comments

Comments
 (0)