-
Notifications
You must be signed in to change notification settings - Fork 4.6k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Support for ArduCopterSolo vehicle type (based on MavLinkMultirotorApi)
See also https://github.com/3drobotics/ardupilot-solo Along the way: - Let MultiRotorParamsFactory issue Solo IDs (for independent connections to ArduPilot instances) - Rename the now-shared 'PX4VehicleSetting' to 'MavLinkVehicleSetting' - Promote some MavLinkMultirotorApi members and methods from private to protected
- Loading branch information
Kerry Moffitt
committed
Sep 10, 2018
1 parent
84aafe6
commit 5a83b38
Showing
13 changed files
with
486 additions
and
97 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
221 changes: 221 additions & 0 deletions
221
AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,221 @@ | ||
| #ifndef msr_airlib_ArduCopterSoloApi_h | ||
| #define msr_airlib_ArduCopterSoloApi_h | ||
|
|
||
| #include "AdHocConnection.hpp" | ||
| #include "vehicles/multirotor/MultiRotor.hpp" | ||
| #include "vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp" | ||
|
|
||
| namespace msr { namespace airlib { | ||
|
|
||
|
|
||
| class ArduCopterSoloApi : public MavLinkMultirotorApi | ||
| { | ||
| public: | ||
| virtual ~ArduCopterSoloApi() | ||
| { | ||
| closeAllConnection(); | ||
| } | ||
|
|
||
| virtual void update() | ||
| { | ||
| if (sensors_ == nullptr) | ||
| return; | ||
|
|
||
| // send GPS and other sensor updates | ||
| const auto gps = getGps(); | ||
| if (gps != nullptr) { | ||
| const auto& gps_output = gps->getOutput(); | ||
| const auto& imu_output = getImu()->getOutput(); | ||
| // const auto& mag_output = getMagnetometer()->getOutput(); | ||
| // const auto& baro_output = getBarometer()->getOutput(); | ||
|
|
||
| SensorMessage packet; | ||
| packet.timestamp = Utils::getTimeSinceEpochNanos() / 1000; | ||
| packet.latitude = gps_output.gnss.geo_point.latitude; | ||
| packet.longitude = gps_output.gnss.geo_point.longitude; | ||
| packet.altitude = gps_output.gnss.geo_point.altitude; | ||
|
|
||
| common_utils::Utils::log("Current LLA: " + gps_output.gnss.geo_point.to_string(), common_utils::Utils::kLogLevelInfo); | ||
|
|
||
| packet.speedN = gps_output.gnss.velocity[0]; | ||
| packet.speedE = gps_output.gnss.velocity[1]; | ||
| packet.speedD = gps_output.gnss.velocity[2]; | ||
|
|
||
| packet.xAccel = imu_output.linear_acceleration[0]; | ||
| packet.yAccel = imu_output.linear_acceleration[1]; | ||
| packet.zAccel = imu_output.linear_acceleration[2]; | ||
|
|
||
| float yaw; | ||
| float pitch; | ||
| float roll; | ||
| VectorMath::toEulerianAngle(imu_output.orientation, pitch, roll, yaw); | ||
| packet.yawDeg = yaw * 180.0 / M_PI; | ||
| packet.pitchDeg = pitch * 180.0 / M_PI; | ||
| packet.rollDeg = roll * 180.0 / M_PI; | ||
|
|
||
| Vector3r bodyRPY(roll, pitch, yaw); | ||
|
|
||
| // In the Unreal world, yaw is rotation around Z, so this seems to be RPY, like PySim | ||
| Vector3r bodyVelocityRPY(imu_output.angular_velocity[0], imu_output.angular_velocity[1], imu_output.angular_velocity[2]); | ||
| Vector3r earthRPY = bodyAnglesToEarthAngles(bodyRPY, bodyVelocityRPY); | ||
|
|
||
| packet.rollRate = earthRPY[0] * 180.0 / M_PI; | ||
| packet.pitchRate = earthRPY[1] * 180.0 / M_PI; | ||
| packet.yawRate = earthRPY[2] * 180.0 / M_PI; | ||
|
|
||
| // Heading appears to be unused by AruPilot. But use yaw for now | ||
| packet.heading = yaw; | ||
|
|
||
| packet.airspeed = std::sqrt( | ||
| packet.speedN * packet.speedN | ||
| + packet.speedE * packet.speedE | ||
| + packet.speedD * packet.speedD); | ||
|
|
||
| packet.magic = 0x4c56414f; | ||
|
|
||
| if (udpSocket_ != nullptr) | ||
| { | ||
| std::vector<uint8_t> msg(sizeof(packet)); | ||
| memcpy(msg.data(), &packet, sizeof(packet)); | ||
| udpSocket_->sendMessage(msg); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| virtual void close() | ||
| { | ||
| MavLinkMultirotorApi::close(); | ||
|
|
||
| if (udpSocket_ != nullptr) { | ||
| udpSocket_->close(); | ||
| udpSocket_->unsubscribe(rotorSubscriptionId_); | ||
| udpSocket_ = nullptr; | ||
| } | ||
| } | ||
|
|
||
| protected: | ||
| virtual void connect() | ||
| { | ||
| if (!is_simulation_mode_) { | ||
|
|
||
| MavLinkMultirotorApi::connect(); | ||
| } | ||
| else { | ||
| const std::string& ip = connection_info_.ip_address; | ||
| int port = connection_info_.ip_port; | ||
|
|
||
| close(); | ||
|
|
||
| if (ip == "") { | ||
| throw std::invalid_argument("UdpIp setting is invalid."); | ||
| } | ||
|
|
||
| if (port == 0) { | ||
| throw std::invalid_argument("UdpPort setting has an invalid value."); | ||
| } | ||
|
|
||
| Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); | ||
| Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.sitl_ip_port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo); | ||
|
|
||
| udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.sitl_ip_port); | ||
| mavlinkcom::AdHocMessageHandler handler = [this](std::shared_ptr<mavlinkcom::AdHocConnection> connection, const std::vector<uint8_t> &msg) { | ||
| this->rotorPowerMessageHandler(connection, msg); | ||
| }; | ||
|
|
||
| rotorSubscriptionId_ = udpSocket_->subscribe(handler); | ||
| } | ||
| } | ||
|
|
||
| private: | ||
| #ifdef __linux__ | ||
| struct __attribute__((__packed__)) SensorMessage { | ||
| #else | ||
| #pragma pack(push,1) | ||
| struct SensorMessage { | ||
| #endif | ||
| // this is the packet sent by the simulator | ||
| // to the APM executable to update the simulator state | ||
| // All values are little-endian | ||
| uint64_t timestamp; | ||
| double latitude, longitude; // degrees | ||
| double altitude; // MSL | ||
| double heading; // degrees | ||
| double speedN, speedE, speedD; // m/s | ||
| double xAccel, yAccel, zAccel; // m/s/s in body frame | ||
| double rollRate, pitchRate, yawRate; // degrees/s/s in earth frame | ||
| double rollDeg, pitchDeg, yawDeg; // euler angles, degrees | ||
| double airspeed; // m/s | ||
| uint32_t magic; // 0x4c56414f | ||
| }; | ||
| #ifndef __linux__ | ||
| #pragma pack(pop) | ||
| #endif | ||
|
|
||
| static const int kArduCopterRotorControlCount = 11; | ||
|
|
||
| struct RotorControlMessage { | ||
| // ArduPilot Solo rotor control datagram format | ||
| uint16_t pwm[kArduCopterRotorControlCount]; | ||
| uint16_t speed, direction, turbulance; | ||
| }; | ||
|
|
||
| std::shared_ptr<mavlinkcom::AdHocConnection> udpSocket_; | ||
| int rotorSubscriptionId_; | ||
|
|
||
| virtual void normalizeRotorControls() | ||
| { | ||
| // change 1000-2000 to 0-1. | ||
| for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) { | ||
| rotor_controls_[i] = (rotor_controls_[i] - 1000.0f) / 1000.0f; | ||
| } | ||
| } | ||
|
|
||
| void rotorPowerMessageHandler(std::shared_ptr<mavlinkcom::AdHocConnection> connection, const std::vector<uint8_t> &msg) | ||
| { | ||
| if (msg.size() != sizeof(RotorControlMessage)) | ||
| { | ||
| Utils::log("Got rotor control message of size " + std::to_string(msg.size()) + " when we were expecting size " + std::to_string(sizeof(RotorControlMessage)), Utils::kLogLevelError); | ||
| return; | ||
| } | ||
|
|
||
| RotorControlMessage rotorControlMessage; | ||
| memcpy(&rotorControlMessage, msg.data(), sizeof(RotorControlMessage)); | ||
|
|
||
| std::lock_guard<std::mutex> guard_actuator(hil_controls_mutex_); //use same mutex as HIL_CONTROl | ||
|
|
||
| for (auto i = 0; i < RotorControlsCount && i < kArduCopterRotorControlCount; ++i) { | ||
| rotor_controls_[i] = rotorControlMessage.pwm[i]; | ||
| } | ||
|
|
||
| normalizeRotorControls(); | ||
| } | ||
|
|
||
| Vector3r bodyAnglesToEarthAngles(Vector3r bodyRPY, Vector3r bodyVelocityRPY) | ||
| { | ||
| float p = bodyVelocityRPY[0]; | ||
| float q = bodyVelocityRPY[1]; | ||
| float r = bodyVelocityRPY[2]; | ||
|
|
||
| // Roll, pitch, yaw | ||
| float phi = bodyRPY[0]; | ||
| float theta = bodyRPY[1]; | ||
|
|
||
| float phiDot = p + tan(theta)*(q*sin(phi) + r * cos(phi)); | ||
|
|
||
| float thetaDot = q * cos(phi) - r * sin(phi); | ||
| if (fabs(cos(theta)) < 1.0e-20) | ||
| { | ||
| theta += 1.0e-10f; | ||
| } | ||
|
|
||
| float psiDot = (q*sin(phi) + r * cos(phi)) / cos(theta); | ||
|
|
||
| return Vector3r(phiDot, thetaDot, psiDot); | ||
| } | ||
|
|
||
| }; | ||
|
|
||
| } | ||
| } //namespace | ||
|
|
||
| #endif |
Oops, something went wrong.