Commit c26f3eb9 authored by Philippe Gaussier's avatar Philippe Gaussier
Browse files

Replace pointers by global and explicit structures for the drone and shared memory.

Suppress empty cpp files to simplify the reading.

PG.
parent 82996603
......@@ -101,7 +101,6 @@ message("\n%% APPLICATION GENERATION")
# %% APP 1
add_executable(
mavlink_server
src/MainApp.cpp
src/Display_MainFrame.cpp
src/Display_Panel.cpp
......@@ -117,10 +116,8 @@ add_executable(
src/Com_Mavlink.cpp
src/Data_Bus.cpp
src/Data_Drone.cpp
src/Abstract_ThreadClass.cpp
# lib/loguru/loguru.cpp
)
# Test trajectory launch
......@@ -129,8 +126,6 @@ add_executable(TestDrone
test/include/PixhawkServer.h
test/src/Data_Drone.cpp
test/include/Data_Drone.h
# test/src/Display_IHM.cpp
# test/include/Display_IHM.h
test/src/Com_SerialReadingThread.cpp
test/include/Com_SerialReadingThread.h
......@@ -139,7 +134,6 @@ add_executable(TestDrone
test/src/Com_Serial.cpp
test/src/Abstract_ThreadClass.cpp
# test/src/Tools_SDL2.cpp
)
......
......@@ -60,9 +60,9 @@ public:
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CONTRUCTOR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Drone();
// Drone();
~Drone();
// ~Drone();
};
......
......@@ -50,7 +50,6 @@ class Ncurses_Panel
{
public :
WINDOW *window;
WINDOW *panel;
int x;
......@@ -63,7 +62,6 @@ public :
~Ncurses_Panel();
void updateSize(int x, int y, int lines, int cols);
};
......
/**
* @author Sylvain Colomer
* @date 18/04/19.
*/
#ifndef PIXHAWK_SERVER_H
#define PIXHAWK_SERVER_H
#include <iostream>
#include <string>
#include <iostream>
#include <unistd.h>
#include <termios.h>
#include <thread>
#include <mutex>
#include <sys/time.h>
// #include <loguru.hpp>
#include "../include/Engine_mainLoop.h"
#endif
......@@ -26,8 +26,8 @@
#include "../include/Data_Bus.h"
class Thread_SerialPort : public Abstract_ThreadClass {
class Thread_SerialPort : public Abstract_ThreadClass
{
private:
// Loop buffer
mavlink_message_t message;
......@@ -62,11 +62,6 @@ private:
/*mavlink_sys_status_t battery_status; // Remaining battery
mavlink_global_vision_position_estimate_t global_vision_position_estimate;*/ // Attitude of the drone
//Channels use to control manually the drone
std::shared_ptr<blc_channel> blc_control_motors; //x, y, z, r
std::shared_ptr<blc_channel> blc_control_commands; // 1=on, 0=off
std::shared_ptr<blc_channel> blc_drone_data;
protected:
/**
......@@ -108,7 +103,6 @@ public:
int testAck(mavlink_command_ack_t *ack, std::string message);
void handle_command_ack(mavlink_command_ack_t *ack);
};
#endif
......@@ -14,7 +14,8 @@ using namespace std;
//%%%%%%%%%%%%%%%%%%%%%%%%% begin/end phase function %%%%%%%%%%%%%%%%%%%%%%%%%%%%
Abstract_ThreadClass::Abstract_ThreadClass(int task_period, int task_deadline){
Abstract_ThreadClass::Abstract_ThreadClass(int task_period, int task_deadline)
{
task_period=task_period;
task_deadline=task_deadline;
......@@ -28,15 +29,15 @@ Abstract_ThreadClass::~Abstract_ThreadClass(){
//%%%%%%%%%%%%%%%%%%%%%%%%% run function %%%%%%%%%%%%%%%%%%%%%%%%%%%%
void Abstract_ThreadClass::run(){
void Abstract_ThreadClass::run()
{
long long currentThreadDelay;
gettimeofday(&begin, 0);
gettimeofday(&front_checkpoint, 0);
while(isRunFlag()){
while(isRunFlag())
{
state = THREAD_STATE_WORK_SLEEP;
usleep(task_period);
......@@ -44,10 +45,11 @@ void Abstract_ThreadClass::run(){
gettimeofday(&end_checkpoint, 0);
currentThreadDelay=(end_checkpoint.tv_sec-front_checkpoint.tv_sec) * 1000000L + (end_checkpoint.tv_usec-front_checkpoint.tv_usec);
if (currentThreadDelay > task_period ){
if (currentThreadDelay > task_period )
{
gettimeofday(&front_checkpoint, 0);
if (currentThreadDelay > task_period + task_deadline){
if (currentThreadDelay > task_period + task_deadline)
{
state = THREAD_STATE_WORK_DEADLINE;
}
else
......@@ -61,11 +63,12 @@ void Abstract_ThreadClass::run(){
//%%%%%%%%%%%%%%%%%%%%%%%%% control function %%%%%%%%%%%%%%%%%%%%%%%%%%%%
void Abstract_ThreadClass::init(){
void Abstract_ThreadClass::init()
{
}
void Abstract_ThreadClass::start(){
void Abstract_ThreadClass::start()
{
cout<< "Start one thread\n";
principalThread= std::thread(&Abstract_ThreadClass::run, this);
......@@ -81,26 +84,28 @@ void Abstract_ThreadClass::stop(){
runFlag_mutex.unlock();
}
void Abstract_ThreadClass::stop_force(){
void Abstract_ThreadClass::stop_force()
{
cout<< "Force Stop one thread\n";
runFlag = false;
}
void Abstract_ThreadClass::play(){
void Abstract_ThreadClass::play()
{
}
void Abstract_ThreadClass::pause(){
void Abstract_ThreadClass::pause()
{
}
void Abstract_ThreadClass::join(){
void Abstract_ThreadClass::join()
{
cout<< "Join one thread\n";
principalThread.join();
}
bool Abstract_ThreadClass::isRunFlag() {
bool Abstract_ThreadClass::isRunFlag()
{
return runFlag;
}
......@@ -9,13 +9,14 @@
#include "../include/Com_Mavlink.h"
MavlinkTools::MavlinkTools(){
MavlinkTools::MavlinkTools()
{
}
// //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PRIMITIVE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
int MavlinkTools::read_message_serial(mavlink_message_t &message, std::shared_ptr<Serial_Port> serial1) {
int MavlinkTools::read_message_serial(mavlink_message_t &message, std::shared_ptr<Serial_Port> serial1)
{
return serial1.get()->read_message(message);
}
......@@ -25,8 +26,6 @@ int MavlinkTools::write_message_serial(mavlink_message_t message, std::shared_pt
// //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CONTROL %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mavlink_message_t MavlinkTools::command_arm(float param1)
{
//Message buffer
......@@ -40,7 +39,6 @@ mavlink_message_t MavlinkTools::command_arm(float param1)
command.confirmation = (uint8_t) 0;
command.param1 = param1;
mavlink_msg_command_long_encode(255, 1, &message, &command);
return message;
......@@ -59,7 +57,6 @@ mavlink_message_t MavlinkTools::command_disarm(float param1)
command.confirmation = (uint8_t) 0;
command.param1 = param1;
mavlink_msg_command_long_encode(255, 1, &message, &command);
return message;
......@@ -150,13 +147,13 @@ mavlink_message_t MavlinkTools::command_pow(float param1)
command.confirmation = (uint8_t) 0;
command.param1 = param1;
mavlink_msg_command_long_encode(255, 1, &message, &command);
return message;
}
//void MockLink::_sendRCChannels(void) {
//void MockLink::_sendRCChannels(void)
//{
// mavlink_message_t msg;
// mavlink_msg_rc_channels_pack_chan(_vehicleSystemId,
......@@ -174,7 +171,8 @@ mavlink_message_t MavlinkTools::command_pow(float param1)
// }
mavlink_command_long_t MavlinkTools::mavlink_newCommand() {
mavlink_command_long_t MavlinkTools::mavlink_newCommand()
{
mavlink_command_long_t com;
com.target_system = 0;
com.target_component = 0.;
......
......@@ -9,21 +9,25 @@
#include "../include/Com_SerialPort.h"
Serial_Port::Serial_Port(std::string uart_name, int baudrate){
Serial_Port::Serial_Port(std::string uart_name, int baudrate)
{
initialize_defaults();
Serial_Port::uart_name = uart_name;
Serial_Port::baudrate = baudrate;
}
Serial_Port::Serial_Port(){
Serial_Port::Serial_Port()
{
initialize_defaults();
}
Serial_Port::~Serial_Port(){
Serial_Port::~Serial_Port()
{
pthread_mutex_destroy(&lock);
}
void Serial_Port::initialize_defaults(){
void Serial_Port::initialize_defaults()
{
// Initialize attributes
debug = false;
fd = -1;
......@@ -45,8 +49,8 @@ void Serial_Port::initialize_defaults(){
/**
* Read from Serial
*/
int Serial_Port::read_message(mavlink_message_t &message){
int Serial_Port::read_message(mavlink_message_t &message)
{
uint8_t cp;
mavlink_status_t status;
uint8_t msgReceived = false;
......@@ -58,13 +62,11 @@ int Serial_Port::read_message(mavlink_message_t &message){
// this function locks the port during read
int result = _read_port(cp);
// --------------------------------------------------------------------------
// PARSE MESSAGE
// --------------------------------------------------------------------------
if (result > 0)
{
msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status);
// check for dropped packets
......@@ -137,8 +139,8 @@ int Serial_Port::write_message(const mavlink_message_t &message){
/**
* Open Serial Port
*/
int Serial_Port::open_serial(){
int Serial_Port::open_serial()
{
fd = _open_port(uart_name.c_str());
// Check success
......@@ -166,15 +168,14 @@ int Serial_Port::open_serial(){
status = true;
return 0;
}
/**
* Close Serial Port
*/
void Serial_Port::close_serial(){
void Serial_Port::close_serial()
{
//Display_IHM::getInstanceOf().printLog("CLOSE PORT\n");
int result = close(fd);
......@@ -190,12 +191,14 @@ void Serial_Port::close_serial(){
/**
* Quit Handler
*/
void Serial_Port::handle_quit(){
try {
void Serial_Port::handle_quit()
{
try
{
close_serial();
}
catch (int error) {
catch (int error)
{
fprintf(stderr,"Warning, could not stop serial port\n");
}
}
......@@ -204,7 +207,8 @@ void Serial_Port::handle_quit(){
/**
* Helper Function - Open Serial Port File Descriptor
*/
int Serial_Port::_open_port(const char* port){
int Serial_Port::_open_port(const char* port)
{
// Open serial port
// O_RDWR - Read and write
// O_NOCTTY - Ignore special chars like CTRL-C
......@@ -374,8 +378,8 @@ bool Serial_Port::_setup_port(int baud)
/**
* Read Port with Lock
*/
int Serial_Port::_read_port(uint8_t &cp){
int Serial_Port::_read_port(uint8_t &cp)
{
// Lock
pthread_mutex_lock(&lock);
......@@ -390,8 +394,8 @@ int Serial_Port::_read_port(uint8_t &cp){
/**
* Write Port with Lock
*/
int Serial_Port::_write_port(char *buf, unsigned len){
int Serial_Port::_write_port(char *buf, unsigned len)
{
// Lock
pthread_mutex_lock(&lock);
......@@ -404,7 +408,6 @@ int Serial_Port::_write_port(char *buf, unsigned len){
// Unlock
pthread_mutex_unlock(&lock);
return bytesWritten;
}
......
......@@ -30,8 +30,8 @@ Wifi_Port::~Wifi_Port()
*
* @return -1 if there is a problem, 0 else
*/
int Wifi_Port::initialize_defaults(int *sock, struct sockaddr_in *locAddr, struct sockaddr_in *targetAddr, int local_port, char *target_ip){
int Wifi_Port::initialize_defaults(int *sock, struct sockaddr_in *locAddr, struct sockaddr_in *targetAddr, int local_port, char *target_ip)
{
struct sockaddr_in possibleTarget; // The possible socket which will received our messages
socklen_t possibleTargetLen = sizeof(possibleTarget);
uint8_t buf[BUFFER_LENGTH];
......@@ -70,11 +70,9 @@ int Wifi_Port::initialize_defaults(int *sock, struct sockaddr_in *locAddr, struc
return -1;
}
/* While we don't find a packet which can indicate sending port we read all messages received on our port */
while (recvfrom(*sock, buf, sizeof(buf), 0, (struct sockaddr*)(&possibleTarget), &possibleTargetLen)<=0 || possibleTarget.sin_addr.s_addr != inet_addr(target_ip))
{
memset(buf,0,256);
time(&currentTime);
timeLeft = difftime(currentTime, startTime);
......@@ -115,7 +113,6 @@ int Wifi_Port::read_message(mavlink_message_t *message)
/* Something received */
if (recsize > 0)
{
printf("Bytes Received : %d\n", (int)recsize); //Size
/* For each part of the tram */
for (int i = 0; i < recsize; ++i)
......
......@@ -22,15 +22,16 @@ DataListener_Keyboard::~DataListener_Keyboard()
}
void DataListener_Keyboard::run(){
void DataListener_Keyboard::run()
{
long long currentThreadDelay;
//bool success=false;
gettimeofday(&begin, 0);
gettimeofday(&front_checkpoint, 0);
while(isRunFlag()){
while(isRunFlag())
{
usleep(task_period);
gettimeofday(&end_checkpoint, 0);
......
......@@ -41,37 +41,38 @@ void Bus::event_newMessage(mavlink_message_t message)
//Refresh last time stamp
drones[message.sysid].last_timestamp = currentTime;
}
void Bus::update(mavlink_heartbeat_t heartbeat)
{
{
// ID
mainFrame_1.get()->setDetail("0 - Id", std::to_string(heartbeat.autopilot) , NCURSES_TEXT_DEFAULT);
mainFrame_1.get()->refresh_details();
/*buffer1 =
"Hearthbeat:"+std::to_string(drone1.get()->heartbeat.type)+
" "+std::to_string(drone1.get()->heartbeat.autopilot)+
" "+std::to_string(drone1.get()->heartbeat.base_mode)+
" "+std::to_string(drone1.get()->heartbeat.custom_mode)+
" "+std::to_string(drone1.get()->heartbeat.system_status)+
" "+std::to_string(drone1.get()->heartbeat.mavlink_version);*/
"Hearthbeat:"+std::to_string(drone1.heartbeat.type)+
" "+std::to_string(drone1.heartbeat.autopilot)+
" "+std::to_string(drone1.heartbeat.base_mode)+
" "+std::to_string(drone1.heartbeat.custom_mode)+
" "+std::to_string(drone1.heartbeat.system_status)+
" "+std::to_string(drone1.heartbeat.mavlink_version);*/
}
void Bus::update(mavlink_sys_status_t battery_status){ // Shows the remaining battery level
if (battery_status.battery_remaining > 20){
void Bus::update(mavlink_sys_status_t battery_status)
{ // Shows the remaining battery level
if (battery_status.battery_remaining > 20)
{
mainFrame_1.get()->setDetail("3 - Battery", std::to_string(battery_status.battery_remaining)+"%" , NCURSES_TEXT_DEFAULT);
}
else if(battery_status.battery_remaining < 0){
else if(battery_status.battery_remaining < 0)
{
mainFrame_1.get()->setDetail("3 - Battery", "Disconnected" , NCURSES_TEXT_RED);
//mainFrame_1.get()->setDetail("Battery", std::to_string(battery_status.battery_remaining)+"%" , NCURSES_TEXT_RED);
}
else{
else
{
mainFrame_1.get()->setDetail("3 - Battery", std::to_string(battery_status.battery_remaining)+"%" , NCURSES_TEXT_ORANGE);
}
mainFrame_1.get()->refresh_details();
......@@ -92,32 +93,40 @@ void Bus::update(mavlink_sys_status_t battery_status){ // Shows the remaini
mainFrame_1.get()->refresh_details();
}*/
void Bus::update(mavlink_flight_information_t flight_information){ // Shows the flight id
void Bus::update(mavlink_flight_information_t flight_information)
{ // Shows the flight id
mainFrame_1.get()->setDetail("4 - Flight time", std::to_string(flight_information.flight_uuid) , NCURSES_TEXT_DEFAULT);
mainFrame_1.get()->refresh_details();
}
void Bus::update(mavlink_altitude_t altitude){ // Shows altitude data
if(altitude.altitude_relative == 0){
void Bus::update(mavlink_altitude_t altitude)
{ // Shows altitude data
if(altitude.altitude_relative == 0)
{
mainFrame_1.get()->setDetail("5 - Altitude", "Unknown" , NCURSES_TEXT_RED);
}
else{
else
{
mainFrame_1.get()->setDetail("5 - Altitude", std::to_string(altitude.altitude_relative)+"m" , NCURSES_TEXT_DEFAULT);
}
mainFrame_1.get()->refresh_details();
}
void Bus::update(mavlink_gps_raw_int_t gps_raw_int){ // Shows if satellite is used
if((gps_raw_int.satellites_visible) > 0){
void Bus::update(mavlink_gps_raw_int_t gps_raw_int)
{ // Shows if satellite is used
if((gps_raw_int.satellites_visible) > 0)
{
mainFrame_1.get()->setDetail("2 - Satellite", std::to_string(gps_raw_int.satellites_visible)+" used" , NCURSES_TEXT_DEFAULT);
}
else{
else
{
mainFrame_1.get()->setDetail("2 - Satellite", "Not used" , NCURSES_TEXT_RED);
}
mainFrame_1.get()->refresh_details();
}
void Bus::update(mavlink_attitude_t attitude){ // Shows attitude of the drone
void Bus::update(mavlink_attitude_t attitude)
{ // Shows attitude of the drone
mainFrame_1.get()->setDetail("7 - Pitch", std::to_string(attitude.pitch*180/PI)+"°" , NCURSES_TEXT_DEFAULT);
mainFrame_1.get()->refresh_details();
mainFrame_1.get()->setDetail("8 - Roll", std::to_string(attitude.roll*180/PI)+"°" , NCURSES_TEXT_DEFAULT);
......@@ -126,12 +135,14 @@ void Bus::update(mavlink_attitude_t attitude){ // Shows attitude of the drone
mainFrame_1.get()->refresh_details();
}
void Bus::update(mavlink_vfr_hud_t vfr_hud){
void Bus::update(mavlink_vfr_hud_t vfr_hud)
{
mainFrame_1.get()->setDetail("6 - Heading", std::to_string(vfr_hud.heading)+"°" , NCURSES_TEXT_DEFAULT);
mainFrame_1.get()->refresh_details();
}
void Bus::update(mavlink_manual_control_t manual_control){
void Bus::update(mavlink_manual_control_t manual_control)
{
mainFrame_1.get()->setDetail("A - X-axis order", std::to_string(manual_control.x) , NCURSES_TEXT_DEFAULT);
mainFrame_1.get()->refresh_details();
mainFrame_1.get()->setDetail("B - Y-axis order", std::to_string(manual_control.y) , NCURSES_TEXT_DEFAULT);
......
/**
* @brief Class that model a drone
*
* @author Sylvain Colomer
* @date 23/04/19
* @version 1.0
*/
#include "../include/Data_Drone.h"
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CONTRUCTOR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/**
* Default constructor. Initialize var to first value
*/
Drone::Drone() {
}
Drone::~Drone() {
}
......@@ -258,14 +258,16 @@ void MainFrame::write_line(std::string text, WINDOW *window, int lines, int cols
wattroff(window, COLOR_PAIR(color));
}
void MainFrame::erase_line(WINDOW *window, int lines){
void MainFrame::erase_line(WINDOW *window, int lines)
{
int lines_max = 0;
int cols_max = 0;
std::string buffer1="";
getmaxyx(window, lines_max, cols_max);