IPython notebook v4: Paparazzi RotorcaftMixing example

rotorcraft_mixing.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# The first two lines of the A matrix represent the coordinates of each rotor in the X,Y plane,\n",
    "# and the third line the direction in which they spin.\n",
    "# Note that in this example the X axis is vertical, and the Y coordinates are in the top row of A.\n",
    "A = np.array([[-0.17,  0.17, -0.25,  0.25, -0.33,  0.33],\n",
    "             [-0.35, -0.35,  0.,    0.,    0.35,  0.35],\n",
    "             [-0.1,   0.1,   0.1,  -0.1,  -0.1,   0.1 ]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Moore-Penrose pseudoinverse of A\n",
    "B = np.linalg.pinv(A)\n",
    "# normalize roll/pitch to the largest of both\n",
    "# normalize yaw to 0.5\n",
    "# and transpose\n",
    "rp_max = B[:,0:1].max()\n",
    "n = np.array([rp_max, rp_max, 2*B[:,2].max()])\n",
    "B_nt = (B / n).T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ -67   67 -256  256 -189  189]\n",
      " [-197 -197    0    0  197  197]\n",
      " [ -77   77  128 -128  -57   57]]\n"
     ]
    }
   ],
   "source": [
    "# scale and round to 256 to return final coefficients\n",
    "scale = 256\n",
    "coeffs = np.around(scale * B_nt).astype(int)\n",
    "print(coeffs)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "<define name=\"ROLL_COEF\"  value=\"{ -67,   67, -256,  256, -189,  189}\"/>\n",
      "<define name=\"PITCH_COEF\" value=\"{-197, -197,    0,    0,  197,  197}\"/>\n",
      "<define name=\"YAW_COEF\"   value=\"{ -77,   77,  128, -128,  -57,   57}\"/>\n"
     ]
    }
   ],
   "source": [
    "# output defines\n",
    "import string\n",
    "rows = ['ROLL_COEF\"  ', 'PITCH_COEF\" ', 'YAW_COEF\"   ']\n",
    "for i, r in enumerate(rows):\n",
    "    print('<define name=\"' + r + 'value=\"{' + string.join(['{:>4d}'.format(c) for c in coeffs[i]], ', ') + '}\"/>')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}