From 1ffbffa0e7be811cd84de546e5d73da3d1b99322 Mon Sep 17 00:00:00 2001
From: Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
Date: Wed, 2 Sep 2015 18:03:20 -0700
Subject: [PATCH] AP_Math: check if failed to allocate memory

---
 libraries/AP_Math/matrix_alg.cpp | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/libraries/AP_Math/matrix_alg.cpp b/libraries/AP_Math/matrix_alg.cpp
index c9c57f84a5..1b44b9a733 100644
--- a/libraries/AP_Math/matrix_alg.cpp
+++ b/libraries/AP_Math/matrix_alg.cpp
@@ -16,7 +16,14 @@ float detnxn(const float C[],const uint8_t n)
 {
   float f;
   float *A = new float[n*n];
+  if( A == NULL) {
+      return 0;
+  }
   int8_t *ipiv = new int8_t[n];
+  if(ipiv == NULL) {
+      delete[] A;
+      return 0;
+  }
   int32_t i0;
   int32_t j;
   int32_t c;
@@ -28,6 +35,7 @@ float detnxn(const float C[],const uint8_t n)
   int32_t b_j;
   int32_t ijA;
   bool isodd;
+
   memcpy(&A[0], &C[0], n*n * sizeof(float));
   for (i0 = 0; i0 < n; i0++) {
     ipiv[i0] = (int8_t)(1 + i0);
@@ -119,8 +127,15 @@ bool inversenxn(const float x[], float y[], const uint8_t n)
     }
 
     float *A = new float[n*n];
+    if( A == NULL ){
+        return false;
+    }
     int32_t i0;
     int32_t *ipiv = new int32_t[n];
+    if(ipiv == NULL) {
+        delete[] A;
+        return false;
+    }
     int32_t j;
     int32_t c;
     int32_t pipk;
@@ -131,6 +146,12 @@ bool inversenxn(const float x[], float y[], const uint8_t n)
     int32_t jy;
     int32_t ijA;
     int32_t *p = new int32_t[n];
+    if(p == NULL) {
+        delete[] A;
+        delete[] ipiv;
+        return false;
+    }
+    
     for (i0 = 0; i0 < n*n; i0++) {
         A[i0] = x[i0];
         y[i0] = 0.0f;