- switch PITCH & ROLL in MSP_SET_ACC_TRIM MSP_ACC_TRIM messages (http…

…://www.multiwii.com/forum/viewtopic.php?f=8&t=6061&p=61216#p61087)

git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii_shared@1741 02679b44-d973-9852-f2fa-63770883b36c

Protocol.cpp

@@ -149,20 +149,24 @@ static void serializeNames(PGM_P s) {
   tailSerialReply();
 }
 
+static void __attribute__ ((noinline)) s_struct_w(uint8_t *cb,uint8_t siz) {
+  while(siz--) *cb++ = read8();
+}
+
+static void s_struct_partial(uint8_t *cb,uint8_t siz) {
+  while(siz--) serialize8(*cb++);
+}
+
 static void s_struct(uint8_t *cb,uint8_t siz) {
   headSerialReply(siz);
-  while(siz--) serialize8(*cb++);
+  s_struct_partial(cb,siz);
   tailSerialReply();
 }
 
 static void mspAck() {
   headSerialReply(0);tailSerialReply();
 }
 
-static void __attribute__ ((noinline)) s_struct_w(uint8_t *cb,uint8_t siz) {
-  while(siz--) *cb++ = read8();
-}
-
 enum MSP_protocol_bytes {
   IDLE,
   HEADER_START,
@@ -472,11 +476,15 @@ void evaluateCommand(uint8_t c) {
       s_struct((uint8_t*)&motor,16);
       break;
     case MSP_ACC_TRIM:
-      s_struct((uint8_t*)&conf.angleTrim[0],4);
+      headSerialReply(4);
+      s_struct_partial((uint8_t*)&conf.angleTrim[PITCH],2);
+      s_struct_partial((uint8_t*)&conf.angleTrim[ROLL],2);
+      tailSerialReply();
       break;
     case MSP_SET_ACC_TRIM:
       mspAck();
-      s_struct_w((uint8_t*)&conf.angleTrim[0],4);
+      s_struct_w((uint8_t*)&conf.angleTrim[PITCH],2);
+      s_struct_w((uint8_t*)&conf.angleTrim[ROLL],2);
       break;
     case MSP_RC:
       s_struct((uint8_t*)&rcData,RC_CHANS*2);
@@ -730,7 +738,7 @@ void evaluateCommand(uint8_t c) {
 void evaluateOtherData(uint8_t sr) {
   #ifndef SUPPRESS_OTHER_SERIAL_COMMANDS
     #if GPS
-	  #if !defined(I2C_GPS)
+      #if !defined(I2C_GPS)
         // on the GPS port, we must avoid interpreting incoming values for other commands because there is no
         // protocol protection as is with MSP commands
         // doing so with single chars would be prone to error.