Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Add attitude control with althold via mavlink

Browse Source 
Allow ordinary attitude positions without a gps system

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
This commit is contained in:
Patrick José Pereira 2018-04-14 00:15:15 -03:00 committed by Jacob Walser
parent 3701fc0937
commit d3da8f2914
3 changed files with 35 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduSub/GCS_Mavlink.cpp
View File

@ -1006,6 +1006,12 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
mavlink_set_attitude_target_t packet;
mavlink_msg_set_attitude_target_decode(msg, &packet);
// ensure type_mask specifies to use attitude
// the thrust can be used from the altitude hold
if (packet.type_mask & (1<<6)) {
sub.set_attitude_target_no_gps = {AP_HAL::millis(), packet};
}
// ensure type_mask specifies to use attitude and thrust
if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) {
break;

8
ArduSub/Sub.h
View File

@ -436,6 +436,14 @@ private:
AP_Terrain terrain{ahrs, mission, rally};
#endif
// used to allow attitude and depth control without a position system
struct attitude_no_gps_struct {
uint32_t last_message_ms;
mavlink_set_attitude_target_t packet;
};
attitude_no_gps_struct set_attitude_target_no_gps {0};
// Top-level logic
// setup the var_info table
AP_Param param_loader;

22
ArduSub/control_althold.cpp
View File

@ -49,7 +49,27 @@ void Sub::althold_run()
// get pilot desired lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
// Check if set_attitude_target_no_gps is valid
if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
float target_yaw;
Quaternion(
set_attitude_target_no_gps.packet.q
).to_euler(
target_roll,
target_pitch,
target_yaw
);
target_roll = degrees(target_roll);
target_pitch = degrees(target_pitch);
target_yaw = degrees(target_yaw);
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
return;
} else {
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
}
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());