File content as of revision 4:dc844fde64d7:

#include "UAV.h"
#include "Motor_Characteristics.h"
#include "math.h"
using namespace std;

UAV::UAV(uint8_t copter_type,uint8_t motor_type) : motor(motor_type)  // motor and propeller chracteristics
{
switch(copter_type)  {
        case QUAD1:                         // 
            {
            radius_from_center = 0.25;      // assume each arm is equal long
            inv_radius_from_center = 1/radius_from_center;
            JxJy = 0.015f;                   // 1.4 * 2* .25^2 *.054;     // inertia kg m^2
            Jz =   0.02f;
            weight = 1.2f;
            X_or_Plus = PLUS;                //  
            nb_motors = 4;
        break;
            }
        case QUAD2:                         // 
            {
            radius_from_center = 0.2;      // assume each arm is equal long
            inv_radius_from_center = 1/radius_from_center;
            JxJy = 0.013f;                   // 1.4 * 2* .25^2 *.054;     // inertia kg m^2
            Jz =   0.021f;
            weight = 1.082f;
            X_or_Plus = PLUS;                //  
            nb_motors = 4;
            break;
            }
    default: break;
    }   
}


UAV::~UAV() {}

void UAV::calc_copter(void){
    static_thrust_F = this->weight * 9.81f;
    static_thrust_n = motor.F2n(static_thrust_F / nb_motors);
    max_thrust =  this->nb_motors * motor.kF * motor.n_max*motor.n_max;
    max_Mxy = 0.5 * 0.2 * this->radius_from_center * max_thrust;                           // only half the motors can roll or pitch, use 10% of max thrust
    max_Mz = this->nb_motors * 0.2 * motor.kM * motor.n_max*motor.n_max; // all motors drive, 
    dndM = 1 / (2 * motor.kM * static_thrust_n);

}
// *****************************************************************************
// Map Torques and Thrust to motor speeds
// *****************************************************************************
void UAV::motor_mix(float* Mxyz,float FT,float *mspeed){
    FT = limit_minmax(FT,0.0f,max_thrust*0.95f);
//    Mxyz[0] = limit_minmax(Mxyz[0],-max_Mxy,max_Mxy);
//    Mxyz[1] = limit_minmax(Mxyz[1],-max_Mxy,max_Mxy);
//    Mxyz[2] = limit_minmax(Mxyz[2],-max_Mz,max_Mz);
    if(X_or_Plus == PLUS){
         mspeed[0] = motor.F2n(0.25f*FT - 0.5f * Mxyz[0] * inv_radius_from_center);  
         mspeed[0] -= 0.25f * dndM * Mxyz[2];         // Motor 1 CCW --> +Mz
         mspeed[1] = motor.F2n(0.25f*FT + 0.5f * Mxyz[0] * inv_radius_from_center);
         mspeed[1] -= 0.25f * dndM * Mxyz[2];
         mspeed[2] = motor.F2n(0.25f*FT - 0.5f * Mxyz[1] * inv_radius_from_center);
         mspeed[2] += 0.25f * dndM * Mxyz[2];
         mspeed[3] = motor.F2n(0.25f*FT + 0.5f * Mxyz[1] * inv_radius_from_center);
         mspeed[3] += 0.25f * dndM * Mxyz[2];
        }
    if(X_or_Plus == X){
         mspeed[0] = motor.F2n(0.25f*FT +(-0.354f*Mxyz[0] -0.354f*Mxyz[1])* inv_radius_from_center);  
         mspeed[0] -= 0.25f * dndM * Mxyz[2];         // Motor 1 CCW --> +Mz
         mspeed[1] = motor.F2n(0.25f*FT +( 0.354f*Mxyz[0] +0.354f*Mxyz[1])* inv_radius_from_center);  
         mspeed[1] -= 0.25f * dndM * Mxyz[2];
         mspeed[2] = motor.F2n(0.25f*FT +( 0.354f*Mxyz[0] -0.354f*Mxyz[1])* inv_radius_from_center);  
         mspeed[2] += 0.25f * dndM * Mxyz[2];
         mspeed[3] = motor.F2n(0.25f*FT +(-0.354f*Mxyz[0] +0.354f*Mxyz[1])* inv_radius_from_center);  
         mspeed[3] += 0.25f * dndM * Mxyz[2];
        }
   }

// *****************************************************************************
// *****************************************************************************
float UAV::limit_minmax(float val,float mi,float ma){
    if(val < mi)
        val = mi;
    if(val > ma)
        val = ma;
    return val;
}

// *****************************************************************************
// Dead zone, from lo ... hi
float UAV::deadzone(float x,float lo,float hi){
return (x>hi)*(x-hi)+(x<lo)*(x-lo);
}