zjk/AUV_150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

0626提交

Browse Source
This commit is contained in:
zjk
2025-06-26 21:00:14 +08:00
parent 8bad51bc5b
commit dbf35f6ba3
4 changed files with 25 additions and 556 deletions
Download Patch File Download Diff File Expand all files Collapse all files

507
missions/alder/XLOG_SHORESIDE_16_6_2025_____22_24_59/XLOG_SHORESIDE_16_6_2025_____22_24_59.slog
View File

@@ -1,507 +0,0 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LOG FILE: ./XLOG_SHORESIDE_16_6_2025_____22_24_59/XLOG_SHORESIDE_16_6_2025_____22_24_59.slog
%% FILE OPENED ON Thu Jan 1 08:00:00 1970
%% LOGSTART 1750083899.229048
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% (1) TIME
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% TIME
%% TIME
0.753351
%% TIME
%% TIME
0.9541769
1.154654
1.355344
1.555892
1.756309
1.957507
2.159124
2.359773
2.56044
2.761579
2.962356
3.163624
3.364308
3.564977
3.765584
3.966173
4.166899
4.367947
4.56872
4.769442
4.970057
5.170623
5.3714
5.572141
5.772642
5.973307
6.173816
6.374367
6.575039
6.775772
%% TIME
%% TIME
6.976394
7.176887
7.377504
7.578273
7.779018
7.979556
8.180116
8.380661
8.581375
8.782269
8.982922
9.183596
9.384163
9.585023
9.785771
9.986298
10.18705
10.38767
10.58915
10.78978
10.99054
11.19117
11.39183
11.59261
11.79331
11.99397
12.19466
12.39533
12.59607
12.79691
%% TIME
%% TIME
12.99757
13.19839
13.40052
13.60162
13.80216
14.00297
14.20373
14.40436
14.60503
14.80572
15.00716
15.20774
15.40825
15.60892
15.80956
16.01028
16.21093
16.41163
16.61239
16.81314
17.01373
17.21427
17.41495
17.61559
17.81621
18.01677
18.21732
18.41789
18.61861
18.82008
%% TIME
%% TIME
19.02093
19.22154
19.4222
19.62307
19.82388
20.02439
20.22731
20.42817
20.6296
20.83033
21.03077
21.23151
21.43203
21.63292
21.83371
22.03494
22.23552
22.43613
22.63677
22.83751
23.03813
23.23886
23.43951
23.64017
23.84067
24.04121
24.24189
24.44258
24.64337
24.844
%% TIME
%% TIME
25.04463
25.2452
25.44573
25.64626
25.847
26.04751
26.24819
26.44885
26.64966
26.85041
27.05103
27.25183
27.45259
27.65336
27.85387
28.05447
28.25517
28.45659
28.65718
28.85778
29.05839
29.25912
29.45987
29.66105
29.86174
30.06238
30.26313
30.46388
30.66472
30.86538
%% TIME
%% TIME
31.06595
31.26655
31.46723
31.66805
31.86872
32.06911
32.26977
32.47035
32.67114
32.87194
33.0725
33.27309
33.47374
33.67441
33.87498
34.07561
34.27614
34.47669
34.67736
34.87791
35.07867
35.27921
35.4797
35.68037
35.88092
36.0815
36.28229
36.48282
36.68356
36.88418
%% TIME
%% TIME
37.08485
37.28559
37.48616
37.68695
37.88758
38.08825
38.28895
38.48963
38.69037
38.89095
39.09159
39.2921
39.49272
39.69319
39.894
40.09477
40.2954
40.49589
40.69654
40.89731
41.0981
41.29871
41.49917
41.69975
41.90029
42.10097
42.30172
42.50249
42.70328
42.904
%% TIME
%% TIME
43.10473
43.30688
43.50759
43.70835
43.90911
44.10994
44.31045
44.51092
44.71182
44.91255
45.11345
45.31411
45.51459
45.7152
45.9157
46.11637
46.31697
46.51752
46.71834
46.91902
47.11984
47.32065
47.52125
47.7226
47.92329
48.12404
48.32464
48.52522
48.72574
48.92664
%% TIME
%% TIME
49.1275
49.32804
49.52874
49.7295
49.93018
50.13099
50.33162
50.53217
50.73272
50.93353
51.13435
51.33578
51.53632
51.73706
51.93772
52.13849
52.33924
52.53995
52.74131
52.94199
53.14274
53.34349
53.54407
53.74475
53.94528
54.14598
54.34656
54.547
54.74763
54.94835
%% TIME
%% TIME
55.14907
55.34962
55.55006
55.75082
55.95134
56.15204
56.35268
56.55323
56.75382
56.95457
57.15536
57.35608
57.55659
57.75765
57.95828
58.15882
58.35953
58.55999
58.76058
58.96128
59.16228
59.36293
59.56335
59.764
59.9645
60.16511
60.36572
60.56624
60.76684
60.96766
%% TIME
%% TIME
61.16849
61.36899
61.5695
61.77005
61.9706
62.17123
62.3718
62.57225
62.77296
62.97364
63.1745
63.37505
63.5755
63.77626
63.97674
64.17739
64.37793
64.57839
64.77893
64.97951
65.18021
65.3808
65.58128
65.782
65.98256
66.18323
66.38409
66.58549
66.78604
66.98836
%% TIME
%% TIME
67.18904
67.38967
67.59016
67.79091
67.99137
68.19202
68.39277
68.59335
68.794
68.99486
69.19559
69.39615
69.59659
69.79731
69.99776
70.19827
70.39908
70.59954
70.80009
71.00086
71.20138
71.40195
71.60251
71.80303
72.00341
72.20387
72.40451
72.60503
72.8055
73.0064
%% TIME
%% TIME
73.20704
73.40755
73.60801
73.80876
74.00935
74.20996
74.41052
74.61089
74.81154
75.0123
75.21298
75.41346
75.61402
75.81472
76.0154
76.21598
76.4164
76.61682
76.81735
77.01857
77.21921
77.41978
77.62023
77.82092
78.02147
78.22216
78.42275
78.62348
78.82409
79.02489
%% TIME
%% TIME
79.22562
79.42619
79.6267
79.82731
80.02774
80.2284
80.42915
80.63017
80.83089
81.03153
81.23204
81.43256
81.63315
81.83399
82.03449
82.23539
82.43602
82.63671
82.83746
83.03824
83.23917
83.4398
83.64045
83.84125
84.04179
84.24239
84.44314
84.64379
84.84455
85.04534
%% TIME
%% TIME
85.24831
85.54721
85.74793
85.94861
86.14924
86.35003
86.55062
86.7512
86.95184
87.15264
87.35327
87.55388
87.75448
87.95504
88.1557
88.35626
88.55692
88.75752
88.95807
89.159
89.35971
89.56033
89.76098
89.96161
90.16245
90.36322
90.564
90.76487
90.9655
91.16648
%% TIME
%% TIME
91.36718
91.56772
91.76833
91.96926
92.17004
92.37056
92.57115
92.77188
92.9725
93.17327
93.37389
93.57453
93.77508
93.97559
94.17622

4
missions/alder/alder.moos
View File

@@ -16,12 +16,12 @@ ProcessConfig = ANTLER
Run = MOOSDB @ NewConsole = false Run = MOOSDB @ NewConsole = false
//Run = uSimMarineV22 @ NewConsole = false //Run = uSimMarineV22 @ NewConsole = false
Run = pNodeReporter @ NewConsole = false Run = pNodeReporter @ NewConsole = false
Run = pLogger @ NewConsole = false //Run = pLogger @ NewConsole = false
//Run = pMarinePIDV22 @ NewConsole = false //Run = pMarinePIDV22 @ NewConsole = false
Run = pMarineViewer @ NewConsole = false Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false Run = uProcessWatch @ NewConsole = false
Run = pHelmIvP @ NewConsole = false Run = pHelmIvP @ NewConsole = false
Run = pAUV150 @ NewConsole = false Run = pAUV150 @ NewConsole = true
//Run = pMotionControler @ NewConsole = false //Run = pMotionControler @ NewConsole = false
} }

66
src/pAUV150/AUV150.cpp
View File

@@ -13,7 +13,7 @@
#include <string> #include <string>
#include <iostream> #include <iostream>
// #define DEBUG_ #define DEBUG_
using namespace std; using namespace std;
@@ -55,10 +55,6 @@ AUV150::AUV150()
m_overrived = true; //手动控制 m_overrived = true; //手动控制
m_control.initialize(); m_control.initialize();
// Controler::rtP.pid_heading.P = 10.0f;
// Controler::rtP.pid_heading.I = 0.1;
// Controler::rtP.pid_heading.D = 0.1f;
} }
//--------------------------------------------------------- //---------------------------------------------------------
@@ -98,37 +94,15 @@ bool AUV150::OnNewMail(MOOSMSG_LIST &NewMail)
string sval = msg.GetString(); string sval = msg.GetString();
if(sval == "false") if(sval == "false")
{ {
std::cout << "======================" << std::endl;
m_overrived == false; m_overrived == false;
} }
else else
{ {
std::cout << "=============//=========" << std::endl;
m_overrived == true; m_overrived == true;
} }
} }
// else if(key == "DESIRED_RUDDER")
// {
// if(m_overrived)
// {
// m_CommandFrame.rudderUp = msg.GetDouble() + 30.0f;
// m_CommandFrame.rudderDown = msg.GetDouble() + 30.0f;
// m_desiredVarTime = msg.GetTime();
// }
// }
// else if(key == "DESIRED_ELEVATOR")
// {
// if(m_overrived)
// {
// m_CommandFrame.rudderLeft = msg.GetDouble()+ 30.0f;
// m_CommandFrame.rudderRight = msg.GetDouble()+ 30.0f;
// }
// }
// else if(key == "DESIRED_THRUST")
// {
// if(m_overrived)
// {
// m_CommandFrame.mainThruster = msg.GetDouble();
// }
// }
else if(key == "DESIRED_HEADING") else if(key == "DESIRED_HEADING")
{ {
m_control.rtU.heading_cmd = msg.GetDouble(); m_control.rtU.heading_cmd = msg.GetDouble();
@@ -171,7 +145,7 @@ bool AUV150::Iterate()
m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y); m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y);
postStatusUpdate("NAV"); postStatusUpdate("NAV");
#endif #endif
AppCastingMOOSApp::PostReport(); // AppCastingMOOSApp::PostReport();
return(true); return(true);
} }
@@ -241,9 +215,9 @@ bool AUV150::OnStartUp()
void AUV150::registerVariables() void AUV150::registerVariables()
{ {
AppCastingMOOSApp::RegisterVariables(); AppCastingMOOSApp::RegisterVariables();
Register("DESIRED_RUDDER",0); // Register("DESIRED_RUDDER",0);
Register("DESIRED_THRUST",0); // Register("DESIRED_THRUST",0);
Register("DESIRED_ELEVATOR",0); // Register("DESIRED_ELEVATOR",0);
Register("DESIRED_HEADING", 0); Register("DESIRED_HEADING", 0);
Register("DESIRED_SPEED", 0); Register("DESIRED_SPEED", 0);
Register("DESIRED_DEPTH", 0); Register("DESIRED_DEPTH", 0);
@@ -290,7 +264,7 @@ bool AUV150::ListenLoop()
{ {
if((m_TcpSocket!=NULL) && m_bConnected) if((m_TcpSocket!=NULL) && m_bConnected)
{ {
int count = m_TcpSocket->iRecieveMessage(buffer, sizeof(FeedbackFrame_150AUV)); int count = m_TcpSocket->iReadMessageWithTimeOut(buffer, sizeof(FeedbackFrame_150AUV),3);
if(count > 0) { if(count > 0) {
std::cout << "Received " << count << " bytes" << std::endl; std::cout << "Received " << count << " bytes" << std::endl;
FeedbackFrame_150AUV *p = reinterpret_cast<FeedbackFrame_150AUV *>(buffer); FeedbackFrame_150AUV *p = reinterpret_cast<FeedbackFrame_150AUV *>(buffer);
@@ -478,25 +452,18 @@ bool AUV150::buildReport()
}; };
//=================DEGUB========================== //=================DEGUB==========================
m_msgs << "MOOS_MANUAL_OVERIDE : " << m_overrived << std::endl; m_msgs << "MOOS_MANUAL_OVERIDE : " << m_overrived << std::endl;
m_msgs << "DirectUpperRudderServoAngleCmd : " << m_control.rtY.DirectUpperRudderServoAngleCmd << std::endl; m_msgs << "DirectUpperRudderServoAngleCmd : " << m_control.rtY.DirectUpperRudderServoAngleCmd << std::endl;
m_msgs << "DirectLowerRudderServoAngleCmd : " << m_control.rtY.DirectLowerRudderServoAngleCmd << std::endl; m_msgs << "DirectLowerRudderServoAngleCmd : " << m_control.rtY.DirectLowerRudderServoAngleCmd << std::endl;
m_msgs << "DirectLeftRudderServoAngleCmd : " << m_control.rtY.DirectLeftRudderServoAngleCmd << std::endl; m_msgs << "DirectLeftRudderServoAngleCmd : " << m_control.rtY.DirectLeftRudderServoAngleCmd << std::endl;
m_msgs << "DirectRightRudderServoAngleCmd : " << m_control.rtY.DirectRightRudderServoAngleCmd << std::endl; m_msgs << "DirectRightRudderServoAngleCmd : " << m_control.rtY.DirectRightRudderServoAngleCmd << std::endl;
m_msgs << "mainThruster : " << m_control.rtY.MainThrusterSpeedCmd << std::endl; m_msgs << "mainThruster : " << m_control.rtY.MainThrusterSpeedCmd << std::endl;
// m_CommandFrame.rudderUp = m_control.rtY.DirectUpperRudderServoAngleCmd + 35.0f;
// m_CommandFrame.rudderDown = m_control.rtY.DirectLowerRudderServoAngleCmd + 35.0f;
// m_CommandFrame.rudderLeft = m_control.rtY.DirectLeftRudderServoAngleCmd + 35.0f;
// m_CommandFrame.rudderRight = m_control.rtY.DirectRightRudderServoAngleCmd + 35.0f;
// m_CommandFrame.mainThruster = m_control.rtY.MainThrusterSpeedCmd;
// =============== Navigation Info =============== // =============== Navigation Info ===============
printTable( printTable(
{"Connect", "Read", "Writ" ,"Conter" ,"Server ip","Latitude", "Longitude","Gap"}, {"Connect", "Fre","Cont" ,"ip","OLat", "OLon","Gap"},
{ {
(isConnected() ? "Yes" : "No"), (isConnected() ? "Yes" : "No"),
doubleToString(getRealReadFreq(), 4) + " Hz", doubleToString(getRealReadFreq(), 4) + " Hz",
doubleToString(getRealWriteFreq(), 4) + " Hz",
ulintToString(getCounter()), ulintToString(getCounter()),
m_serverHost, m_serverHost,
doubleToString(m_OriginLatitude, 5) + "deg", doubleToString(m_OriginLatitude, 5) + "deg",
@@ -527,7 +494,7 @@ bool AUV150::buildReport()
// =============== Velocity Info =============== // =============== Velocity Info ===============
printTable( printTable(
{"Latitude", "Longitude", "Altitude","East Velocity","North Velocity","Down Velocity"}, {"Lat", "Lon", "Alt","East Vel","North Vel","Down Vel"},
{ {
doubleToString(m_status.insLatitude, 4) + "deg", doubleToString(m_status.insLatitude, 4) + "deg",
doubleToString(m_status.insLongitude, 4) + "deg", doubleToString(m_status.insLongitude, 4) + "deg",
@@ -539,7 +506,7 @@ bool AUV150::buildReport()
); );
m_msgs << "----------------DVL-----------------\n"; m_msgs << "----------------DVL-----------------\n";
printTable( printTable(
{"DVL X Velocity", "DVL Y Velocity", "DVL Z Velocity"}, {"DVL X Vel", "DVL Y Vel", "DVL Z Vel"},
{ {
doubleToString(m_status.dvlVelX, 2) + "m/s", doubleToString(m_status.dvlVelX, 2) + "m/s",
doubleToString(m_status.dvlVelY, 2) + "m/s", doubleToString(m_status.dvlVelY, 2) + "m/s",
@@ -575,7 +542,7 @@ bool AUV150::buildReport()
// =============== System Status =============== // =============== System Status ===============
printTable( printTable(
{"Light", "Leak", "Power" , "Emergency power" , "Payload", "DVL Status","Thruster Status"}, {"Light", "Leak", "Power" , "Emergency power" , "PZ", "DVL","Thruster Status"},
{ {
m_status.ledSwitch ? "ON" : "OFF", m_status.ledSwitch ? "ON" : "OFF",
m_status.leakStatus ? "LEAK" : "NORMAL", m_status.leakStatus ? "LEAK" : "NORMAL",
@@ -623,7 +590,7 @@ bool AUV150::updateStatus(FeedbackFrame_150AUV &feedbackFrame)
m_status.dvlStatus = feedbackFrame.dvlStatus; m_status.dvlStatus = feedbackFrame.dvlStatus;
m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y); m_geodesy.LatLong2LocalGrid(m_status.insLatitude, m_status.insLongitude, m_pos_x, m_pos_y);
// 以下为转换的数据
m_status.x = m_pos_x; m_status.x = m_pos_x;
m_status.y = m_pos_y; m_status.y = m_pos_y;
m_status.z = m_status.depthSensor; m_status.z = m_status.depthSensor;
@@ -667,8 +634,13 @@ void AUV150::postStatusUpdate(std::string prefix)
Notify(prefix+"_ALTITUDE", m_status.insAltitude, m_curr_time); Notify(prefix+"_ALTITUDE", m_status.insAltitude, m_curr_time);
} }
// 发布误差信息 // 发布AUV信息
prefix = "AUV150";
Notify(prefix+"_HEADING_ERROR", m_status.heading_error, m_curr_time); Notify(prefix+"_HEADING_ERROR", m_status.heading_error, m_curr_time);
Notify(prefix+"_PITCH_ERROR", m_status.pitch_error, m_curr_time); Notify(prefix+"_PITCH_ERROR", m_status.pitch_error, m_curr_time);
Notify(prefix+"_DEPTH_ERROR", m_status.depth_error, m_curr_time); Notify(prefix+"_DEPTH_ERROR", m_status.depth_error, m_curr_time);
Notify(prefix+"_DESIRED_RUDDER",m_status.desired_rudder);
Notify(prefix+"_DESIRED_EVEVATOR",m_status.desired_elevator);
Notify(prefix+"_DESIRED_THRUST",m_status.desired_thrust);
} }

4
src/pAUV150/AUV150.h
View File

@@ -120,6 +120,10 @@ struct AUV150_Status
double heading_error; double heading_error;
double pitch_error; double pitch_error;
double depth_error; double depth_error;
double desired_rudder;
double desired_elevator;
double desired_thrust;
}; };
class AUV150 : public AppCastingMOOSApp class AUV150 : public AppCastingMOOSApp