Copter: add all rotating Monocopter heli frame type

ArduCopter/GCS_Mavlink.cpp

@@ -177,13 +177,27 @@ int16_t GCS_MAVLINK_Copter::vfr_hud_throttle() const
  */
 void NOINLINE Copter::send_rpm(mavlink_channel_t chan)
 {
-#if RPM_ENABLED == ENABLED
+#if RPM_ENABLED == ENABLED || FRAME_CONFIG == HELI_FRAME
+    float rpm_1 = 0.0f;
+    float rpm_2 = 0.0f;
+
+    #if RPM_ENABLED == ENABLED
     if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) {
-        mavlink_msg_rpm_send(
-            chan,
-            rpm_sensor.get_rpm(0),
-            rpm_sensor.get_rpm(1));
+        rpm_1 = rpm_sensor.get_rpm(0);
+        rpm_2 = rpm_sensor.get_rpm(1);
+    }
+    #endif
+
+    // for Monocopters
+    if ((AP_Motors::motor_frame_class)copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_MONO) {
+        rpm_2 = rpm_1;
+        rpm_1 = copter.ahrs_view->rpm;
     }
+
+    mavlink_msg_rpm_send(
+    chan,
+    rpm_1,
+    rpm_2);
 #endif
 }
 
@@ -240,6 +254,33 @@ void GCS_MAVLINK_Copter::send_pid_tuning()
     }
 }
 
+void GCS_MAVLINK_Copter::send_attitude() const
+{
+    const AP_AHRS &ahrs = AP::ahrs();
+
+    float r = ahrs.roll;
+    float p = ahrs.pitch;
+    float y = ahrs.yaw;
+
+    // for a all rotating frame we must send the ahrs view roll, pitch and yaw
+    if ((AP_Motors::motor_frame_class)copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_MONO) {
+        r = copter.ahrs_view->roll;
+        p = copter.ahrs_view->pitch;
+        y = copter.ahrs_view->yaw;
+    }
+
+    const Vector3f omega = ahrs.get_gyro();
+    mavlink_msg_attitude_send(
+        chan,
+        AP_HAL::millis(),
+        r,
+        p,
+        y,
+        omega.x,
+        omega.y,
+        omega.z);
+}
+
 uint8_t GCS_MAVLINK_Copter::sysid_my_gcs() const
 {
     return copter.g.sysid_my_gcs;

ArduCopter/GCS_Mavlink.h

@@ -38,6 +38,8 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK
 
     void handle_mount_message(const mavlink_message_t* msg) override;
 
+    void send_attitude() const override;
+
 private:
 
     void handleMessage(mavlink_message_t * msg) override;

ArduCopter/system.cpp

@@ -420,7 +420,8 @@ void Copter::set_default_frame_class()
 {
     if (FRAME_CONFIG == HELI_FRAME &&
         g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_DUAL &&
-        g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD) {
+        g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD &&
+        g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_MONO) {
         g2.frame_class.set(AP_Motors::MOTOR_FRAME_HELI);
     }
 }
@@ -441,6 +442,7 @@ MAV_TYPE Copter::get_frame_mav_type()
         case AP_Motors::MOTOR_FRAME_HELI:
         case AP_Motors::MOTOR_FRAME_HELI_DUAL:
         case AP_Motors::MOTOR_FRAME_HELI_QUAD:
+        case AP_Motors::MOTOR_FRAME_HELI_MONO:
             return MAV_TYPE_HELICOPTER;
         case AP_Motors::MOTOR_FRAME_TRI:
             return MAV_TYPE_TRICOPTER;
@@ -475,6 +477,8 @@ const char* Copter::get_frame_string()
             return "HELI_DUAL";
         case AP_Motors::MOTOR_FRAME_HELI_QUAD:
             return "HELI_QUAD";
+        case AP_Motors::MOTOR_FRAME_HELI_MONO:
+            return "HELI_MONO";
         case AP_Motors::MOTOR_FRAME_TRI:
             return "TRI";
         case AP_Motors::MOTOR_FRAME_SINGLE:
@@ -496,6 +500,20 @@ const char* Copter::get_frame_string()
  */
 void Copter::allocate_motors(void)
 {
+    switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
+        default:
+            ahrs_view = ahrs.create_view(ROTATION_NONE);
+            break;
+#if FRAME_CONFIG == HELI_FRAME
+        case AP_Motors::MOTOR_FRAME_HELI_MONO:
+            ahrs_view = ahrs.create_view(ROTATION_NONE, 0.0f, true);
+            break;
+#endif
+    }
+    if (ahrs_view == nullptr) {
+        AP_HAL::panic("Unable to allocate AP_AHRS_View");
+    }
+
     switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
 #if FRAME_CONFIG != HELI_FRAME
         case AP_Motors::MOTOR_FRAME_QUAD:
@@ -537,7 +555,13 @@ void Copter::allocate_motors(void)
             motors_var_info = AP_MotorsHeli_Quad::var_info;
             AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI);
             break;
-
+
+        case AP_Motors::MOTOR_FRAME_HELI_MONO:
+            motors = new AP_MotorsHeli_Mono(*ahrs_view, copter.scheduler.get_loop_rate_hz());
+            motors_var_info = AP_MotorsHeli_Mono::var_info;
+            AP_Param::set_frame_type_flags(AP_PARAM_FRAME_HELI);
+            break;
+
         case AP_Motors::MOTOR_FRAME_HELI:
         default:
             motors = new AP_MotorsHeli_Single(copter.scheduler.get_loop_rate_hz());
@@ -551,10 +575,6 @@ void Copter::allocate_motors(void)
     }
     AP_Param::load_object_from_eeprom(motors, motors_var_info);
 
-    ahrs_view = ahrs.create_view(ROTATION_NONE);
-    if (ahrs_view == nullptr) {
-        AP_HAL::panic("Unable to allocate AP_AHRS_View");
-    }
 
     const struct AP_Param::GroupInfo *ac_var_info;
 

libraries/AP_AHRS/AP_AHRS.cpp

@@ -375,13 +375,13 @@ void AP_AHRS::update_cd_values(void)
 /*
   create a rotated view of AP_AHRS with optional pitch trim
  */
-AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation, float pitch_trim_deg)
+AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation, float pitch_trim_deg, bool unspin)
 {
     if (_view != nullptr) {
         // can only have one
         return nullptr;
     }
-    _view = new AP_AHRS_View(*this, rotation, pitch_trim_deg);
+    _view = new AP_AHRS_View(*this, rotation, pitch_trim_deg, unspin);
     return _view;
 }
 

libraries/AP_AHRS/AP_AHRS.h

@@ -546,7 +546,7 @@ class AP_AHRS
     }
 
     // create a view
-    AP_AHRS_View *create_view(enum Rotation rotation, float pitch_trim_deg=0);
+    AP_AHRS_View *create_view(enum Rotation rotation, float pitch_trim_deg=0, bool unspin = false);
 
     // return calculated AOA
     float getAOA(void);

libraries/AP_AHRS/AP_AHRS_View.cpp

@@ -21,10 +21,12 @@
 #include "AP_AHRS_View.h"
 #include <stdio.h>
 
-AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_trim_deg) :
+AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_trim_deg, bool unspin) :
     rotation(_rotation),
     ahrs(_ahrs)
 {
+    _unspin = unspin;
+
     switch (rotation) {
     case ROTATION_NONE:
         y_angle = 0;
@@ -64,6 +66,12 @@ void AP_AHRS_View::update(bool skip_ins_update)
 
     rot_body_to_ned.to_euler(&roll, &pitch, &yaw);
 
+    // unspin and recalculate NED rotations
+    if (_unspin) {
+        unspin_update();
+        rot_body_to_ned.from_euler(roll, pitch, yaw);
+    }
+
     roll_sensor  = degrees(roll) * 100;
     pitch_sensor = degrees(pitch) * 100;
     yaw_sensor   = degrees(yaw) * 100;
@@ -77,7 +85,12 @@ void AP_AHRS_View::update(bool skip_ins_update)
 }
 
 // return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
-Vector3f AP_AHRS_View::get_gyro_latest(void) const {
+Vector3f AP_AHRS_View::get_gyro_latest(void) const
+{
+    if (_unspin) {
+        return gyro;
+    }
+
     Vector3f gyro_latest = ahrs.get_gyro_latest();
     gyro_latest.rotate(rotation);
     return gyro_latest;
@@ -96,3 +109,136 @@ Vector2f AP_AHRS_View::rotate_body_to_earth2D(const Vector2f &bf) const
     return Vector2f(bf.x * trig.cos_yaw - bf.y * trig.sin_yaw,
                     bf.x * trig.sin_yaw + bf.y * trig.cos_yaw);
 }
+
+
+// Create 'un-spinned' view for all rotating vehicles such as monocopter
+void AP_AHRS_View::unspin_update(void)
+{
+    // buffer index, increment one for each sample
+    const uint16_t last_end_index = end_index;
+    end_index++;
+    if (end_index > BUFFER_LENGTH - 1) {
+        end_index = 0;
+    }
+
+    // insure buffer does not overlap
+    if (end_index == start_index) {
+        start_index++;
+        if (start_index > BUFFER_LENGTH - 1) {
+            start_index = 0;
+        }
+    }
+
+    // rotate so pitch and roll are relative to fixed 'virtual' yaw
+    const float sin_yaw = sinf(yaw - vitual_forward);
+    const float cos_yaw = cosf(yaw - vitual_forward);
+    virtual_roll[end_index] =  cos_yaw*roll - sin_yaw*pitch;
+    virtual_pitch[end_index] = sin_yaw*roll + cos_yaw*pitch;
+    virtual_roll_rate[end_index] =  cos_yaw*gyro.x - sin_yaw*gyro.y;
+    virtual_pitch_rate[end_index] = sin_yaw*gyro.x + cos_yaw*gyro.y;
+
+    // keep track of rotation angle
+    yaw_diff[end_index] = wrap_PI(yaw - true_yaw);
+    true_yaw = yaw;
+    rotation_angle = wrap_2PI(vitual_forward - true_yaw);
+
+    // keep track of time
+    time[end_index] = AP_HAL::micros64();
+
+    // Integrate virtual roll and pitch - trapezium rule
+    roll_int[end_index] = (virtual_roll[end_index] + virtual_roll[last_end_index]) * 0.5f * (time[end_index] - time[last_end_index]);
+    pitch_int[end_index] = (virtual_pitch[end_index] + virtual_pitch[last_end_index]) * 0.5f * (time[end_index] - time[last_end_index]);
+    roll_rate_int[end_index] = (virtual_roll_rate[end_index] + virtual_roll_rate[last_end_index]) * 0.5f * (time[end_index] - time[last_end_index]);
+    pitch_rate_int[end_index] = (virtual_pitch_rate[end_index] + virtual_pitch_rate[last_end_index]) * 0.5f * (time[end_index] - time[last_end_index]);
+
+    // add up all since last full rotation
+    int16_t i = start_index;
+    float yaw_sum = 0.0f;
+    float roll_int_sum = 0.0f;
+    float pitch_int_sum = 0.0f;
+    float roll_rate_int_sum = 0.0f;
+    float pitch_rate_int_sum = 0.0f;
+    while (i != end_index) {
+        yaw_sum += yaw_diff[i];
+        roll_int_sum += roll_int[i];
+        pitch_int_sum += pitch_int[i];
+        roll_rate_int_sum += roll_rate_int[i];
+        pitch_rate_int_sum += pitch_rate_int[i];
+        i++;
+        if (i > BUFFER_LENGTH - 1) {
+            i = 0;
+        }
+    }
+    yaw_sum += yaw_diff[end_index];
+    roll_int_sum += roll_int[end_index];
+    pitch_int_sum += pitch_int[end_index];
+    roll_rate_int_sum += roll_rate_int[end_index];
+    pitch_rate_int_sum += pitch_rate_int[end_index];
+
+    // take off oldest until less than 360
+    while (abs(yaw_sum - yaw_diff[start_index]) > M_2PI) {
+        yaw_sum -= yaw_diff[start_index];
+        roll_int_sum -= roll_int[start_index];
+        pitch_int_sum -= pitch_int[start_index];
+        roll_rate_int_sum -= roll_rate_int[start_index];
+        pitch_rate_int_sum -= pitch_rate_int[start_index];
+        start_index++;
+        if (start_index > BUFFER_LENGTH - 1) {
+            start_index = 0;
+        }
+    }
+
+    // Find time exactly 1 rotation ago, provided one rotation has past
+    // this allows rolling integration for exactly one rotation
+    const float yaw_sum_less_one = abs(yaw_sum - yaw_diff[start_index]);
+    yaw_sum = abs(yaw_sum);
+    rpm = 0.0f;
+    if (yaw_sum > M_2PI) {
+        // match virtual yaw rate to desired
+        vitual_forward += yaw_rate * (time[end_index] - time[last_end_index]) * 0.000001f;
+        vitual_forward = wrap_2PI(vitual_forward);
+        yaw = vitual_forward;
+        gyro.z = yaw_rate;
+
+        // Calculate Interpolation factors
+        const float interp_0 = (yaw_sum_less_one - M_2PI)/(yaw_sum_less_one - yaw_sum);
+        const float interp_1 = 1 - interp_0;
+
+        // Error caused by misalignment of arrays for values and difference/integrals
+        // i.e. diff array one element shorter than values
+        int16_t start_index_a = start_index - 1;
+        if (start_index_a < 0) {
+            start_index_a = BUFFER_LENGTH - 1;
+        }
+
+        // Interpolation time, roll and pitch exactly one rotation ago
+        const float Cross_time = time[start_index_a] * interp_0 + time[start_index] * interp_1;
+        const float Cross_roll = virtual_roll[start_index_a] * interp_0 + virtual_roll[start_index] * interp_1;
+        const float Cross_pitch = virtual_pitch[start_index_a] * interp_0 + virtual_pitch[start_index] * interp_1;
+        const float Cross_roll_rate = virtual_roll_rate[start_index_a] * interp_0 + virtual_roll_rate[start_index] * interp_1;
+        const float Cross_pitch_rate = virtual_pitch_rate[start_index_a] * interp_0 + virtual_pitch_rate[start_index] * interp_1;
+
+        // Trapezium rule integration from cross point to next point
+        const float Cross_roll_int = (Cross_roll + virtual_roll[start_index]) * 0.5f * (time[start_index] - Cross_time);
+        const float Cross_pitch_int = (Cross_pitch + virtual_pitch[start_index]) * 0.5f * (time[start_index] - Cross_time);
+        const float Cross_roll_rate_int = (Cross_roll_rate + virtual_roll_rate[start_index]) * 0.5f * (time[start_index] - Cross_time);
+        const float Cross_pitch_rate_int = (Cross_pitch_rate + virtual_pitch_rate[start_index]) * 0.5f * (time[start_index] - Cross_time);
+
+        // rotation time
+        const float rotation_time = time[end_index] - Cross_time;
+        rpm = 60000000.0f / rotation_time;
+
+        // Calculate total integrated roll and pitch and divide by rotation time
+        roll = (roll_int_sum - roll_int[start_index] + Cross_roll_int) / rotation_time;
+        pitch = (pitch_int_sum - pitch_int[start_index] + Cross_pitch_int) / rotation_time;
+        gyro.x = (roll_rate_int_sum - roll_rate_int[start_index] + Cross_roll_rate_int) / rotation_time;
+        gyro.y = (pitch_rate_int_sum - pitch_rate_int[start_index] + Cross_pitch_rate_int) / rotation_time;
+
+        roll = wrap_PI(roll);
+        pitch = wrap_PI(pitch);
+
+    } else if (abs(gyro.z) < 0.01f) {
+        // if were not in rotation 'lock' and not rotating then set current yaw as the new 'virtual forward' direction
+        vitual_forward = yaw;
+    }
+}