Skip to content

GitLab

Commit 4eeebb0a authored by Amine AIT-GANI's avatar Amine AIT-GANI
Browse files

Mise a jour des fonctions de test

parent 845e553f
Pipeline #1453 canceled with stage
Hide whitespace changes
Inline Side-by-side
Com_server/test/src/Com_SerialReadingThread.cpp
View file @ 4eeebb0a
......@@ -122,6 +122,191 @@ void Serial_Port_ReadingThread::run()
}
/*MAVLINK_MSG_ID_HEARTBEAT 0
MAVLINK_MSG_ID_SYS_STATUS 1
MAVLINK_MSG_ID_SYSTEM_TIME 2
MAVLINK_MSG_ID_PING 4
MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL 5
MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK 6
MAVLINK_MSG_ID_AUTH_KEY 7
MAVLINK_MSG_ID_SET_MODE 11
MAVLINK_MSG_ID_PARAM_REQUEST_READ 20
MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21
MAVLINK_MSG_ID_PARAM_VALUE 22
MAVLINK_MSG_ID_PARAM_SET 23
MAVLINK_MSG_ID_GPS_RAW_INT 24
MAVLINK_MSG_ID_GPS_STATUS 25
MAVLINK_MSG_ID_SCALED_IMU 26
MAVLINK_MSG_ID_RAW_IMU 27
MAVLINK_MSG_ID_RAW_PRESSURE 28
MAVLINK_MSG_ID_SCALED_PRESSURE 29
MAVLINK_MSG_ID_ATTITUDE 30
MAVLINK_MSG_ID_ATTITUDE_QUATERNION 31
MAVLINK_MSG_ID_LOCAL_POSITION_NED 32
MAVLINK_MSG_ID_GLOBAL_POSITION_INT 33
MAVLINK_MSG_ID_RC_CHANNELS_SCALED 34
MAVLINK_MSG_ID_RC_CHANNELS_RAW 35
MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 36
MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST 37
MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST 38
MAVLINK_MSG_ID_MISSION_ITEM 39
MAVLINK_MSG_ID_MISSION_REQUEST 40
MAVLINK_MSG_ID_MISSION_SET_CURRENT 41
MAVLINK_MSG_ID_MISSION_CURRENT 42
MAVLINK_MSG_ID_MISSION_REQUEST_LIST 43
MAVLINK_MSG_ID_MISSION_COUNT 44
MAVLINK_MSG_ID_MISSION_CLEAR_ALL 45
MAVLINK_MSG_ID_MISSION_ITEM_REACHED 46
MAVLINK_MSG_ID_MISSION_ACK 47
MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN 48
MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN 49
MAVLINK_MSG_ID_PARAM_MAP_RC 50
MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA 54
MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA 55
MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV 61
MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT 62
MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV 63
MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV 64
MAVLINK_MSG_ID_RC_CHANNELS 65
MAVLINK_MSG_ID_REQUEST_DATA_STREAM 66
MAVLINK_MSG_ID_DATA_STREAM 67
MAVLINK_MSG_ID_MANUAL_CONTROL 69
MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70
MAVLINK_MSG_ID_MISSION_ITEM_INT 73
MAVLINK_MSG_ID_VFR_HUD 74
MAVLINK_MSG_ID_COMMAND_INT 75
MAVLINK_MSG_ID_COMMAND_LONG 76
MAVLINK_MSG_ID_COMMAND_ACK 77
MAVLINK_MSG_ID_MANUAL_SETPOINT 81
MAVLINK_MSG_ID_SET_ATTITUDE_TARGET 82
MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED 84
MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED 85
MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT 86
MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT 87
MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET 89
MAVLINK_MSG_ID_HIL_STATE 90
MAVLINK_MSG_ID_HIL_CONTROLS 91
MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW 92
MAVLINK_MSG_ID_OPTICAL_FLOW 100
MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE 101
MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 102
MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 103
MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE 104
MAVLINK_MSG_ID_HIGHRES_IMU 105
MAVLINK_MSG_ID_OPTICAL_FLOW_RAD 106
MAVLINK_MSG_ID_HIL_SENSOR 107
MAVLINK_MSG_ID_SIM_STATE 108
MAVLINK_MSG_ID_RADIO_STATUS 109
MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL 110
MAVLINK_MSG_ID_TIMESYNC 111
MAVLINK_MSG_ID_CAMERA_TRIGGER 112
MAVLINK_MSG_ID_HIL_GPS 113
MAVLINK_MSG_ID_HIL_OPTICAL_FLOW 114
MAVLINK_MSG_ID_HIL_STATE_QUATERNION 115
MAVLINK_MSG_ID_SCALED_IMU2 116
MAVLINK_MSG_ID_LOG_REQUEST_LIST 117
MAVLINK_MSG_ID_LOG_ENTRY 118
MAVLINK_MSG_ID_LOG_REQUEST_DATA 119
MAVLINK_MSG_ID_LOG_DATA 120
MAVLINK_MSG_ID_LOG_ERASE 121
MAVLINK_MSG_ID_LOG_REQUEST_END 122
MAVLINK_MSG_ID_GPS_INJECT_DATA 123
MAVLINK_MSG_ID_GPS2_RAW 124
MAVLINK_MSG_ID_POWER_STATUS 125
MAVLINK_MSG_ID_SERIAL_CONTROL 126
MAVLINK_MSG_ID_GPS_RTK 127
MAVLINK_MSG_ID_GPS2_RTK 128
MAVLINK_MSG_ID_SCALED_IMU3 129
MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 130
MAVLINK_MSG_ID_ENCAPSULATED_DATA 131
MAVLINK_MSG_ID_DISTANCE_SENSOR 132
MAVLINK_MSG_ID_TERRAIN_REQUEST 133
MAVLINK_MSG_ID_TERRAIN_DATA 134
MAVLINK_MSG_ID_TERRAIN_CHECK 135
MAVLINK_MSG_ID_TERRAIN_REPORT 136
MAVLINK_MSG_ID_SCALED_PRESSURE2 137
MAVLINK_MSG_ID_ATT_POS_MOCAP 138
MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET 139
MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET 140
MAVLINK_MSG_ID_ALTITUDE 141
MAVLINK_MSG_ID_RESOURCE_REQUEST 142
MAVLINK_MSG_ID_SCALED_PRESSURE3 143
MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE 146
MAVLINK_MSG_ID_BATTERY_STATUS 147
MAVLINK_MSG_ID_AUTOPILOT_VERSION 148
MAVLINK_MSG_ID_LANDING_TARGET 149
MAVLINK_MSG_ID_SENSOR_OFFSETS 150
MAVLINK_MSG_ID_SET_MAG_OFFSETS 151
MAVLINK_MSG_ID_MEMINFO 152
MAVLINK_MSG_ID_AP_ADC 153
MAVLINK_MSG_ID_DIGICAM_CONFIGURE 154
MAVLINK_MSG_ID_DIGICAM_CONTROL 155
MAVLINK_MSG_ID_MOUNT_CONFIGURE 156
MAVLINK_MSG_ID_MOUNT_CONTROL 157
MAVLINK_MSG_ID_MOUNT_STATUS 158
MAVLINK_MSG_ID_FENCE_POINT 160
MAVLINK_MSG_ID_FENCE_FETCH_POINT 161
MAVLINK_MSG_ID_FENCE_STATUS 162
MAVLINK_MSG_ID_AHRS 163
MAVLINK_MSG_ID_SIMSTATE 164
MAVLINK_MSG_ID_HWSTATUS 165
MAVLINK_MSG_ID_RADIO 166
MAVLINK_MSG_ID_LIMITS_STATUS 167
MAVLINK_MSG_ID_WIND 168
MAVLINK_MSG_ID_DATA16 169
MAVLINK_MSG_ID_DATA32 170
MAVLINK_MSG_ID_DATA64 171
MAVLINK_MSG_ID_DATA96 172
MAVLINK_MSG_ID_RANGEFINDER 173
MAVLINK_MSG_ID_AIRSPEED_AUTOCAL 174
MAVLINK_MSG_ID_RALLY_POINT 175
MAVLINK_MSG_ID_RALLY_FETCH_POINT 176
MAVLINK_MSG_ID_COMPASSMOT_STATUS 177
MAVLINK_MSG_ID_AHRS2 178
MAVLINK_MSG_ID_CAMERA_STATUS 179
MAVLINK_MSG_ID_CAMERA_FEEDBACK 180
MAVLINK_MSG_ID_BATTERY2 181
MAVLINK_MSG_ID_AHRS3 182
MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST 183
MAVLINK_MSG_ID_LED_CONTROL 186
MAVLINK_MSG_ID_MAG_CAL_PROGRESS 191
MAVLINK_MSG_ID_MAG_CAL_REPORT 192
MAVLINK_MSG_ID_EKF_STATUS_REPORT 193
MAVLINK_MSG_ID_PID_TUNING 194
MAVLINK_MSG_ID_GIMBAL_REPORT 200
MAVLINK_MSG_ID_GIMBAL_CONTROL 201
MAVLINK_MSG_ID_GIMBAL_RESET 202
MAVLINK_MSG_ID_GIMBAL_AXIS_CALIBRATION_PROGRESS 203
MAVLINK_MSG_ID_GIMBAL_SET_HOME_OFFSETS 204
MAVLINK_MSG_ID_GIMBAL_HOME_OFFSET_CALIBRATION_RESULT 205
MAVLINK_MSG_ID_GIMBAL_SET_FACTORY_PARAMETERS 206
MAVLINK_MSG_ID_GIMBAL_FACTORY_PARAMETERS_LOADED 207
MAVLINK_MSG_ID_GIMBAL_ERASE_FIRMWARE_AND_CONFIG 208
MAVLINK_MSG_ID_GIMBAL_PERFORM_FACTORY_TESTS 209
MAVLINK_MSG_ID_GIMBAL_REPORT_FACTORY_TESTS_PROGRESS 210
MAVLINK_MSG_ID_GOPRO_POWER_ON 215
MAVLINK_MSG_ID_GOPRO_POWER_OFF 216
MAVLINK_MSG_ID_GOPRO_COMMAND 217
MAVLINK_MSG_ID_GOPRO_RESPONSE 218
MAVLINK_MSG_ID_RPM 226
MAVLINK_MSG_ID_VIBRATION 241
MAVLINK_MSG_ID_HOME_POSITION 242
MAVLINK_MSG_ID_SET_HOME_POSITION 243
MAVLINK_MSG_ID_MESSAGE_INTERVAL 244
MAVLINK_MSG_ID_EXTENDED_SYS_STATE 245
MAVLINK_MSG_ID_ADSB_VEHICLE 246
MAVLINK_MSG_ID_V2_EXTENSION 248
MAVLINK_MSG_ID_MEMORY_VECT 249
MAVLINK_MSG_ID_DEBUG_VECT 250
MAVLINK_MSG_ID_NAMED_VALUE_FLOAT 251
MAVLINK_MSG_ID_NAMED_VALUE_INT 252
MAVLINK_MSG_ID_STATUSTEXT 253
MAVLINK_MSG_ID_DEBUG 254
MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
*/
static const struct { const char *name; uint32_t msgid; } mavlink_message_names[] = MAVLINK_MESSAGE_NAMES;
void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
......@@ -155,6 +340,7 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
case MAVLINK_MSG_ID_HEARTBEAT:
{
//Display_IHM::getInstanceOf().printLog("heartbeat");
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(&message, &drone1.heartbeat);
buffer1 =
"Hearthbeat:"+std::to_string(drone1.heartbeat.type)+
......@@ -163,6 +349,15 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
" "+std::to_string(drone1.heartbeat.custom_mode)+
" "+std::to_string(drone1.heartbeat.system_status)+
" "+std::to_string(drone1.heartbeat.mavlink_version);
hb=drone1.heartbeat;
printf("Heartbeat Message (%d) : ",message.msgid);
printf("Type: %d ", hb.type);
printf("Autopilot type: %d ", hb.autopilot);
printf("System mode: %d ", hb.base_mode);
printf("Custom mode: %d ", hb.custom_mode);
printf("System status: %d ", hb.system_status);
printf("Mavlink version: %d \n", hb.mavlink_version);
cout<<buffer1<<endl;
......@@ -187,8 +382,13 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
//Display_IHM::getInstanceOf().printData(buffer1, 9, 1);
buffer1 = "Voltage: "+std::to_string(drone1.sys_status.voltage_battery);
//Display_IHM::getInstanceOf().printData(buffer1, 10, 1);
float volt = drone1.sys_status.voltage_battery / 1000.0f; // mV
float curr = drone1.sys_status.current_battery / 100.0f; // 10 mA or -1
float rem = drone1.sys_status.battery_remaining / 100.0f; // or -1
cout<<buffer1<<endl;
printf("volt = %f, curr = %f , remaining = %f \n",volt,curr,rem);
break;
}
......@@ -196,6 +396,7 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
mavlink_msg_battery_status_decode(&message, &drone1.battery_status);
printf("MAVLINK_MSG_ID_BATTERY_STATUS \n");
break;
}
......@@ -205,6 +406,11 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
break;
}
case MAVLINK_MSG_ID_VFR_HUD : //74
{
printf("MAVLINK_MSG_ID_VFR_HUD ???\n");
break;
}
//ACCELEROMETERS
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
......@@ -360,10 +566,53 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
{
printf("MAVLINK_MSG_ID_SERVO_OUTPUT_RAW ???\n");
break;
}
case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
{
printf("MAVLINK_MSG_ID_EXTENDED_SYS_STATE \n");
mavlink_msg_extended_sys_state_decode(&message, &drone1.extended_sys_state);
printf("vtol_state %d, landed_state %d \n", drone1.extended_sys_state.vtol_state,drone1.extended_sys_state.landed_state);
break;
}
case MAVLINK_MSG_ID_ALTITUDE:
{
printf("MAVLINK_MSG_ID_ALTITUDE \n");
mavlink_altitude_t* altitude;
mavlink_msg_altitude_decode(&message, &drone1.altitude);
altitude= &drone1.altitude;
printf("altitude_local %f altitude_relative %f altitude_terrain %f bottom_clearance %f\n",altitude->altitude_local , altitude->altitude_relative, altitude->altitude_terrain,altitude->bottom_clearance );
break;
}
case MAVLINK_MSG_ID_VIBRATION:
{
// printf("MAVLINK_MSG_ID_VIBRATION ???\n");
break;
}
case MAVLINK_MSG_ID_PING:
{
printf("MAVLINK_MSG_ID_PING ???\n");
break;
}
case MAVLINK_MSG_ID_COMMAND_LONG:
{
printf("MAVLINK_MSG_ID_COMMAND_LONG \n");
mavlink_msg_command_long_decode(&message, &drone1.command_long);
printf(" %f %f %f %f %f %f %f \n", drone1.command_long.param1, drone1.command_long.param2 , drone1.command_long.param3 , drone1.command_long.param4 , drone1.command_long.param5 , drone1.command_long.param6 , drone1.command_long.param7 );
// command_long->command = mavlink_msg_command_long_get_command(msg);
printf("target_system= %d , target_component= %d , confirm = %d \n", drone1.command_long.target_system , drone1.command_long.target_component , drone1.command_long.confirmation);
break;
}
default:
{
//printf("Warning, did not handle message id %i\n",message.msgid);
printf("=======================\n\n Warning, did not handle message id %i\n======================\n\n",message.msgid);
break;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment