From 65d36c44afdacc1ed4762fe63c3e0f8a4a6f1317 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 12 Mar 2013 12:12:23 -0700 Subject: [PATCH 01/75] Prevent flips at high throttle Conflicts: src/drivers/ardrone_interface/ardrone_motor_control.c --- .../ardrone_interface/ardrone_motor_control.c | 84 ++++++++----------- 1 file changed, 36 insertions(+), 48 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index ecd31a073d..d3b414150c 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -369,11 +370,9 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float yaw_control = actuators->control[2]; float motor_thrust = actuators->control[3]; - //printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust); - const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ - const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */ + const float scaling = 510.0f; /**< 100% thrust equals a value of 510 which works, 512 leads to cutoff */ const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ @@ -382,71 +381,56 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float motor_calc[4] = {0}; float output_band = 0.0f; - float band_factor = 0.75f; const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ - float yaw_factor = 1.0f; static bool initialized = false; /* publish effective outputs */ static struct actuator_controls_effective_s actuator_controls_effective; static orb_advert_t actuator_controls_effective_pub; - if (motor_thrust <= min_thrust) { - motor_thrust = min_thrust; - output_band = 0.0f; - } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { - output_band = band_factor * (motor_thrust - min_thrust); - } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { - output_band = band_factor * startpoint_full_control; - } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { - output_band = band_factor * (max_thrust - motor_thrust); + /* linearly scale the control inputs from 0 to startpoint_full_control */ + if (motor_thrust < startpoint_full_control) { + output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1 + } else { + output_band = 1.0f; } + roll_control *= output_band; + pitch_control *= output_band; + yaw_control *= output_band; + + //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer // FRONT (MOTOR 1) motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); - // RIGHT (MOTOR 2) motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); - // BACK (MOTOR 3) motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); - // LEFT (MOTOR 4) motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); - // if we are not in the output band - if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band - && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band - && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band - && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { + /* if one motor is saturated, reduce throttle */ + float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust; + + + if (saturation > 0.0f) { + + /* reduce the motor thrust according to the saturation */ + motor_thrust = motor_thrust - saturation; - yaw_factor = 0.5f; - yaw_control *= yaw_factor; // FRONT (MOTOR 1) motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); - // RIGHT (MOTOR 2) motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); - // BACK (MOTOR 3) motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); - // LEFT (MOTOR 4) motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); } - for (int i = 0; i < 4; i++) { - //check for limits - if (motor_calc[i] < motor_thrust - output_band) { - motor_calc[i] = motor_thrust - output_band; - } - if (motor_calc[i] > motor_thrust + output_band) { - motor_calc[i] = motor_thrust + output_band; - } - } /* publish effective outputs */ actuator_controls_effective.control_effective[0] = roll_control; @@ -467,25 +451,29 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls /* set the motor values */ - /* scale up from 0..1 to 10..512) */ + /* scale up from 0..1 to 10..500) */ motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas); motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas); motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas); motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas); - /* Keep motors spinning while armed and prevent overflows */ + /* scale up from 0..1 to 10..500) */ + motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas)); - /* Failsafe logic - should never be necessary */ - motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10; - motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10; - motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10; - motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10; + /* Failsafe logic for min values - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas; + motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : min_gas; + motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : min_gas; + motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : min_gas; - /* Failsafe logic - should never be necessary */ - motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511; - motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511; - motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511; - motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511; + /* Failsafe logic for max values - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] <= max_gas) ? motor_pwm[0] : max_gas; + motor_pwm[1] = (motor_pwm[1] <= max_gas) ? motor_pwm[1] : max_gas; + motor_pwm[2] = (motor_pwm[2] <= max_gas) ? motor_pwm[2] : max_gas; + motor_pwm[3] = (motor_pwm[3] <= max_gas) ? motor_pwm[3] : max_gas; /* send motors via UART */ ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); From 43eca3eb0e0267f1eb314fc2f5ee6d9eaef93d80 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 24 Jun 2013 12:39:33 +1000 Subject: [PATCH 02/75] build: use unqualified com port names on windows the qualified names were not working with current versions of python --- makefiles/upload.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 4b01b447d0..a0671f6adc 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -18,7 +18,7 @@ ifeq ($(SYSTYPE),Linux) SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" endif ifeq ($(SERIAL_PORTS),) -SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" +SERIAL_PORTS = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" endif .PHONY: all upload-$(METHOD)-$(BOARD) From 75a6e095676a7951c7077f757b7a10ea1399729f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 13:20:34 +0200 Subject: [PATCH 03/75] Ignore files for ctags (used with SublimeText3) --- .gitignore | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 3fdc1bf2a2..76f45281ce 100644 --- a/.gitignore +++ b/.gitignore @@ -27,4 +27,6 @@ mavlink/include/mavlink/v0.9/ /NuttX /Documentation/doxy.log /Documentation/html/ -/Documentation/doxygen*objdb*tmp \ No newline at end of file +/Documentation/doxygen*objdb*tmp +.tags +.tags_sorted_by_file \ No newline at end of file From 1fed9ef1b1ec2d0bf7c2cba7d60be77e37faaf40 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 23 Aug 2013 13:37:58 +0200 Subject: [PATCH 04/75] Make px_mkfw.py and px_upload.py compatible with both python 2.7 and 3.3 --- Tools/px_mkfw.py | 8 ++++---- Tools/px_uploader.py | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index faef9ca335..b598a65a1a 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -1,7 +1,7 @@ #!/usr/bin/env python ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -60,7 +60,7 @@ def mkdesc(): proto['description'] = "" proto['git_identity'] = "" proto['build_time'] = 0 - proto['image'] = base64.b64encode(bytearray()) + proto['image'] = bytes() proto['image_size'] = 0 return proto @@ -99,12 +99,12 @@ if args.description != None: if args.git_identity != None: cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--dirty"]) p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout - desc['git_identity'] = p.read().strip() + desc['git_identity'] = str(p.read().strip()) p.close() if args.image != None: f = open(args.image, "rb") bytes = f.read() desc['image_size'] = len(bytes) - desc['image'] = base64.b64encode(zlib.compress(bytes,9)) + desc['image'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') print(json.dumps(desc, indent=4)) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d2ebf16987..a5277ed21a 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -1,7 +1,7 @@ #!/usr/bin/env python ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -102,7 +102,7 @@ class firmware(object): 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d]) - crcpad = bytearray('\xff\xff\xff\xff') + crcpad = bytearray(b'\xff\xff\xff\xff') def __init__(self, path): From 2075cba636eff6bb59aeefe0eee7eaa13276b16e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Aug 2013 22:13:49 +0200 Subject: [PATCH 05/75] Added H frame to docs --- Documentation/control_flow.graffle | 1962 ++++++++++++++++++++++++++-- 1 file changed, 1873 insertions(+), 89 deletions(-) diff --git a/Documentation/control_flow.graffle b/Documentation/control_flow.graffle index 2535231108..6188c44308 100644 --- a/Documentation/control_flow.graffle +++ b/Documentation/control_flow.graffle @@ -5,7 +5,7 @@ ApplicationVersion com.omnigroup.OmniGraffle - 139.16.0.171715 + 139.18.0.187838 CreationDate 2013-02-22 17:51:02 +0000 @@ -26,9 +26,9 @@ MasterSheets ModificationDate - 2013-04-20 15:38:47 +0000 + 2013-08-25 05:58:40 +0000 Modifier - Lorenz Meier + Lorenz NotesVisible NO OriginVisible @@ -55,7 +55,7 @@ NSPaperName string - iso-a3 + BADB3137-012D-43BB-AFDE-2C0069A874AD NSPaperSize @@ -123,7 +123,7 @@ Bounds - {{320.41170773935767, 646.55758982070449}, {100, 24}} + {{320.41170773792788, 646.55758667263535}, {100, 24}} Class ShapedGraphic FitText @@ -171,7 +171,7 @@ Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -193,8 +193,8 @@ 45 Points - {283.47949897905681, 658.66217570036315} - {459.99996984645378, 658.44980851233595} + {283.47949897630235, 658.66217426345122} + {459.99996984638915, 658.4498036008282} Style @@ -243,7 +243,7 @@ Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -289,7 +289,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural @@ -340,7 +340,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural @@ -389,7 +389,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -437,7 +437,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -461,8 +461,8 @@ 38 Points - {584.52245687751122, 125.49999965650382} - {584.56069426576869, 157.00000090441219} + {584.50076858662646, 125.50000000000021} + {584.50207726115275, 157.00000000001324} Style @@ -511,7 +511,7 @@ Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -532,8 +532,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 34 Points - {210.47275259888471, 495.87499981747118} - {210.43193555768113, 543.06250166479344} + {210.49312053369275, 495.87499999999562} + {210.48281498396668, 543.06249999958038} Style @@ -582,7 +582,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -602,8 +602,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 32 Points - {210.46968123779743, 580.06249992316259} - {210.44442316539605, 627.25000030102035} + {210.47945786724097, 580.0625} + {210.47913680605853, 627.24999999999989} Style @@ -637,8 +637,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 30 Points - {210.48620562018775, 690.25} - {210.49612530146715, 737.4375} + {210.48620103078346, 690.25} + {210.49611383706232, 737.4375} Style @@ -694,7 +694,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -731,7 +731,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -789,7 +789,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -801,7 +801,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Bounds - {{152.49994329565658, 397.00120984204113}, {116, 24}} + {{152.49998995675264, 397.00120984204113}, {116, 24}} Class ShapedGraphic FitText @@ -849,7 +849,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -877,8 +877,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} -1 Points - {584.49994992834695, 220} - {584.4997453697481, 348.6875000000008} + {584.49999370649277, 220} + {584.49999370649277, 348.6875} Style @@ -971,7 +971,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -991,8 +991,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 17 Points - {210.49997491180866, 385.6875} - {210.49991091996927, 432.875} + {210.49998995675264, 385.6875} + {210.49998995675264, 432.875} Style @@ -1029,7 +1029,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Points {283.49999999999511, 270.00001907176534} - {339.50000000000028, 270.00001907176534} + {339.49999999999994, 270.00001907176534} Style @@ -1098,8 +1098,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 13 Points - {210.49171354592596, 207} - {210.49498558851923, 238.4999999999998} + {210.49209551674531, 207} + {210.49601794208507, 238.5} Style @@ -1133,8 +1133,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 12 Points - {210.49498558851246, 138.5000000000002} - {210.48997117702493, 170.00000000001342} + {210.49601794208485, 138.5} + {210.49203588416967, 170} Style @@ -1190,7 +1190,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1234,7 +1234,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1278,7 +1278,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1322,7 +1322,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1360,7 +1360,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1395,7 +1395,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1430,7 +1430,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1675,7 +1675,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1774,7 +1774,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2145,7 +2145,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2536,7 +2536,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2927,7 +2927,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3361,7 +3361,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3404,7 +3404,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3791,7 +3791,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4178,7 +4178,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4565,7 +4565,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4963,7 +4963,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5062,7 +5062,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5421,7 +5421,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5780,7 +5780,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -6175,7 +6175,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -6660,7 +6660,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7019,7 +7019,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7406,7 +7406,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7793,7 +7793,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8300,7 +8300,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8404,7 +8404,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8795,7 +8795,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9166,7 +9166,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9537,7 +9537,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9928,7 +9928,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10367,7 +10367,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10779,7 +10779,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10883,7 +10883,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11270,7 +11270,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11629,7 +11629,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11988,7 +11988,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -12375,7 +12375,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -12810,7 +12810,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -13257,6 +13257,1697 @@ PX4IO or PX4FMU} 1 0/72 in = 1.0000 in GraphicsList + + Class + Group + Graphics + + + Bounds + {{281.4621559838219, 891.87422762618871}, {48, 14}} + Class + ShapedGraphic + FitText + YES + Flow + Resize + ID + 1534 + Shape + Rectangle + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + Pad + 0 + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\b\fs24 \cf0 QUAD +\b0 H} + VerticalPad + 0 + + Wrap + NO + + + Class + Group + Graphics + + + Bounds + {{282.10284531231378, 729.4992324185539}, {50.666667938232422, 36}} + Class + ShapedGraphic + ID + 1536 + Rotation + 270 + Shape + HorizontalTriangle + Style + + fill + + Color + + b + 0.13195 + g + 0.165669 + r + 1 + + + + Text + + VerticalPad + 0 + + + + Bounds + {{271.43617794923421, 713.47324050519865}, {72, 72}} + Class + ShapedGraphic + ID + 1537 + Shape + Rectangle + Style + + stroke + + CornerRadius + 9 + + + + + ID + 1535 + Rotation + 315 + + + Class + Group + Graphics + + + Bounds + {{387.95096684220994, 833.99511602139876}, {24, 24}} + Class + ShapedGraphic + ID + 1539 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 4} + VerticalPad + 0 + + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1541 + Points + + {447.56635253821332, 822.0678021003348} + {445.35664384700522, 823.21685061976291} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{359.66328626413321, 875.75030562593793}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1542 + Rotation + 45 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + + + Bounds + {{349.2634215034181, 795.53620935597417}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1543 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{346.5134215034181, 792.78620935597417}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1544 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1540 + Rotation + 315 + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1546 + Points + + {352.51941963025371, 870.09214125245364} + {354.7291283214617, 868.94309273302531} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{424.42248590433371, 797.90963772685075}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1547 + Rotation + 225 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + + + Bounds + {{349.32235066504887, 795.12373399681428}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1548 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{346.57235066504887, 792.37373399681428}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1549 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1545 + Rotation + 315 + + + Bounds + {{349.03928365032027, 795.08342417943641}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1550 + Rotation + 315 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.234512 + g + 0.673128 + r + 0.148947 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + + + ID + 1538 + Rotation + 315 + + + Class + Group + Graphics + + + Bounds + {{202.98738428569231, 833.92908910567212}, {24, 24}} + Class + ShapedGraphic + ID + 1552 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 2} + VerticalPad + 0 + + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1554 + Points + + {167.72554658547227, 821.64822718882488} + {169.93525527668044, 822.79727570825287} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{239.62861285955245, 875.33073071442777}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1555 + Rotation + 135 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{164.52847762026795, 795.1166344444639}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1556 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{161.77847762026795, 792.3666344444639}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1557 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1553 + Rotation + 225 + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1559 + Points + + {262.41893329590295, 870.02611253847215} + {260.20922460469507, 868.87706401904404} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{174.51586702182317, 797.84360901286959}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1560 + Rotation + 315 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{164.11600226110755, 795.05770528283301}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1561 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{161.36600226110755, 792.30770528283301}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1562 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1558 + Rotation + 225 + VFlip + YES + + + Bounds + {{164.07569244372991, 795.01739580669891}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1563 + Rotation + 225 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.673128 + g + 0.534129 + r + 0.217952 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + ID + 1551 + Rotation + 315 + + + Class + Group + Graphics + + + Bounds + {{424.02444829866442, 666.20122319568361}, {24, 24}} + Class + ShapedGraphic + ID + 1565 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 1} + VerticalPad + 0 + + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1567 + Points + + {388.76261059844421, 653.92036127883637} + {390.97231928965209, 655.06940979826459} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{460.66567687252439, 707.60286480443915}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1568 + Rotation + 135 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{385.56554163323983, 627.3887685344755}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1569 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{382.81554163323983, 624.6387685344755}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1570 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1566 + Rotation + 225 + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1572 + Points + + {483.455997308875, 702.29824662848364} + {481.24628861766723, 701.14919810905553} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{395.55293103479517, 630.11574310288108}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1573 + Rotation + 315 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{385.15306627407972, 627.32983937284462}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1574 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{382.40306627407972, 624.57983937284462}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1575 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1571 + Rotation + 225 + VFlip + YES + + + Bounds + {{385.11275645670196, 627.28952989671052}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1576 + Rotation + 225 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.673128 + g + 0.534129 + r + 0.217952 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + ID + 1564 + Rotation + 315 + + + Class + Group + Graphics + + + Bounds + {{170.01738471163631, 670.02445557113811}, {24, 24}} + Class + ShapedGraphic + ID + 1578 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 3} + VerticalPad + 0 + + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1580 + Points + + {229.63277040763995, 658.09714165007426} + {227.42306171643185, 659.24619016950237} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{141.72970413355978, 711.77964517567739}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1581 + Rotation + 45 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + + + Bounds + {{131.32983937284456, 631.5655489057134}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1582 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{128.57983937284456, 628.8155489057134}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1583 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1579 + Rotation + 315 + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1585 + Points + + {134.58583749968011, 706.12148080219276} + {136.79554619088805, 704.97243228276466} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{206.48890377375986, 633.93897727658998}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1586 + Rotation + 225 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + + + Bounds + {{131.38876853447516, 631.15307354655363}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1587 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{128.63876853447516, 628.40307354655363}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1588 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1584 + Rotation + 315 + + + Bounds + {{131.1057015197467, 631.11276372917564}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1589 + Rotation + 315 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.234512 + g + 0.673128 + r + 0.148947 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + + + ID + 1577 + Rotation + 315 + + + Bounds + {{253.2315878349163, 736.67782196944336}, {8, 134}} + Class + ShapedGraphic + ID + 1590 + Rotation + 225 + Shape + Rectangle + + + Bounds + {{371.51084102985902, 644.26864825135988}, {8, 134}} + Class + ShapedGraphic + ID + 1591 + Rotation + 240 + Shape + Rectangle + + + Bounds + {{353.64074716687509, 736.67780398678804}, {8, 134}} + Class + ShapedGraphic + ID + 1592 + Rotation + 315 + Shape + Rectangle + + + Bounds + {{234.00404905414581, 645.26862880510475}, {8, 134}} + Class + ShapedGraphic + ID + 1593 + Rotation + 300 + Shape + Rectangle + + + ID + 1533 + Class Group @@ -13353,7 +15044,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -13744,7 +15435,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14135,7 +15826,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14526,7 +16217,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14928,7 +16619,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14966,7 +16657,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -15337,7 +17028,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -15708,7 +17399,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -16115,7 +17806,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -16581,6 +18272,94 @@ PX4IO or PX4FMU} VPages 1 + + ActiveLayerIndex + 0 + AutoAdjust + + BackgroundGraphic + + Bounds + {{0, 0}, {806, 1132}} + Class + SolidGraphic + ID + 2 + Style + + shadow + + Draws + NO + + stroke + + Draws + NO + + + + BaseZoom + 0 + CanvasOrigin + {0, 0} + ColumnAlign + 1 + ColumnSpacing + 36 + DisplayScale + 1 0/72 in = 1.0000 in + GraphicsList + + GridInfo + + HPages + 1 + KeepToScale + + Layers + + + Lock + NO + Name + Ebene 1 + Print + YES + View + YES + + + LayoutInfo + + Animate + NO + circoMinDist + 18 + circoSeparation + 0.0 + layoutEngine + dot + neatoSeparation + 0.0 + twopiSeparation + 0.0 + + Orientation + 2 + PrintOnePage + + RowAlign + 1 + RowSpacing + 36 + SheetTitle + Arbeitsfläche 6 + UniqueID + 6 + VPages + 1 + SmartAlignmentGuidesActive YES @@ -16600,7 +18379,7 @@ PX4IO or PX4FMU} Frame - {{96, 56}, {1043, 822}} + {{170, 4}, {1043, 774}} ListView OutlineWidth @@ -16614,7 +18393,7 @@ PX4IO or PX4FMU} SidebarWidth 120 VisibleRegion - {{-51, -33}, {908, 683}} + {{-51, 366}, {908, 635}} Zoom 1 ZoomValues @@ -16644,6 +18423,11 @@ PX4IO or PX4FMU} 1 1 + + Arbeitsfläche 6 + 1 + 1 + From ad732ee3a146b40c2b600eb78f804086105a4c57 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:00:32 +1000 Subject: [PATCH 06/75] free perf counters in driver destructor this prevents drivers that probe on one bus then instantiate on another from leaving behind stale/duplicate perf counters --- src/drivers/airspeed/airspeed.cpp | 5 +++++ src/drivers/hmc5883/hmc5883.cpp | 5 +++++ src/drivers/ms5611/ms5611.cpp | 6 ++++++ 3 files changed, 16 insertions(+) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 277d8249a3..e54f5b3abe 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -106,6 +106,11 @@ Airspeed::~Airspeed() /* free any existing reports */ if (_reports != nullptr) delete[] _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d77f03bb72..1a337ca3a8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -359,6 +359,11 @@ HMC5883::~HMC5883() /* free any existing reports */ if (_reports != nullptr) delete[] _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index d9268c0b30..b572e042c6 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -225,6 +225,12 @@ MS5611::~MS5611() if (_reports != nullptr) delete[] _reports; + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + delete _interface; } From fdbc09e2a53281b8dda7c48676dcf695a79ba373 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:31:27 +1000 Subject: [PATCH 07/75] avoid counters going above limit in INCREMENT() when using INCREMENT() the counter would temporarily read equal to limit, which could cause an issue if the task is preempted. (this macro should be in a common header, though which header?) --- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/bma180/bma180.cpp | 2 +- src/drivers/hmc5883/hmc5883.cpp | 2 +- src/drivers/l3gd20/l3gd20.cpp | 2 +- src/drivers/lsm303d/lsm303d.cpp | 2 +- src/drivers/mb12xx/mb12xx.cpp | 2 +- src/drivers/ms5611/ms5611.cpp | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 89dfb22d70..b87494b40f 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -165,5 +165,5 @@ protected: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index cfb6256704..079b5d21cb 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -234,7 +234,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) BMA180::BMA180(int bus, spi_dev_e device) : diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 1a337ca3a8..a5229b2371 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -311,7 +311,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) /* * Driver 'main' command. diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 5e0a2119a1..e6d765e13a 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -300,7 +300,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index cf5f8d94ce..05d6f1881d 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -421,7 +421,7 @@ private: /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index c5f49fb36e..f834169935 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -184,7 +184,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) /* * Driver 'main' command. diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index b572e042c6..4e43f19c5d 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -77,7 +77,7 @@ static const int ERROR = -1; #endif /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) /* helper macro for arithmetic - returns the square of the argument */ #define POW2(_x) ((_x) * (_x)) From 935ed2fe49370e5eafe3b5445eda2c5714162216 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:03:43 +1000 Subject: [PATCH 08/75] meas_airspeed: don't use stale/bad data in airspeed reading also fixed handling of perf counters on error --- src/drivers/meas_airspeed/meas_airspeed.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 666bd30e64..b1cb2b3d84 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -162,6 +162,8 @@ MEASAirspeed::collect() if (ret < 0) { log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } @@ -169,9 +171,14 @@ MEASAirspeed::collect() if (status == 2) { log("err: stale data"); - + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; } else if (status == 3) { log("err: fault"); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; } //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); From 76a9e34e08573ff67ad34eb0251e3a7bdefd0406 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:26:21 +1000 Subject: [PATCH 09/75] I2C airspeed driver needs 2 retries this prevents I2C transfer errors every few seconds with the meas_airspeed driver --- src/drivers/airspeed/airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index e54f5b3abe..1ec61eb60b 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -157,7 +157,7 @@ Airspeed::probe() */ _retries = 4; int ret = measure(); - _retries = 0; + _retries = 2; return ret; } From 0fb3be64eace32c9be8a17dab3d1cc3ee0459f9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 11:18:10 +0200 Subject: [PATCH 10/75] More verbosity on RC cal fail in sensors app --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e857b1c2f7..a9c49c4415 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -673,7 +673,7 @@ Sensors::parameters_update() if (!isfinite(_parameters.scaling_factor[i]) || _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { - + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, _parameters.scaling_factor[i], (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0; rc_valid = false; From 4c3c09990262bd0f9e9574dd950da62bb7b432a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 14 Aug 2013 15:53:45 +1000 Subject: [PATCH 11/75] USB: set attributes for bus power, no remote wakeup this may help the USB bus providing the full 500mA on some systems --- nuttx-configs/px4fmu-v1/nsh/defconfig | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a6f914a647..68ab4da32d 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -538,9 +538,10 @@ CONFIG_USBDEV=y # # CONFIG_USBDEV_ISOCHRONOUS is not set # CONFIG_USBDEV_DUALSPEED is not set -CONFIG_USBDEV_SELFPOWERED=y -# CONFIG_USBDEV_BUSPOWERED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_REMOTEWAKEUP is not set # CONFIG_USBDEV_DMA is not set # CONFIG_USBDEV_TRACE is not set From b7ee1d34294b536c6a92c8e72455f24684a5db4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 14:34:49 +0200 Subject: [PATCH 12/75] Prevented an analog airspeed corner case from happening --- src/modules/sensors/sensor_params.c | 3 ++- src/modules/sensors/sensors.cpp | 9 +++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index fd2a6318eb..16bfc8094f 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -68,7 +68,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); +PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a9c49c4415..88a6bebb81 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -301,6 +301,7 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; + float diff_pres_analog_enabled; int board_rotation; int external_mag_rotation; @@ -348,6 +349,7 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; + param_t diff_pres_analog_enabled; param_t rc_map_roll; param_t rc_map_pitch; @@ -617,6 +619,7 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); @@ -784,6 +787,7 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); + param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -1266,9 +1270,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /** * The voltage divider pulls the signal down, only act on - * a valid voltage from a connected sensor + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. */ - if (voltage > 0.4f) { + if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) { float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor From 49ef30b834888b91e7238662b23d651addf3b4b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 14:58:29 +0200 Subject: [PATCH 13/75] Reworked how start scripts work, relying on io detect now --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 2 +- ROMFS/px4fmu_common/init.d/08_ardrone | 2 +- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 2 +- ROMFS/px4fmu_common/init.d/10_io_f330 | 43 ++++++++------ ROMFS/px4fmu_common/init.d/15_io_tbs | 2 +- ROMFS/px4fmu_common/init.d/30_io_camflyer | 2 +- ROMFS/px4fmu_common/init.d/31_io_phantom | 2 +- ROMFS/px4fmu_common/init.d/40_io_segway | 2 +- ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 2 +- ROMFS/px4fmu_common/init.d/rc.boarddetect | 66 ---------------------- ROMFS/px4fmu_common/init.d/rc.hil | 10 ---- 11 files changed, 35 insertions(+), 100 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.boarddetect diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index f57e4bd68d..b106a974f8 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -25,7 +25,7 @@ then param set RC_SCALE_YAW 3 param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index eb9f82f771..f6d82a5ec4 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index 44fbb79b7b..794342a0b1 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 7b6509bf88..5af4f97b5a 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -1,6 +1,6 @@ #!nsh -echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame" +echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" # # Load default params for this platform @@ -24,7 +24,7 @@ then param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_P 0.3 - param save /fs/microsd/params + param save fi # @@ -32,24 +32,30 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - -# -# Start MAVLink (depends on orb) -# -mavlink start -usleep 5000 + +set EXIT_ON_END no # -# Start and configure PX4IO interface +# Start and configure PX4IO or FMU interface # -sh /etc/init.d/rc.io +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 -# -# Set PWM values for DJI ESCs -# -px4io idle 900 900 900 900 -px4io min 1200 1200 1200 1200 -px4io max 1800 1800 1800 1800 + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi # # Load mixer @@ -65,3 +71,8 @@ pwm -u 400 -m 0xff # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs index b4f063e524..3fce394035 100644 --- a/ROMFS/px4fmu_common/init.d/15_io_tbs +++ b/ROMFS/px4fmu_common/init.d/15_io_tbs @@ -24,7 +24,7 @@ then param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_P 0.2 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 6a0bd4da82..390649f898 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 7183138622..45563310ce 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 2890f43bec..4b7ed52869 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -9,7 +9,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index 2c82180131..938663f0c2 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -25,7 +25,7 @@ then param set RC_SCALE_YAW 3 param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/rc.boarddetect b/ROMFS/px4fmu_common/init.d/rc.boarddetect deleted file mode 100644 index f233e51df4..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.boarddetect +++ /dev/null @@ -1,66 +0,0 @@ -#!nsh -# -# If we are still in flight mode, work out what airframe -# configuration we have and start up accordingly. -# -if [ $MODE != autostart ] -then - echo "[init] automatic startup cancelled by user script" -else - echo "[init] detecting attached hardware..." - - # - # Assume that we are PX4FMU in standalone mode - # - set BOARD PX4FMU - - # - # Are we attached to a PX4IOAR (AR.Drone carrier board)? - # - if boardinfo test name PX4IOAR - then - set BOARD PX4IOAR - if [ -f /etc/init.d/rc.PX4IOAR ] - then - echo "[init] reading /etc/init.d/rc.PX4IOAR" - usleep 500 - sh /etc/init.d/rc.PX4IOAR - fi - else - echo "[init] PX4IOAR not detected" - fi - - # - # Are we attached to a PX4IO? - # - if boardinfo test name PX4IO - then - set BOARD PX4IO - if [ -f /etc/init.d/rc.PX4IO ] - then - echo "[init] reading /etc/init.d/rc.PX4IO" - usleep 500 - sh /etc/init.d/rc.PX4IO - fi - else - echo "[init] PX4IO not detected" - fi - - # - # Looks like we are stand-alone - # - if [ $BOARD == PX4FMU ] - then - echo "[init] no expansion board detected" - if [ -f /etc/init.d/rc.standalone ] - then - echo "[init] reading /etc/init.d/rc.standalone" - sh /etc/init.d/rc.standalone - fi - fi - - # - # We may not reach here if the airframe-specific script exits the shell. - # - echo "[init] startup done." -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil index a843b7ffbc..eccb2b7678 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ b/ROMFS/px4fmu_common/init.d/rc.hil @@ -13,16 +13,6 @@ mavlink start -b 230400 -d /dev/ttyS1 # Create a fake HIL /dev/pwm_output interface hil mode_pwm -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi - # # Force some key parameters to sane values # MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor From a48be0446bf78f73d7550864150c1cf3de4dafa3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 14:58:53 +0200 Subject: [PATCH 14/75] Added IO detect command to be smart about what to start before actually doing it --- src/drivers/px4io/px4io.cpp | 65 ++++++++++++++++++++++++++++++++++++- 1 file changed, 64 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 392bc9f0ae..dda1b825cb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -119,10 +119,17 @@ public: /** * Initialize the PX4IO class. * - * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers. + * Retrieve relevant initial system parameters. Initialize PX4IO registers. */ virtual int init(); + /** + * Detect if a PX4IO is connected. + * + * Only validate if there is a PX4IO to talk to. + */ + virtual int detect(); + /** * IO Control handler. * @@ -475,6 +482,29 @@ PX4IO::~PX4IO() g_dev = nullptr; } +int +PX4IO::detect() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) + return ret; + + /* get some parameters */ + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { + log("protocol/firmware mismatch"); + mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); + return -1; + } + + return 0; +} + int PX4IO::init() { @@ -1894,6 +1924,7 @@ start(int argc, char *argv[]) if (OK != g_dev->init()) { delete g_dev; + g_dev = nullptr; errx(1, "driver init failed"); } @@ -1920,6 +1951,35 @@ start(int argc, char *argv[]) exit(0); } +void +detect(int argc, char *argv[]) +{ + if (g_dev != nullptr) + errx(0, "already loaded"); + + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + + if (OK != g_dev->detect()) { + delete g_dev; + g_dev = nullptr; + errx(1, "driver detect did not succeed"); + } + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + exit(0); +} + void bind(int argc, char *argv[]) { @@ -2079,6 +2139,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) start(argc - 1, argv + 1); + if (!strcmp(argv[1], "detect")) + detect(argc - 1, argv + 1); + if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { From feb4dad9e1c8e766cac93b55437c8234493ed71b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 15:22:24 +0200 Subject: [PATCH 15/75] Fixed IO detect message --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dda1b825cb..beeb019748 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -497,10 +497,10 @@ PX4IO::detect() /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); if (protocol != PX4IO_PROTOCOL_VERSION) { - log("protocol/firmware mismatch"); - mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); + log("IO not installed"); return -1; } + log("IO found"); return 0; } From 0731d331bfbee88f91fa4737ec77c0a66ce171f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 15:50:06 +0200 Subject: [PATCH 16/75] ROMFS reshuffling / cleanup, in sync with QGC --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 64 --------------- ROMFS/px4fmu_common/init.d/02_io_quad_x | 47 ----------- ROMFS/px4fmu_common/init.d/100_mpx_easystar | 68 ++++++++++++++++ .../init.d/{10_io_f330 => 10_dji_f330} | 0 ROMFS/px4fmu_common/init.d/11_dji_f450 | 78 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/15_tbs_discovery | 78 +++++++++++++++++++ .../init.d/{15_io_tbs => 16_3dr_iris} | 39 ++++++---- ROMFS/px4fmu_common/init.d/30_io_camflyer | 5 +- ROMFS/px4fmu_common/init.d/31_io_phantom | 5 +- ROMFS/px4fmu_common/init.d/rcS | 27 +++---- 10 files changed, 269 insertions(+), 142 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/01_fmu_quad_x delete mode 100644 ROMFS/px4fmu_common/init.d/02_io_quad_x create mode 100644 ROMFS/px4fmu_common/init.d/100_mpx_easystar rename ROMFS/px4fmu_common/init.d/{10_io_f330 => 10_dji_f330} (100%) create mode 100644 ROMFS/px4fmu_common/init.d/11_dji_f450 create mode 100644 ROMFS/px4fmu_common/init.d/15_tbs_discovery rename ROMFS/px4fmu_common/init.d/{15_io_tbs => 16_3dr_iris} (67%) diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x deleted file mode 100644 index b106a974f8..0000000000 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ /dev/null @@ -1,64 +0,0 @@ -#!nsh - -echo "[init] 01_fmu_quad_x: PX4FMU Quad X with PWM outputs" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 - param set MC_ATT_D 0 - param set MC_YAWPOS_D 0 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.08 - param set RC_SCALE_PITCH 1 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_YAW 3 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Start PWM output -# -fmu mode_pwm - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm -u 400 -m 0xff - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -# Exit, because /dev/ttyS0 is needed for MAVLink -exit diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x deleted file mode 100644 index a37c26ad1d..0000000000 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ /dev/null @@ -1,47 +0,0 @@ -#!nsh - -echo "[init] 02_io_quad_x: PX4FMU+PX4IO Quad X with PWM outputs" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm -u 400 -m 0xff - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar new file mode 100644 index 0000000000..e1cefdb993 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -0,0 +1,68 @@ +#!nsh + +echo "[init] Multiplex Easystar" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +# +# Start MAVLink (depends on orb) +# +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +# +# Start and configure PX4IO interface +# +sh /etc/init.d/rc.io + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 + +# +# Start the commander +# +commander start + +# +# Start the sensors +# +sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + +# +# Start GPS interface +# +gps start + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix +fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 similarity index 100% rename from ROMFS/px4fmu_common/init.d/10_io_f330 rename to ROMFS/px4fmu_common/init.d/10_dji_f330 diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 new file mode 100644 index 0000000000..5af4f97b5a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -0,0 +1,78 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi + +# +# Load mixer +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery new file mode 100644 index 0000000000..4e7406b66d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -0,0 +1,78 @@ +#!nsh + +echo "[init] Team Blacksheep Discovery Quad" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.006 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 5.0 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi + +# +# Load the mixer for a quad with wide arms +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/16_3dr_iris similarity index 67% rename from ROMFS/px4fmu_common/init.d/15_io_tbs rename to ROMFS/px4fmu_common/init.d/16_3dr_iris index 3fce394035..5bcaaaac14 100644 --- a/ROMFS/px4fmu_common/init.d/15_io_tbs +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -1,6 +1,6 @@ #!nsh -echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad" +echo "[init] 3DR Iris Quad" # # Load default params for this platform @@ -33,23 +33,29 @@ fi # param set MAV_TYPE 2 -# -# Start MAVLink (depends on orb) -# -mavlink start -usleep 5000 +set EXIT_ON_END no # -# Start and configure PX4IO interface +# Start and configure PX4IO or FMU interface # -sh /etc/init.d/rc.io +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 -# -# Set PWM values for DJI ESCs -# -px4io idle 900 900 900 900 -px4io min 1180 1180 1180 1180 -px4io max 1800 1800 1800 1800 + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi # # Load the mixer for a quad with wide arms @@ -65,3 +71,8 @@ pwm -u 400 -m 0xff # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 390649f898..f7e653362f 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -61,12 +61,13 @@ sh /etc/init.d/rc.logging gps start # -# Start the attitude estimator (depends on orb) +# Start the attitude and position estimator # -kalman_demo start +att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 45563310ce..e1e6099270 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -56,12 +56,13 @@ sh /etc/init.d/rc.logging gps start # -# Start the attitude estimator +# Start the attitude and position estimator # -kalman_demo start +att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6da03402e9..3ef6eb75ef 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -125,17 +125,6 @@ then # Check if auto-setup from one of the standard scripts is wanted # SYS_AUTOSTART = 0 means no autostart (default) # - if param compare SYS_AUTOSTART 1 - then - sh /etc/init.d/01_fmu_quad_x - set MODE custom - fi - - if param compare SYS_AUTOSTART 2 - then - sh /etc/init.d/02_io_quad_x - set MODE custom - fi if param compare SYS_AUTOSTART 8 then @@ -151,13 +140,25 @@ then if param compare SYS_AUTOSTART 10 then - sh /etc/init.d/10_io_f330 + sh /etc/init.d/10_dji_f330 + set MODE custom + fi + + if param compare SYS_AUTOSTART 11 + then + sh /etc/init.d/11_dji_f450 set MODE custom fi if param compare SYS_AUTOSTART 15 then - sh /etc/init.d/15_io_tbs + sh /etc/init.d/15_tbs_discovery + set MODE custom + fi + + if param compare SYS_AUTOSTART 16 + then + sh /etc/init.d/16_3dr_iris set MODE custom fi From 756d67f2de759cb8d1e4be26deb108fc4e32899d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 16:38:09 +0200 Subject: [PATCH 17/75] Removed non-helpful verbosity --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index beeb019748..1fa3fbbfbe 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1969,7 +1969,7 @@ detect(int argc, char *argv[]) if (OK != g_dev->detect()) { delete g_dev; g_dev = nullptr; - errx(1, "driver detect did not succeed"); + exit(1); } if (g_dev != nullptr) { From 5ef7e71bb0ca81551622fb5089b34c46770c0706 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 16:42:43 +0200 Subject: [PATCH 18/75] More graceful startup messages --- ROMFS/px4fmu_common/init.d/rcS | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3ef6eb75ef..56b068ccac 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -191,13 +191,13 @@ then commander start # Start px4io if present - if px4io start + if px4io detect then - echo "PX4IO driver started" + px4io start else if fmu mode_serial then - echo "FMU driver started" + echo "FMU driver (no PWM) started" fi fi From 007f4d79cbd7edec6fb69e4556f35bac8dce72f2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 28 Aug 2013 16:51:02 +0200 Subject: [PATCH 19/75] param MC_RCLOSS_THR removed from scripts --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 1 - ROMFS/px4fmu_common/init.d/11_dji_f450 | 1 - ROMFS/px4fmu_common/init.d/15_tbs_discovery | 1 - ROMFS/px4fmu_common/init.d/16_3dr_iris | 1 - 4 files changed, 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 5af4f97b5a..fb141e8a73 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -16,7 +16,6 @@ then param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 param set MC_ATT_P 7.0 - param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.0 param set MC_YAWPOS_P 2.0 diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 5af4f97b5a..fb141e8a73 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -16,7 +16,6 @@ then param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 param set MC_ATT_P 7.0 - param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.0 param set MC_YAWPOS_P 2.0 diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index 4e7406b66d..1fcde3e27f 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -16,7 +16,6 @@ then param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 param set MC_ATT_P 5.0 - param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.15 param set MC_YAWPOS_P 0.5 diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index 5bcaaaac14..fa2cb1feb0 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -16,7 +16,6 @@ then param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 param set MC_ATT_P 5.0 - param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.15 param set MC_YAWPOS_P 0.5 From f2f66432d76dad2587d3e8089d0c728eb8fdf2ae Mon Sep 17 00:00:00 2001 From: tstellanova Date: Wed, 28 Aug 2013 08:49:21 -0700 Subject: [PATCH 20/75] Grab UTC time from GPS --- src/drivers/gps/ubx.cpp | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b579db7150..ba5d14cc49 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -42,13 +42,14 @@ * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf */ - -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include + #include #include #include @@ -452,7 +453,16 @@ UBX::handle_message() timeinfo.tm_min = packet->min; timeinfo.tm_sec = packet->sec; time_t epoch = mktime(&timeinfo); - + +#ifndef CONFIG_RTC + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = packet->time_nanoseconds; + clock_settime(CLOCK_REALTIME,&ts); +#endif + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); _gps_position->timestamp_time = hrt_absolute_time(); From 53813d44a0c6322e46e72c0439e0265ddea76e9e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 28 Aug 2013 21:41:52 +0200 Subject: [PATCH 21/75] sdlog2: "landed" flag logging --- src/modules/sdlog2/sdlog2.c | 9 +++++---- src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2b21bb5a5a..e83fb7dd3a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -890,13 +890,14 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (unsigned char) buf_status.main_state; - log_msg.body.log_STAT.navigation_state = (unsigned char) buf_status.navigation_state; - log_msg.body.log_STAT.arming_state = (unsigned char) buf_status.arming_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; - log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; + log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; + log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; LOGBUFFER_WRITE_AND_COUNT(STAT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d99892fe2e..4eeb65a87e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -155,6 +155,7 @@ struct log_STAT_s { float battery_current; float battery_remaining; uint8_t battery_warning; + uint8_t landed; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -263,7 +264,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfffB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), From b986607a750db74e05c60d09b048971bb0cfb18f Mon Sep 17 00:00:00 2001 From: tstellanova Date: Wed, 28 Aug 2013 18:32:16 -0700 Subject: [PATCH 22/75] Add defines for RC15 params (16 channels total) minor cleanup of rc sanity check code --- src/modules/sensors/sensor_params.c | 7 +++++++ src/modules/sensors/sensors.cpp | 23 ++++++++++++++--------- 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 16bfc8094f..dd86f3e2e1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -158,6 +158,13 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC15_MIN, 1000); +PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC15_MAX, 2000); +PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); + + PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 88a6bebb81..e98c4d5489 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -660,7 +660,9 @@ int Sensors::parameters_update() { bool rc_valid = true; - + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + /* rc values */ for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { @@ -670,18 +672,21 @@ Sensors::parameters_update() param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); param_get(_parameter_handles.dz[i], &(_parameters.dz[i])); - _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); - + tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; + /* handle blowup in the scaling factor calculation */ - if (!isfinite(_parameters.scaling_factor[i]) || - _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || - _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { - warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, _parameters.scaling_factor[i], (int)(_parameters.rev[i])); + if (!isfinite(tmpScaleFactor) || + (tmpRevFactor < 0.000001f) || + (tmpRevFactor > 0.2f) ) { + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ - _parameters.scaling_factor[i] = 0; + _parameters.scaling_factor[i] = 0.0f; rc_valid = false; } - + else { + _parameters.scaling_factor[i] = tmpScaleFactor; + } } /* handle wrong values */ From 55cfa5152c5e3ca3148eee776ec4467dd53eeca3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Aug 2013 16:41:59 +0200 Subject: [PATCH 23/75] Made low/critical warnings consisting --- src/drivers/blinkm/blinkm.cpp | 4 ++-- src/modules/commander/commander.cpp | 16 ++++++++-------- src/modules/uORB/topics/vehicle_status.h | 6 +++--- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 490002254d..2361f4dd18 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -541,7 +541,7 @@ BlinkM::led() printf(" cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; @@ -553,7 +553,7 @@ BlinkM::led() led_color_8 = LED_YELLOW; led_blink = LED_BLINK; - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a548f943e2..e90300affc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -890,7 +890,7 @@ int commander_thread_main(int argc, char *argv[]) if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; battery_tune_played = false; } @@ -902,7 +902,7 @@ int commander_thread_main(int argc, char *argv[]) if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; battery_tune_played = false; if (armed.armed) { @@ -1160,12 +1160,12 @@ int commander_thread_main(int argc, char *argv[]) if (tune_arm() == OK) arm_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* play tune on battery warning */ if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ if (tune_critical_bat() == OK) battery_tune_played = true; @@ -1258,10 +1258,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (armed->armed) { /* armed, solid */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { @@ -1272,10 +1272,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang } else if (armed->ready_to_arm) { for (i = 0; i < 3; i++) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 43218eac4e..1c184d3a7a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -153,9 +153,9 @@ enum VEHICLE_TYPE { }; enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ - VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */ + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ + VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ }; /** From 9bcfe49cff7b8fce6dfbeac7f8233e554672ebff Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Aug 2013 16:45:33 +0200 Subject: [PATCH 24/75] Changed RGBLED behaviour, breathe mode when disarmed and ready to arm --- src/drivers/drv_rgbled.h | 2 + src/drivers/rgbled/rgbled.cpp | 46 ++++++++++++-- src/modules/commander/commander.cpp | 95 +++++++++++------------------ 3 files changed, 78 insertions(+), 65 deletions(-) diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 3c8bdec5d3..07c6186dde 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -78,6 +78,7 @@ /** set pattern */ #define RGBLED_SET_PATTERN _RGBLEDIOC(7) + /* structure passed to RGBLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless @@ -115,6 +116,7 @@ typedef enum { RGBLED_MODE_BLINK_SLOW, RGBLED_MODE_BLINK_NORMAL, RGBLED_MODE_BLINK_FAST, + RGBLED_MODE_BREATHE, RGBLED_MODE_PATTERN } rgbled_mode_t; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 05f079ede2..feb8f1c6c9 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -96,6 +96,11 @@ private: rgbled_mode_t _mode; rgbled_pattern_t _pattern; + float _brightness; + uint8_t _r; + uint8_t _g; + uint8_t _b; + bool _should_run; bool _running; int _led_interval; @@ -104,6 +109,7 @@ private: void set_color(rgbled_color_t ledcolor); void set_mode(rgbled_mode_t mode); void set_pattern(rgbled_pattern_t *pattern); + void set_brightness(float brightness); static void led_trampoline(void *arg); void led(); @@ -128,6 +134,10 @@ RGBLED::RGBLED(int bus, int rgbled) : _color(RGBLED_COLOR_OFF), _mode(RGBLED_MODE_OFF), _running(false), + _brightness(1.0f), + _r(0), + _g(0), + _b(0), _led_interval(0), _counter(0) { @@ -191,7 +201,6 @@ int RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = ENOTTY; - switch (cmd) { case RGBLED_SET_RGB: /* set the specified RGB values */ @@ -246,6 +255,15 @@ RGBLED::led() else set_on(false); break; + case RGBLED_MODE_BREATHE: + if (_counter >= 30) + _counter = 0; + if (_counter <= 15) { + set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f)); + } else { + set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f)); + } + break; case RGBLED_MODE_PATTERN: /* don't run out of the pattern array and stop if the next frame is 0 */ if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) @@ -294,7 +312,7 @@ RGBLED::set_color(rgbled_color_t color) { case RGBLED_COLOR_AMBER: // amber set_rgb(255,20,0); break; - case RGBLED_COLOR_DIM_RED: // red + case RGBLED_COLOR_DIM_RED: // red set_rgb(90,0,0); break; case RGBLED_COLOR_DIM_YELLOW: // yellow @@ -306,7 +324,7 @@ RGBLED::set_color(rgbled_color_t color) { case RGBLED_COLOR_DIM_GREEN: // green set_rgb(0,90,0); break; - case RGBLED_COLOR_DIM_BLUE: // blue + case RGBLED_COLOR_DIM_BLUE: // blue set_rgb(0,0,90); break; case RGBLED_COLOR_DIM_WHITE: // white @@ -347,6 +365,12 @@ RGBLED::set_mode(rgbled_mode_t mode) _should_run = true; _led_interval = 100; break; + case RGBLED_MODE_BREATHE: + _should_run = true; + set_on(true); + _counter = 0; + _led_interval = 1000/15; + break; case RGBLED_MODE_PATTERN: _should_run = true; set_on(true); @@ -377,6 +401,13 @@ RGBLED::set_pattern(rgbled_pattern_t *pattern) set_mode(RGBLED_MODE_PATTERN); } +void +RGBLED::set_brightness(float brightness) { + + _brightness = brightness; + set_rgb(_r, _g, _b); +} + int RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) { @@ -413,7 +444,12 @@ RGBLED::set_on(bool on) int RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b) { - const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)}; + /* save the RGB values in case we want to change the brightness later */ + _r = r; + _g = g; + _b = b; + + const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)}; return transfer(msg, sizeof(msg), nullptr, 0); } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e90300affc..776cd5766d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -689,6 +689,8 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); + toggle_status_leds(&status, &armed, true); + /* now initialized */ commander_initialized = true; thread_running = true; @@ -1252,72 +1254,45 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (changed) { - int i; - rgbled_pattern_t pattern; - memset(&pattern, 0, sizeof(pattern)); + /* XXX TODO blink fast when armed and serious error occurs */ if (armed->armed) { - /* armed, solid */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - - } else { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; - } - - pattern.duration[0] = 1000; - + rgbled_set_mode(RGBLED_MODE_ON); } else if (armed->ready_to_arm) { - for (i = 0; i < 3; i++) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - - } else { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; - } - - pattern.duration[i * 2] = 200; - - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 800; - } - - if (status->condition_global_position_valid) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i * 2] = 1000; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 800; - - } else { - for (i = 3; i < 6; i++) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i * 2] = 100; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 100; - } - - pattern.color[6 * 2] = RGBLED_COLOR_OFF; - pattern.duration[6 * 2] = 700; - } - + rgbled_set_mode(RGBLED_MODE_BREATHE); } else { - for (i = 0; i < 3; i++) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - pattern.duration[i * 2] = 200; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 200; - } - - /* not ready to arm, blink at 10Hz */ + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); } - rgbled_set_pattern(&pattern); + + } + + if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { + switch (status->battery_warning) { + case VEHICLE_BATTERY_WARNING_LOW: + rgbled_set_color(RGBLED_COLOR_YELLOW); + break; + case VEHICLE_BATTERY_WARNING_CRITICAL: + rgbled_set_color(RGBLED_COLOR_AMBER); + break; + default: + break; + } + } else { + switch (status->main_state) { + case MAIN_STATE_MANUAL: + rgbled_set_color(RGBLED_COLOR_WHITE); + break; + case MAIN_STATE_SEATBELT: + case MAIN_STATE_EASY: + rgbled_set_color(RGBLED_COLOR_GREEN); + break; + case MAIN_STATE_AUTO: + rgbled_set_color(RGBLED_COLOR_BLUE); + break; + default: + break; + } } /* give system warnings on error LED, XXX maybe add memory usage warning too */ From 5146dd8ff80f6cff127fbdac27f4cb92e5954924 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 Aug 2013 10:09:31 +0200 Subject: [PATCH 25/75] position_estimator_inav: critical bug fixed --- .../position_estimator_inav_main.c | 35 +++++++++++-------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 4a4572b09c..88057b3237 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -429,26 +429,31 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { /* initialize reference position if needed */ if (!ref_xy_inited) { - if (ref_xy_init_start == 0) { - ref_xy_init_start = t; + /* require EPH < 10m */ + if (gps.eph_m < 10.0f) { + if (ref_xy_init_start == 0) { + ref_xy_init_start = t; - } else if (t > ref_xy_init_start + ref_xy_init_delay) { - ref_xy_inited = true; - /* reference GPS position */ - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; + } else if (t > ref_xy_init_start + ref_xy_init_delay) { + ref_xy_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; - local_pos.ref_lat = gps.lat; - local_pos.ref_lon = gps.lon; - local_pos.ref_timestamp = t; + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_timestamp = t; - /* initialize projection */ - map_projection_init(lat, lon); - warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + } + } else { + ref_xy_init_start = 0; } } From 3a00def1899223514ea0f9bd6bd567ac11d02f7e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 Aug 2013 10:11:24 +0200 Subject: [PATCH 26/75] commander: switch to AUTO_READY or AUTO_MISSION immediately, don't try to stay on ground --- src/modules/commander/commander.cpp | 84 +++++++++---------- src/modules/commander/commander_params.c | 2 +- .../commander/state_machine_helper.cpp | 23 ++--- 3 files changed, 52 insertions(+), 57 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 776cd5766d..8ddd86d03f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -153,6 +153,7 @@ static uint64_t last_print_mode_reject_time = 0; /* if connected via USB */ static bool on_usb_power = false; +static float takeoff_alt = 5.0f; /* tasks waiting for low prio thread */ typedef enum { @@ -492,9 +493,10 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); + param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); /* welcome user */ - warnx("[commander] starting"); + warnx("starting"); /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -733,8 +735,11 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_component_id, &(status.component_id)); status_changed = true; - /* Re-check RC calibration */ + /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check()); + + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); } } @@ -1253,7 +1258,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang #endif if (changed) { - /* XXX TODO blink fast when armed and serious error occurs */ if (armed->armed) { @@ -1263,8 +1267,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang } else { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); } - - } if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { @@ -1455,54 +1457,52 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (status->main_state == MAIN_STATE_AUTO) { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + // TODO AUTO_LAND handling if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -5.0f || status->condition_landed) { - res = TRANSITION_NOT_CHANGED; + if (local_pos->z > -takeoff_alt || status->condition_landed) { + return TRANSITION_NOT_CHANGED; } } - - if (res != TRANSITION_NOT_CHANGED) { - /* check again, state can be changed */ - if (status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here + if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && + status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && + status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && + status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { + /* possibly on ground, switch to TAKEOFF if needed */ + if (local_pos->z > -takeoff_alt || status->condition_landed) { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + return res; + } + } + /* switch to AUTO mode */ + if (status->rc_signal_found_once && !status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* not landed */ - if (status->rc_signal_found_once && !status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } + if (status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { - /* switch to MISSION in air when no RC control */ - if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || - status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || - status->navigation_state == NAVIGATION_STATE_AUTO_RTL || - status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - res = TRANSITION_NOT_CHANGED; - - } else { - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - } + /* LOITER */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } - } + } else { + /* switch to MISSION when no RC control and first time in some AUTO mode */ + if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + res = TRANSITION_NOT_CHANGED; + } else { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + } + } } else { /* disarmed, always switch to AUTO_READY */ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0a1703b2e0..f22dac0c15 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,7 +45,7 @@ #include #include -PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 674f3feda3..3cef10185e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -348,20 +348,15 @@ navigation_state_transition(struct vehicle_status_s *status, navigation_state_t break; case NAVIGATION_STATE_AUTO_TAKEOFF: - - /* only transitions from AUTO_READY */ - if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_LOITER: From 61936412f3ccf9aff5a12d47b8c5fe34ff1a04fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Aug 2013 15:40:28 +0200 Subject: [PATCH 27/75] Speeded up IO startup, needs review --- ROMFS/px4fmu_common/init.d/rcS | 6 +++++- src/drivers/px4io/uploader.h | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 56b068ccac..982e638fb5 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -110,8 +110,12 @@ then # # Upgrade PX4IO firmware # - if px4io update + if px4io detect then + echo "PX4IO running, not upgrading" + else + echo "Attempting to upgrade PX4IO" + px4io update if [ -d /fs/microsd ] then echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index a4a8a88fe6..135202dd1b 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 100); + int get_sync(unsigned timeout = 1000); int sync(); int get_info(int param, uint32_t &val); int erase(); From e2b602339adef80af84d6d396adc1962b1f86826 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Aug 2013 17:05:21 +0200 Subject: [PATCH 28/75] Cleanup of detect return --- src/drivers/px4io/px4io.cpp | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1fa3fbbfbe..026b879497 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1966,18 +1966,16 @@ detect(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "driver alloc failed"); - if (OK != g_dev->detect()) { - delete g_dev; - g_dev = nullptr; + ret = g_dev->detect() + + delete g_dev; + g_dev = nullptr; + + if (ret) + /* nonzero, error */ exit(1); - } - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - exit(0); + else + exit(0); } void From 2b62497fb5dd83db17b1d2851f686049841faa7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Aug 2013 17:19:03 +0200 Subject: [PATCH 29/75] Fixed build error --- src/drivers/px4io/px4io.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 026b879497..47baa5770a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -890,7 +890,6 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) int PX4IO::set_min_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; if (len > _max_actuators) /* fail with error */ @@ -903,7 +902,6 @@ PX4IO::set_min_values(const uint16_t *vals, unsigned len) int PX4IO::set_max_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; if (len > _max_actuators) /* fail with error */ @@ -1370,7 +1368,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned } int ret = _interface->write((page << 8) | offset, (void *)values, num_values); - if (ret != num_values) { + if (ret != (int)num_values) { debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); return -1; } @@ -1393,7 +1391,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v } int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); - if (ret != num_values) { + if (ret != (int)num_values) { debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); return -1; } @@ -1966,16 +1964,17 @@ detect(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "driver alloc failed"); - ret = g_dev->detect() + int ret = g_dev->detect(); delete g_dev; g_dev = nullptr; - if (ret) + if (ret) { /* nonzero, error */ exit(1); - else + } else { exit(0); + } } void From fff1856377b687bc290f4a828363de0d416b401c Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Sat, 31 Aug 2013 03:11:58 +0900 Subject: [PATCH 30/75] There was a bug in position_estimator_mc. Solved --- .../position_estimator_mc/position_estimator_mc_main.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index 984bd13298..3639618193 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -497,7 +497,12 @@ int position_estimator_mc_thread_main(int argc, char *argv[]) local_pos_est.vz = x_z_aposteriori_k[1]; local_pos_est.timestamp = hrt_absolute_time(); if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){ - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + if(local_pos_est_pub > 0) + orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + else + local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); + //char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]); + //mavlink_log_info(mavlink_fd, buf); } } } /* end of poll return value check */ From 9f2419698ff4d0703f58e67f29f4981fa7b4f0c5 Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Sat, 31 Aug 2013 03:22:19 +0900 Subject: [PATCH 31/75] Topic has been changed for quaternion-based attitude controller. --- src/modules/uORB/topics/vehicle_attitude_setpoint.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index a7cda34a83..686fd765c5 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -64,6 +64,12 @@ struct vehicle_attitude_setpoint_s float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ bool R_valid; /**< Set to true if rotation matrix is valid */ + //! For quaternion-based attitude control + float q_d[4]; /** Desired quaternion for quaternion control */ + bool q_d_valid; /**< Set to true if quaternion vector is valid */ + float q_e[4]; /** Attitude error in quaternion */ + bool q_e_valid; /**< Set to true if quaternion error vector is valid */ + float thrust; /**< Thrust in Newton the power system should generate */ }; From 6a6fe6937146723cb86dc0fc6e46db38b328dacd Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Sat, 31 Aug 2013 03:23:03 +0900 Subject: [PATCH 32/75] Bug in so3 estimator related to R matrix --- .../attitude_estimator_so3_comp_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 236052b569..86bda3c75e 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -730,7 +730,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // Because proper mount of PX4 will give you a reversed accelerometer readings. NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); - // Convert q->R. + // Convert q->R, This R converts inertial frame to body frame. Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 @@ -794,7 +794,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { From efca6f2cb1848523969c7f29abba7389e48affbc Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Fri, 30 Aug 2013 20:12:35 -0400 Subject: [PATCH 33/75] Remove unused tones and save about 6K flash space - Comment out unused tones - Create symbolic tone numbers rather than hardcoded --- src/drivers/drv_tone_alarm.h | 20 ++ src/drivers/stm32/tone_alarm/tone_alarm.cpp | 321 +++++++++--------- src/modules/commander/commander_helper.cpp | 16 +- .../preflight_check/preflight_check.c | 6 +- src/systemcmds/tests/test_hrt.c | 4 +- 5 files changed, 194 insertions(+), 173 deletions(-) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 0c6afc64c4..4a399076d4 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -125,4 +125,24 @@ enum tone_pitch { TONE_NOTE_MAX }; +enum { + TONE_STOP_TUNE = 0, + TONE_STARTUP_TUNE, + TONE_ERROR_TUNE, + TONE_NOTIFY_POSITIVE_TUNE, + TONE_NOTIFY_NEUTRAL_TUNE, + TONE_NOTIFY_NEGATIVE_TUNE, + TONE_CHARGE_TUNE, + /* Do not include these unused tunes + TONE_DIXIE_TUNE, + TONE_CUCURACHA_TUNE, + TONE_YANKEE_TUNE, + TONE_DAISY_TUNE, + TONE_WILLIAM_TELL_TUNE, */ + TONE_ARMING_WARNING_TUNE, + TONE_BATTERY_WARNING_SLOW_TUNE, + TONE_BATTERY_WARNING_FAST_TUNE, + TONE_NUMBER_OF_TUNES +}; + #endif /* DRV_TONE_ALARM_H_ */ diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index ad21f7143f..a726247d55 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -233,8 +233,7 @@ public: private: static const unsigned _tune_max = 1024; // be reasonable about user tunes - static const char * const _default_tunes[]; - static const unsigned _default_ntunes; + static const char * _default_tunes[TONE_NUMBER_OF_TUNES - 1]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -305,161 +304,7 @@ private: }; // predefined tune array -const char * const ToneAlarm::_default_tunes[] = { - "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc", // startup tune - "MBT200a8a8a8PaaaP", // ERROR tone - "MFT200e8a8a", // NotifyPositive tone - "MFT200e8e", // NotifyNeutral tone - "MFT200e8c8e8c8e8c8", // NotifyNegative tone - "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge! - "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16", // dixie - "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8", // cucuracha - "MNT150L8O2GGABGBADGGABL4GL8F+", // yankee - "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy - "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell - "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" - "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" - "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" - "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" - "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" - "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" - "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" - "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" - "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" - "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" - "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" - "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" - "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" - "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" - "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" - "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" - "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" - "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" - "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" - "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" - "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" - "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" - "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" - "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" - "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" - "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" - "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" - "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" - "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" - "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" - "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" - "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" - "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" - "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" - "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" - "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" - "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" - "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" - "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" - "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" - "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" - "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" - "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" - "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" - "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" - "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" - "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" - "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" - "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" - "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" - "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" - "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." - "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" - "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" - "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" - "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" - "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" - "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" - "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" - "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" - "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" - "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" - "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" - "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" - "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" - "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" - "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" - "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" - "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" - "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" - "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" - "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" - "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" - "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" - "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" - "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" - "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" - "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" - "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" - "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" - "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" - "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" - "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" - "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" - "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" - "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" - "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" - "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" - "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" - "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" - "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" - "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" - "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" - "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" - "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" - "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" - "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" - "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" - "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" - "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" - "O2E2P64", - "MNT75L1O2G", //arming warning - "MBNT100a8", //battery warning slow - "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" //battery warning fast // XXX why is there a break before a repetition -}; - -const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); +const char * ToneAlarm::_default_tunes[TONE_NUMBER_OF_TUNES - 1]; // semitone offsets from C for the characters 'A'-'G' const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; @@ -479,6 +324,162 @@ ToneAlarm::ToneAlarm() : { // enable debug() calls //_debug_enabled = true; + _default_tunes[TONE_STARTUP_TUNE - 1] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune + _default_tunes[TONE_ERROR_TUNE - 1] = "MBT200a8a8a8PaaaP"; // ERROR tone + _default_tunes[TONE_NOTIFY_POSITIVE_TUNE - 1] = "MFT200e8a8a"; // NotifyPositive tone + _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE - 1] = "MFT200e8e"; // NotifyNeutral tone + _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE - 1] = "MFT200e8c8e8c8e8c8"; // NotifyNegative tone + _default_tunes[TONE_CHARGE_TUNE - 1] = + "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! +#if 0 // don't include unused tunes... but keep them for nostalgic reason + _default_tunes[TONE_DIXIE_TUNE - 1] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie + _default_tunes[TONE_CUCURACHA_TUNE - 1] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha + _default_tunes[TONE_YANKEE_TUNE - 1] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee + _default_tunes[TONE_DAISY_TUNE - 1] = + "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8"; // daisy + _default_tunes[TONE_WILLIAM_TELL_TUNE - 1] = + "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell + "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" + "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" + "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" + "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" + "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" + "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" + "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" + "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" + "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" + "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" + "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" + "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" + "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" + "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" + "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" + "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" + "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" + "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" + "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" + "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" + "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" + "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" + "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" + "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" + "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" + "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" + "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" + "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" + "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" + "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" + "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" + "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" + "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" + "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" + "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" + "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" + "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" + "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" + "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" + "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" + "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" + "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" + "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" + "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" + "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" + "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" + "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" + "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" + "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" + "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" + "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" + "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." + "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" + "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" + "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" + "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" + "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" + "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" + "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" + "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" + "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" + "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" + "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" + "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" + "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" + "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" + "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" + "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" + "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" + "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" + "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" + "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" + "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" + "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" + "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" + "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" + "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" + "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" + "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" + "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" + "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" + "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" + "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" + "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" + "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" + "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" + "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" + "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" + "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" + "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" + "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" + "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" + "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" + "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" + "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" + "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" + "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" + "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" + "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" + "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" + "O2E2P64"; +#endif + _default_tunes[TONE_ARMING_WARNING_TUNE - 1] = "MNT75L1O2G"; //arming warning + _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE - 1] = "MBNT100a8"; //battery warning slow + _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE - 1] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast } ToneAlarm::~ToneAlarm() @@ -873,7 +874,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) case TONE_SET_ALARM: debug("TONE_SET_ALARM %u", arg); - if (arg <= _default_ntunes) { + if (arg <= TONE_NUMBER_OF_TUNES) { if (arg == 0) { // stop the tune _tune = nullptr; @@ -1008,10 +1009,10 @@ tone_alarm_main(int argc, char *argv[]) } if ((argc > 1) && !strcmp(argv[1], "start")) - play_tune(1); + play_tune(TONE_STARTUP_TUNE); if ((argc > 1) && !strcmp(argv[1], "stop")) - play_tune(0); + play_tune(TONE_STOP_TUNE); if ((tune = strtol(argv[1], nullptr, 10)) != 0) play_tune(tune); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 5df5d8d0c3..7feace2b4b 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -99,44 +99,44 @@ void buzzer_deinit() void tune_error() { - ioctl(buzzer, TONE_SET_ALARM, 2); + ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); } void tune_positive() { - ioctl(buzzer, TONE_SET_ALARM, 3); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); } void tune_neutral() { - ioctl(buzzer, TONE_SET_ALARM, 4); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } void tune_negative() { - ioctl(buzzer, TONE_SET_ALARM, 5); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } int tune_arm() { - return ioctl(buzzer, TONE_SET_ALARM, 12); + return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE); } int tune_low_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 13); + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE); } int tune_critical_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 14); + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); } void tune_stop() { - ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); } static int leds; diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 4c19dcffb6..a323261cc1 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -176,15 +176,15 @@ system_eval: led_toggle(leds, LED_AMBER); if (i % 10 == 0) { - ioctl(buzzer, TONE_SET_ALARM, 4); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } else if (i % 5 == 0) { - ioctl(buzzer, TONE_SET_ALARM, 2); + ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); } usleep(100000); } /* stop alarm */ - ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); /* switch on leds */ led_on(leds, LED_BLUE); diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index f6e540401f..ba6b86adba 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -137,7 +137,7 @@ int test_tone(int argc, char *argv[]) tone = atoi(argv[1]); if (tone == 0) { - result = ioctl(fd, TONE_SET_ALARM, 0); + result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); @@ -148,7 +148,7 @@ int test_tone(int argc, char *argv[]) } } else { - result = ioctl(fd, TONE_SET_ALARM, 0); + result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); From f246b68c7b53c44ae00e0f3fca724d1fbfab8f5d Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Fri, 30 Aug 2013 20:24:14 -0400 Subject: [PATCH 34/75] Fix parameter range check --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index a726247d55..262fe6e4c2 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -874,7 +874,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) case TONE_SET_ALARM: debug("TONE_SET_ALARM %u", arg); - if (arg <= TONE_NUMBER_OF_TUNES) { + if (arg < TONE_NUMBER_OF_TUNES) { if (arg == 0) { // stop the tune _tune = nullptr; From b80f0d46ae97aaa4038958ff97165e0c154b745c Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sat, 31 Aug 2013 01:03:32 -0400 Subject: [PATCH 35/75] Allow tone_alarm cmd to use tone names as well as number - remove script dependency on hard coded tone numbers - fix bug restarting repeating tone after stop --- ROMFS/px4fmu_common/init.d/rc.io | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- src/drivers/drv_tone_alarm.h | 2 +- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 86 +++++++++++++-------- 4 files changed, 56 insertions(+), 36 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 85f00e582e..aaf91b3166 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -19,5 +19,5 @@ then fi else # SOS - tone_alarm 6 + tone_alarm error fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 982e638fb5..b1648daa7d 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -20,7 +20,7 @@ then else echo "[init] no microSD card found" # Play SOS - tone_alarm 2 + tone_alarm error fi # diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 4a399076d4..f0b8606200 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -132,8 +132,8 @@ enum { TONE_NOTIFY_POSITIVE_TUNE, TONE_NOTIFY_NEUTRAL_TUNE, TONE_NOTIFY_NEGATIVE_TUNE, - TONE_CHARGE_TUNE, /* Do not include these unused tunes + TONE_CHARGE_TUNE, TONE_DIXIE_TUNE, TONE_CUCURACHA_TUNE, TONE_YANKEE_TUNE, diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 262fe6e4c2..d181065418 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -230,10 +230,14 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); + inline const char *name(int tune) { + return _tune_names[tune]; + } private: static const unsigned _tune_max = 1024; // be reasonable about user tunes - static const char * _default_tunes[TONE_NUMBER_OF_TUNES - 1]; + const char * _default_tunes[TONE_NUMBER_OF_TUNES]; + const char * _tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -303,9 +307,6 @@ private: }; -// predefined tune array -const char * ToneAlarm::_default_tunes[TONE_NUMBER_OF_TUNES - 1]; - // semitone offsets from C for the characters 'A'-'G' const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; @@ -324,20 +325,20 @@ ToneAlarm::ToneAlarm() : { // enable debug() calls //_debug_enabled = true; - _default_tunes[TONE_STARTUP_TUNE - 1] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune - _default_tunes[TONE_ERROR_TUNE - 1] = "MBT200a8a8a8PaaaP"; // ERROR tone - _default_tunes[TONE_NOTIFY_POSITIVE_TUNE - 1] = "MFT200e8a8a"; // NotifyPositive tone - _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE - 1] = "MFT200e8e"; // NotifyNeutral tone - _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE - 1] = "MFT200e8c8e8c8e8c8"; // NotifyNegative tone - _default_tunes[TONE_CHARGE_TUNE - 1] = - "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! + _default_tunes[TONE_STARTUP_TUNE] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune + _default_tunes[TONE_ERROR_TUNE] = "MBT200a8a8a8PaaaP"; // ERROR tone + _default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone + _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone + _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone #if 0 // don't include unused tunes... but keep them for nostalgic reason - _default_tunes[TONE_DIXIE_TUNE - 1] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie - _default_tunes[TONE_CUCURACHA_TUNE - 1] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha - _default_tunes[TONE_YANKEE_TUNE - 1] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee - _default_tunes[TONE_DAISY_TUNE - 1] = + _default_tunes[TONE_CHARGE_TUNE] = + "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! + _default_tunes[TONE_DIXIE_TUNE] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie + _default_tunes[TONE_CUCURACHA_TUNE] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha + _default_tunes[TONE_YANKEE_TUNE] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee + _default_tunes[TONE_DAISY_TUNE] = "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8"; // daisy - _default_tunes[TONE_WILLIAM_TELL_TUNE - 1] = + _default_tunes[TONE_WILLIAM_TELL_TUNE] = "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" @@ -477,9 +478,18 @@ ToneAlarm::ToneAlarm() : "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" "O2E2P64"; #endif - _default_tunes[TONE_ARMING_WARNING_TUNE - 1] = "MNT75L1O2G"; //arming warning - _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE - 1] = "MBNT100a8"; //battery warning slow - _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE - 1] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning + _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow + _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + + _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune + _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone + _tune_names[TONE_NOTIFY_POSITIVE_TUNE] = "positive"; // Notify Positive tone + _tune_names[TONE_NOTIFY_NEUTRAL_TUNE] = "neutral"; // Notify Neutral tone + _tune_names[TONE_NOTIFY_NEGATIVE_TUNE] = "negative"; // Notify Negative tone + _tune_names[TONE_ARMING_WARNING_TUNE] = "arming"; // arming warning + _tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat"; // battery warning slow + _tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast } ToneAlarm::~ToneAlarm() @@ -875,16 +885,18 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) debug("TONE_SET_ALARM %u", arg); if (arg < TONE_NUMBER_OF_TUNES) { - if (arg == 0) { + if (arg == TONE_STOP_TUNE) { // stop the tune _tune = nullptr; _next = nullptr; + _repeat = false; + _default_tune_number = 0; } else { /* always interrupt alarms, unless they are repeating and already playing */ if (!(_repeat && _default_tune_number == arg)) { /* play the selected tune */ _default_tune_number = arg; - start_tune(_default_tunes[arg - 1]); + start_tune(_default_tunes[arg]); } } } else { @@ -1008,22 +1020,30 @@ tone_alarm_main(int argc, char *argv[]) } } - if ((argc > 1) && !strcmp(argv[1], "start")) - play_tune(TONE_STARTUP_TUNE); + if (argc > 1) { + if (!strcmp(argv[1], "start")) + play_tune(TONE_STARTUP_TUNE); - if ((argc > 1) && !strcmp(argv[1], "stop")) - play_tune(TONE_STOP_TUNE); + if (!strcmp(argv[1], "stop")) + play_tune(TONE_STOP_TUNE); - if ((tune = strtol(argv[1], nullptr, 10)) != 0) - play_tune(tune); + if ((tune = strtol(argv[1], nullptr, 10)) != 0) + play_tune(tune); - /* if it looks like a PLAY string... */ - if (strlen(argv[1]) > 2) { - const char *str = argv[1]; - if (str[0] == 'M') { - play_string(str); + /* It might be a tune name */ + for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) + if (!strcmp(g_dev->name(tune), argv[1])) + play_tune(tune); + + /* if it looks like a PLAY string... */ + if (strlen(argv[1]) > 2) { + const char *str = argv[1]; + if (str[0] == 'M') { + play_string(str); + } } + } - errx(1, "unrecognised command, try 'start', 'stop' or an alarm number"); + errx(1, "unrecognised command, try 'start', 'stop' or an alarm number or name"); } From 7292e8c7220eb473fef5eb3a95a11fefd1d1e7e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:21:57 +0200 Subject: [PATCH 36/75] Hotfix for mavlink logbuffer, needs another round of validation. --- src/modules/mavlink/mavlink.c | 5 ++++- src/modules/systemlib/mavlink_log.c | 8 ++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index d7b0fa9c7e..78e01cefbf 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,7 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 5); + mavlink_logbuffer_init(&lb, 2); int ch; char *device_name = "/dev/ttyS1"; @@ -738,6 +738,9 @@ int mavlink_thread_main(int argc, char *argv[]) /* Reset the UART flags to original state */ tcsetattr(uart, TCSANOW, &uart_config_original); + /* destroy log buffer */ + mavlink_logbuffer_destroy(&lb); + thread_running = false; exit(0); diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 27608bdbfe..03ca71375f 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -54,6 +54,14 @@ __EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); } +__EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb) +{ + lb->size = 0; + lb->start = 0; + lb->count = 0; + free(lb->elems); +} + __EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { return lb->count == (int)lb->size; From acc06e7eb1dafa8b4bc22b34c2b2c567d67e6b37 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:22:15 +0200 Subject: [PATCH 37/75] Added logbuffer free function --- src/include/mavlink/mavlink_log.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index a28ff3a687..5054937e07 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -107,7 +107,6 @@ struct mavlink_logmessage { struct mavlink_logbuffer { unsigned int start; - // unsigned int end; unsigned int size; int count; struct mavlink_logmessage *elems; @@ -115,6 +114,8 @@ struct mavlink_logbuffer { void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); +void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb); + int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); From 669a8029212a715118bf380a524fb7ced64ccacc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:22:41 +0200 Subject: [PATCH 38/75] Hotfix: Better PX4IO detection feedbak --- src/drivers/px4io/px4io.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 47baa5770a..c88abe59a8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -497,7 +497,13 @@ PX4IO::detect() /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); if (protocol != PX4IO_PROTOCOL_VERSION) { - log("IO not installed"); + if (protocol == _io_reg_get_error) { + log("IO not installed"); + } else { + log("IO version error"); + mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); + } + return -1; } log("IO found"); @@ -914,7 +920,6 @@ PX4IO::set_max_values(const uint16_t *vals, unsigned len) int PX4IO::set_idle_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; if (len > _max_actuators) /* fail with error */ @@ -1612,7 +1617,7 @@ PX4IO::print_status() } printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); printf("\nidle values"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); @@ -2233,7 +2238,7 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 1500. */ uint16_t failsafe[8]; - for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) { + for (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) { /* set channel to commandline argument or to 900 for non-provided channels */ if (argc > i + 2) { @@ -2265,7 +2270,7 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 900. */ uint16_t min[8]; - for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++) + for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++) { /* set channel to commanline argument or to 900 for non-provided channels */ if (argc > i + 2) { @@ -2300,7 +2305,7 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 2100. */ uint16_t max[8]; - for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++) + for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) { /* set channel to commanline argument or to 2100 for non-provided channels */ if (argc > i + 2) { @@ -2335,7 +2340,7 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 0. */ uint16_t idle[8]; - for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) + for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) { /* set channel to commanline argument or to 0 for non-provided channels */ if (argc > i + 2) { From 56975e0dd1e68ed34194b86eb5f82c51ae648391 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:23:03 +0200 Subject: [PATCH 39/75] Hotfixed position control param update --- .../multirotor_pos_control.c | 61 +++++++++---------- 1 file changed, 30 insertions(+), 31 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 8227f76c5e..a25448af2b 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -252,36 +252,35 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - int paramcheck_counter = 0; - while (!thread_should_exit) { - /* check parameters at 1 Hz */ - if (++paramcheck_counter >= 50) { - paramcheck_counter = 0; - bool param_updated; - orb_check(param_sub, ¶m_updated); - if (param_updated) { - parameters_update(¶ms_h, ¶ms); + bool param_updated; + orb_check(param_sub, ¶m_updated); - for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - /* use integral_limit_out = tilt_max / 2 */ - float i_limit; + if (param_updated) { + /* clear updated flag */ + struct parameter_update_s ps; + orb_copy(ORB_ID(parameter_update), param_sub, &ps); + /* update params */ + parameters_update(¶ms_h, ¶ms); - if (params.xy_vel_i == 0.0) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0; + for (int i = 0; i < 2; i++) { + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); + /* use integral_limit_out = tilt_max / 2 */ + float i_limit; - } else { - i_limit = 1.0f; // not used really - } + if (params.xy_vel_i == 0.0f) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); + } else { + i_limit = 1.0f; // not used really } - pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); - thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } + + pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); + thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } bool updated; @@ -351,7 +350,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double)-local_pos_sp.z); } /* move altitude setpoint with throttle stick */ @@ -377,7 +376,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.y = local_pos.y; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } /* move position setpoint with roll/pitch stick */ @@ -429,7 +428,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double)-local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { @@ -444,7 +443,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) double lat_home = local_pos.ref_lat * 1e-7; double lon_home = local_pos.ref_lon * 1e-7; map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); } if (global_pos_sp_reproject) { @@ -468,7 +467,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.yaw = global_pos_sp.yaw; att_sp.yaw_body = global_pos_sp.yaw; - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); } else { if (!local_pos_sp_valid) { @@ -481,7 +480,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp_valid = true; } - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } @@ -505,7 +504,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* set altitude setpoint to 5m under ground, * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double)-local_pos_sp.z); } reset_sp_z = true; @@ -515,7 +514,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double)-local_pos_sp.z); } } @@ -527,7 +526,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } } @@ -588,7 +587,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } thrust_pid_set_integral(&z_vel_pid, -i); - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i); + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); From 14cfb53534265cae8690734f2b0d98b8bbf34da5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:23:24 +0200 Subject: [PATCH 40/75] Hotfix: Correct frame print string --- ROMFS/px4fmu_common/init.d/11_dji_f450 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index fb141e8a73..1827a2c114 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -1,6 +1,6 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" +echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" # # Load default params for this platform From fb4ca82b84c5c49a968cdaf7a9fa8098a9ed9e15 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:33:57 +0200 Subject: [PATCH 41/75] Hotfix: Cleanup on startup scripts --- ROMFS/px4fmu_common/init.d/rc.sensors | 8 -------- ROMFS/px4fmu_common/init.d/rc.usb | 8 ++++++++ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 61bb097281..2828a108bf 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -7,14 +7,6 @@ # Start sensor drivers here. # -# -# Check for UORB -# -if uorb start -then - echo "uORB started" -fi - ms5611 start adc start diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index abeed0ca3c..9264985d6d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -8,6 +8,14 @@ echo "Starting MAVLink on this USB console" # Stop tone alarm tone_alarm stop +# +# Check for UORB +# +if uorb start +then + echo "uORB started" +fi + # Tell MAVLink that this link is "fast" if mavlink stop then From 87f7ebdd75ac11139f7b1ae568590ab1eabbd45c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:38:29 +0200 Subject: [PATCH 42/75] Hotfix: Rely on gyro calibration --- .../attitude_estimator_ekf_main.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 65abcde1e3..a70a14fe4b 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -308,18 +308,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); if (!initialized) { + // XXX disabling init for now + initialized = true; - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; + // gyro_offsets[0] += raw.gyro_rad_s[0]; + // gyro_offsets[1] += raw.gyro_rad_s[1]; + // gyro_offsets[2] += raw.gyro_rad_s[2]; + // offset_count++; - if (hrt_absolute_time() - start_time > 3000000LL) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - } + // if (hrt_absolute_time() - start_time > 3000000LL) { + // initialized = true; + // gyro_offsets[0] /= offset_count; + // gyro_offsets[1] /= offset_count; + // gyro_offsets[2] /= offset_count; + // } } else { From 3f4315b4767ff221936e135b3252794a952f2b95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:49:36 +0200 Subject: [PATCH 43/75] Hotfix: Increase stack size for low prio commander task --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8ddd86d03f..45e0307e6a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -594,7 +594,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; /* low priority */ From ccc5bef3af2852172b18f33edaffb809a0a1ffcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:56:39 +0200 Subject: [PATCH 44/75] Hotfix for IO-less systems --- ROMFS/px4fmu_common/init.d/rcS | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b1648daa7d..9bb8e4a494 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -115,14 +115,18 @@ then echo "PX4IO running, not upgrading" else echo "Attempting to upgrade PX4IO" - px4io update - if [ -d /fs/microsd ] + if px4io update then - echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log - fi + if [ -d /fs/microsd ] + then + echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log + fi - # Allow IO to safely kick back to app - usleep 200000 + # Allow IO to safely kick back to app + usleep 200000 + else + echo "No PX4IO to upgrade here" + fi fi # From 54644e4206ffb40764f8f00e5ec41d98f54104a1 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sat, 31 Aug 2013 16:06:15 -0400 Subject: [PATCH 45/75] Allow tone_alarm cmd to take filename as parameter - Restore ability to play beloved William Tell Overture... from file --- misc/tones/charge.txt | 1 + misc/tones/cucuracha.txt | 1 + misc/tones/daisy.txt | 1 + misc/tones/dixie.txt | 1 + misc/tones/tell.txt | 1 + misc/tones/yankee.txt | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 193 ++++---------------- 7 files changed, 40 insertions(+), 159 deletions(-) create mode 100644 misc/tones/charge.txt create mode 100644 misc/tones/cucuracha.txt create mode 100644 misc/tones/daisy.txt create mode 100644 misc/tones/dixie.txt create mode 100644 misc/tones/tell.txt create mode 100644 misc/tones/yankee.txt diff --git a/misc/tones/charge.txt b/misc/tones/charge.txt new file mode 100644 index 0000000000..cf33c58b3e --- /dev/null +++ b/misc/tones/charge.txt @@ -0,0 +1 @@ +MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4 \ No newline at end of file diff --git a/misc/tones/cucuracha.txt b/misc/tones/cucuracha.txt new file mode 100644 index 0000000000..dce328c03b --- /dev/null +++ b/misc/tones/cucuracha.txt @@ -0,0 +1 @@ +MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8 \ No newline at end of file diff --git a/misc/tones/daisy.txt b/misc/tones/daisy.txt new file mode 100644 index 0000000000..e39410621c --- /dev/null +++ b/misc/tones/daisy.txt @@ -0,0 +1 @@ +MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8 \ No newline at end of file diff --git a/misc/tones/dixie.txt b/misc/tones/dixie.txt new file mode 100644 index 0000000000..f3ddd19ed3 --- /dev/null +++ b/misc/tones/dixie.txt @@ -0,0 +1 @@ +MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16 \ No newline at end of file diff --git a/misc/tones/tell.txt b/misc/tones/tell.txt new file mode 100644 index 0000000000..ea77d1a245 --- /dev/null +++ b/misc/tones/tell.txt @@ -0,0 +1 @@ +T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8 P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8 O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16 O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4 O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8 O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16 O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8 O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16 O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8 O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8 O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8 O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8 O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8 O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16 O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16 O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8 O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8 O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16 O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16 O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16 O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16 O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8 O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8 O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16 O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64 O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16 O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16 O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16 O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16 O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16 O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16 O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16 O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16 O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16 O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16 O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16 O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16 O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16 O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16 O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16 O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16 O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16 O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16 O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16 O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16 O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16 O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16 O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16 O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16 O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16 O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16 O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16 O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16 O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16 O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16 O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16 O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16 O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16 O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16 O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16 O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16 O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16 O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16 O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16 O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16 O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8 O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16 O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8 O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8 O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8 O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16 O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8 O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8 O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8 O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8 O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16 O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16 O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8 O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8 O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16 O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16 O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16 O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16 O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16 O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16 O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16 P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16 P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16 O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8 O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16 O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16 O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16 O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16 O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16 P16O4E16P16D+16P16C+16P16O3B16P16A+16P16 O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16 O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16 O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16 O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16 O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16 P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16 O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16 O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16 O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16 O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8 O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16 O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16 O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8 O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16 O4E16P16D+16P16C+16P16O3B16P16A+16P16A16 P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16 O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8 O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8 O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16 O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16 O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16 O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16 O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16 O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8 O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16 O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16 O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8 O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8 O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8 O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16 O2E2P64 \ No newline at end of file diff --git a/misc/tones/yankee.txt b/misc/tones/yankee.txt new file mode 100644 index 0000000000..3677b39c17 --- /dev/null +++ b/misc/tones/yankee.txt @@ -0,0 +1 @@ +MNT150L8O2GGABGBADGGABL4GL8F+ \ No newline at end of file diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index d181065418..a582ece174 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -235,7 +235,7 @@ public: } private: - static const unsigned _tune_max = 1024; // be reasonable about user tunes + static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes const char * _default_tunes[TONE_NUMBER_OF_TUNES]; const char * _tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; @@ -330,154 +330,6 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone -#if 0 // don't include unused tunes... but keep them for nostalgic reason - _default_tunes[TONE_CHARGE_TUNE] = - "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! - _default_tunes[TONE_DIXIE_TUNE] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie - _default_tunes[TONE_CUCURACHA_TUNE] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha - _default_tunes[TONE_YANKEE_TUNE] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee - _default_tunes[TONE_DAISY_TUNE] = - "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8"; // daisy - _default_tunes[TONE_WILLIAM_TELL_TUNE] = - "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell - "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" - "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" - "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" - "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" - "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" - "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" - "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" - "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" - "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" - "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" - "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" - "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" - "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" - "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" - "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" - "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" - "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" - "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" - "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" - "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" - "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" - "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" - "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" - "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" - "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" - "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" - "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" - "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" - "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" - "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" - "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" - "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" - "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" - "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" - "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" - "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" - "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" - "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" - "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" - "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" - "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" - "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" - "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" - "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" - "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" - "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" - "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" - "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" - "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" - "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" - "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" - "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." - "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" - "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" - "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" - "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" - "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" - "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" - "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" - "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" - "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" - "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" - "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" - "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" - "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" - "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" - "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" - "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" - "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" - "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" - "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" - "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" - "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" - "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" - "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" - "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" - "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" - "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" - "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" - "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" - "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" - "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" - "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" - "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" - "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" - "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" - "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" - "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" - "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" - "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" - "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" - "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" - "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" - "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" - "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" - "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" - "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" - "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" - "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" - "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" - "O2E2P64"; -#endif _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast @@ -983,7 +835,7 @@ play_tune(unsigned tune) } int -play_string(const char *str) +play_string(const char *str, bool free_buffer) { int fd, ret; @@ -995,6 +847,9 @@ play_string(const char *str) ret = write(fd, str, strlen(str) + 1); close(fd); + if (free_buffer) + free((void *)str); + if (ret < 0) err(1, "play tune"); exit(0); @@ -1021,29 +876,49 @@ tone_alarm_main(int argc, char *argv[]) } if (argc > 1) { - if (!strcmp(argv[1], "start")) + const char *argv1 = argv[1]; + + if (!strcmp(argv1, "start")) play_tune(TONE_STARTUP_TUNE); - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv1, "stop")) play_tune(TONE_STOP_TUNE); - if ((tune = strtol(argv[1], nullptr, 10)) != 0) + if ((tune = strtol(argv1, nullptr, 10)) != 0) play_tune(tune); /* It might be a tune name */ for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) - if (!strcmp(g_dev->name(tune), argv[1])) + if (!strcmp(g_dev->name(tune), argv1)) play_tune(tune); + /* If it is a file name then load and play it as a string */ + if (*argv1 == '/') { + FILE *fd = fopen(argv1, "r"); + int sz; + char *buffer; + if (fd == nullptr) + errx(1, "couldn't open '%s'", argv1); + fseek(fd, 0, SEEK_END); + sz = ftell(fd); + fseek(fd, 0, SEEK_SET); + buffer = (char *)malloc(sz + 1); + if (buffer == nullptr) + errx(1, "not enough memory memory"); + fread(buffer, sz, 1, fd); + /* terminate the string */ + buffer[sz] = 0; + play_string(buffer, true); + } + /* if it looks like a PLAY string... */ - if (strlen(argv[1]) > 2) { - const char *str = argv[1]; - if (str[0] == 'M') { - play_string(str); + if (strlen(argv1) > 2) { + if (*argv1 == 'M') { + play_string(argv1, false); } } } - errx(1, "unrecognised command, try 'start', 'stop' or an alarm number or name"); + errx(1, "unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); } From a1c4c0fd787401f8ed332fa974a88a74ae079383 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 00:09:59 +0200 Subject: [PATCH 46/75] Comments on scaling factors --- src/modules/sensors/sensor_params.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bd431c9ebd..6c8e514b67 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -159,6 +159,8 @@ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ +/* PX4IOAR: 0.00838095238 */ +/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); From c3408332fd0e6d77661a26fade8662a8b9be9970 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 10:29:12 +0200 Subject: [PATCH 47/75] Added test to test unlink() --- src/systemcmds/tests/tests_file.c | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 6f75b9812e..47f480758c 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -76,9 +76,39 @@ test_file(int argc, char *argv[]) close(fd); + unlink("/fs/microsd/testfile"); + warnx("512KiB in %llu microseconds", end - start); perf_print_counter(wperf); perf_free(wperf); + warnx("running unlink test"); + + /* ensure that common commands do not run against file count limits */ + for (unsigned i = 0; i < 64; i++) { + + warnx("unlink iteration #%u", i); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + if (fd < 0) + errx(1, "failed opening test file before unlink()"); + int ret = write(fd, buf, sizeof(buf)); + if (ret < 0) + errx(1, "failed writing test file before unlink()"); + close(fd); + + ret = unlink("/fs/microsd/testfile"); + if (ret != OK) + errx(1, "failed unlinking test file"); + + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + if (fd < 0) + errx(1, "failed opening test file after unlink()"); + ret = write(fd, buf, sizeof(buf)); + if (ret < 0) + errx(1, "failed writing test file after unlink()"); + close(fd); + } + return 0; } From f6bf1c7bf2d83bfc70fb8c4b9f589b25bcccc5e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 10:29:30 +0200 Subject: [PATCH 48/75] Closing files that should be closed --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 45e0307e6a..8e8226f091 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -319,6 +319,8 @@ void print_status() break; } + close(state_sub); + warnx("arming: %s", armed_str); } @@ -1764,5 +1766,7 @@ void *commander_low_prio_loop(void *arg) } + close(cmd_sub); + return 0; } From 9eff3170a3aa63d17fbaefd39619866fb745b237 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 10:30:04 +0200 Subject: [PATCH 49/75] More verbosity on RC check --- src/modules/systemlib/rc_check.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 9d47d8000d..f3a2aee9eb 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -112,13 +112,13 @@ int rc_calibration_check(void) { } if (param_trim < param_min) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim > param_max) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max); /* give system time to flush error message in case there are more */ usleep(100000); } From 2d83c6f825db02f04624467f3d0b492ee371c72a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 12:47:10 +0200 Subject: [PATCH 50/75] Closing all opened file descriptors, fixed param save issue, tests clean --- .../commander/accelerometer_calibration.cpp | 4 +- .../commander/airspeed_calibration.cpp | 9 ++-- src/modules/commander/gyro_calibration.cpp | 20 +++++---- src/modules/commander/mag_calibration.cpp | 44 +++++-------------- src/modules/commander/rc_calibration.cpp | 3 ++ src/modules/systemlib/param/param.c | 11 ++++- src/modules/systemlib/rc_check.c | 7 ++- .../preflight_check/preflight_check.c | 7 +++ 8 files changed, 57 insertions(+), 48 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ed6707f045..cfa7d9e8a1 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -241,8 +241,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; int orient = detect_orientation(mavlink_fd, sensor_combined_sub); - if (orient < 0) + if (orient < 0) { + close(sensor_combined_sub); return ERROR; + } if (data_collected[orient]) { mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index e414e5f707..248eb4a66e 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -85,6 +85,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + close(diff_pres_sub); return ERROR; } } @@ -95,6 +96,7 @@ int do_airspeed_calibration(int mavlink_fd) if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + close(diff_pres_sub); return ERROR; } @@ -104,18 +106,17 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + close(diff_pres_sub); return ERROR; } - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - + close(diff_pres_sub); return OK; } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + close(diff_pres_sub); return ERROR; } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 33566d4e53..82a0349c9e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -60,12 +60,12 @@ int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); - const int calibration_count = 5000; + const unsigned calibration_count = 5000; int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; - int calibration_counter = 0; + unsigned calibration_counter = 0; float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; /* set offsets to zero */ @@ -101,6 +101,8 @@ int do_gyro_calibration(int mavlink_fd) gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[2] += raw.gyro_rad_s[2]; calibration_counter++; + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); } else { poll_errcount++; @@ -108,6 +110,7 @@ int do_gyro_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); + close(sub_sensor_combined); return ERROR; } } @@ -147,24 +150,23 @@ int do_gyro_calibration(int mavlink_fd) if (save_ret != 0) { warnx("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, "gyro store failed"); + close(sub_sensor_combined); return ERROR; } - mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_neutral(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); + close(sub_sensor_combined); return ERROR; } /*** --- SCALING --- ***/ - mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x"); - mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); + mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); unsigned rotations_count = 30; @@ -187,8 +189,8 @@ int do_gyro_calibration(int mavlink_fd) /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) && (hrt_absolute_time() - start_time > 5 * 1e6)) { - mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); - mavlink_log_info(mavlink_fd, "gyro calibration done"); + mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); + close(sub_sensor_combined); return OK; } @@ -281,9 +283,11 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "gyro calibration done"); /* third beep by cal end routine */ + close(sub_sensor_combined); return OK; } else { mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + close(sub_sensor_combined); return ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index e386160271..b0d4266be5 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -142,7 +142,6 @@ int do_mag_calibration(int mavlink_fd) axis_index++; mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); axis_deadline += calibration_interval / 3; @@ -152,14 +151,6 @@ int do_mag_calibration(int mavlink_fd) break; } - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { @@ -169,30 +160,10 @@ int do_mag_calibration(int mavlink_fd) y[calibration_counter] = mag.y; z[calibration_counter] = mag.z; - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - calibration_counter++; + if (calibration_counter % (calibration_maxcount / 20) == 0) + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount); + } else { poll_errcount++; @@ -200,6 +171,10 @@ int do_mag_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); + close(sub_mag); + free(x); + free(y); + free(z); return ERROR; } @@ -211,7 +186,9 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; + mavlink_log_info(mavlink_fd, "mag cal progress <70> percent"); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + mavlink_log_info(mavlink_fd, "mag cal progress <80> percent"); free(x); free(y); @@ -269,6 +246,7 @@ int do_mag_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + close(sub_mag); return ERROR; } @@ -288,11 +266,13 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "magnetometer calibration completed"); mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); + close(sub_mag); return OK; /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + close(sub_mag); return ERROR; } } diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index fe87a3323b..9fc1d64700 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -40,6 +40,7 @@ #include "commander_helper.h" #include +#include #include #include #include @@ -80,9 +81,11 @@ int do_rc_calibration(int mavlink_fd) if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + close(sub_man); return ERROR; } mavlink_log_info(mavlink_fd, "trim calibration done"); + close(sub_man); return OK; } diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 69a9bdf9b5..24b52d1a92 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -505,8 +505,15 @@ param_get_default_file(void) int param_save_default(void) { + int result; + /* delete the file in case it exists */ - unlink(param_get_default_file()); + struct stat buffer; + if (stat(param_get_default_file(), &buffer) == 0) { + result = unlink(param_get_default_file()); + if (result != OK) + warnx("unlinking file %s failed.", param_get_default_file()); + } /* create the file */ int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); @@ -516,7 +523,7 @@ param_save_default(void) return -1; } - int result = param_export(fd, false); + result = param_export(fd, false); close(fd); if (result != 0) { diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index f3a2aee9eb..60d6473b8c 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -66,7 +66,7 @@ int rc_calibration_check(void) { // count++; // } - + int channel_fail_count = 0; for (int i = 0; i < RC_CHANNELS_MAX; i++) { /* should the channel be enabled? */ @@ -142,7 +142,12 @@ int rc_calibration_check(void) { /* sanity checks pass, enable channel */ if (count) { mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); usleep(100000); } + + channel_fail_count += count; } + + return channel_fail_count; } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index a323261cc1..f5bd01ce8c 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -142,6 +142,10 @@ int preflight_check_main(int argc, char *argv[]) bool rc_ok = (OK == rc_calibration_check()); + /* warn */ + if (!rc_ok) + warnx("rc calibration test failed"); + /* require RC ok to keep system_ok */ system_ok &= rc_ok; @@ -156,6 +160,9 @@ system_eval: } else { fflush(stdout); + warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); + fflush(stderr); + int buzzer = open("/dev/tone_alarm", O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); From f0a750714ae07976b295c0a857d85cbd57a09a72 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 1 Sep 2013 14:12:48 +0200 Subject: [PATCH 51/75] pid.c: fixed bad merge of seatbelt_multirotor, should fix EASY/AUTO modes --- src/modules/systemlib/pid/pid.c | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 4996a8f661..562f3ac04b 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -168,8 +168,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabsf(i) > pid->intmax) { + if ((pid->limit != 0.0f && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + fabsf(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; @@ -186,11 +186,13 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (isfinite(output)) { - if (output > pid->limit) { - output = pid->limit; + if (pid->limit != 0.0f) { + if (output > pid->limit) { + output = pid->limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->limit) { + output = -pid->limit; + } } pid->last_output = output; From ff23feb24e2b8a9ad0bfc8fb1556939df5512310 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 15:32:40 +0200 Subject: [PATCH 52/75] Hotfix: Chasing log buffer issue --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 78e01cefbf..a8ca19d7a4 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,7 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 2); + mavlink_logbuffer_init(&lb, 10); int ch; char *device_name = "/dev/ttyS1"; From e553087d10e7187c12e5e0f28937585a732a46da Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 1 Sep 2013 20:17:24 +0200 Subject: [PATCH 53/75] pid.limit != 0 fix --- src/modules/systemlib/pid/pid.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 562f3ac04b..739503ed41 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -51,6 +51,8 @@ #include "pid.h" #include +#define SIGMA 0.000001f + __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min) { @@ -168,7 +170,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if ((pid->limit != 0.0f && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || fabsf(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; @@ -186,7 +188,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (isfinite(output)) { - if (pid->limit != 0.0f) { + if (pid->limit > SIGMA) { if (output > pid->limit) { output = pid->limit; From fddaa49ed8fadb5f2574225f121d6d8aa8654406 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 1 Sep 2013 22:44:58 +0200 Subject: [PATCH 54/75] Make sure the unit tests aren't included as part of the standard firmware --- makefiles/config_px4fmu-v1_default.mk | 4 ++-- makefiles/config_px4fmu-v2_default.mk | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index fff43c6b47..3e94c35344 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -94,8 +94,8 @@ MODULES += modules/sdlog2 # # Unit tests # -MODULES += modules/unit_test -MODULES += modules/commander/commander_tests +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests # # Library modules diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ab0da35596..bd73bde5ea 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -88,8 +88,8 @@ MODULES += modules/sdlog2 # # Unit tests # -MODULES += modules/unit_test -MODULES += modules/commander/commander_tests +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests # # Library modules From 589ae937ee7afe5903f98f4eea25a26a79aca8f1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Sep 2013 17:55:27 +0200 Subject: [PATCH 55/75] Allow mixer upload when PWM is on --- src/modules/px4iofirmware/mixer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index deed258364..0edd91b243 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -358,8 +358,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while outputs armed */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + /* do not allow a mixer change while safety off */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { return; } From 027d45acbfa06dc155f6a18d0288fb8230d05c9f Mon Sep 17 00:00:00 2001 From: tstellanova Date: Mon, 2 Sep 2013 09:13:36 -0700 Subject: [PATCH 56/75] respect NAV_TAKEOFF_ALT instead of using hardcoded default takeoff value --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 5 +++-- .../multirotor_pos_control/multirotor_pos_control_params.c | 2 ++ .../multirotor_pos_control/multirotor_pos_control_params.h | 2 ++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a25448af2b..c194f627d0 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -227,7 +227,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - const float takeoff_alt_default = 10.0f; + float ref_alt = 0.0f; hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; @@ -244,6 +244,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); @@ -423,7 +424,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - local_pos_sp.z = -takeoff_alt_default; + local_pos_sp.z = -params.takeoff_alt; local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 9c1ef2edb3..4d65118511 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -59,6 +59,7 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { + h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); h->z_p = param_find("MPC_Z_P"); @@ -84,6 +85,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h) int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { + param_get(h->takeoff_alt,&(p->takeoff_alt)); param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); param_get(h->z_p, &(p->z_p)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 3ec85a3641..79bc9b72bf 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -41,6 +41,7 @@ #include struct multirotor_position_control_params { + float takeoff_alt; float thr_min; float thr_max; float z_p; @@ -63,6 +64,7 @@ struct multirotor_position_control_params { }; struct multirotor_position_control_param_handles { + param_t takeoff_alt; param_t thr_min; param_t thr_max; param_t z_p; From 193a52d8132ce03aa0de9547f77ca8aeb67220f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 2 Sep 2013 23:13:01 +0200 Subject: [PATCH 57/75] multirotor_pos_control: added takeoff gap (hardcoded 3m), fixed code style --- .../multirotor_pos_control/multirotor_pos_control.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index c194f627d0..5260a0963f 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -244,7 +244,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); - + for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); @@ -351,7 +351,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } /* move altitude setpoint with throttle stick */ @@ -424,12 +424,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - local_pos_sp.z = -params.takeoff_alt; + local_pos_sp.z = - params.takeoff_alt - 3.0f; local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { @@ -505,7 +505,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* set altitude setpoint to 5m under ground, * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); } reset_sp_z = true; @@ -515,7 +515,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); } } From 40e18948727b41843fc0766e520f92b8cc841296 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 2 Sep 2013 23:30:32 +0200 Subject: [PATCH 58/75] Added parameter NAV_TAKEOFF_GAP --- src/modules/commander/commander_params.c | 1 + src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- .../multirotor_pos_control/multirotor_pos_control_params.c | 4 +++- .../multirotor_pos_control/multirotor_pos_control_params.h | 6 ++++-- 4 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f22dac0c15..40d0386d5e 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -46,6 +46,7 @@ #include PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f); PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 5260a0963f..3365230724 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -424,7 +424,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - local_pos_sp.z = - params.takeoff_alt - 3.0f; + local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 4d65118511..bf9578ea3d 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -60,6 +60,7 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + h->takeoff_gap = param_find("NAV_TAKEOFF_GAP"); h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); h->z_p = param_find("MPC_Z_P"); @@ -85,7 +86,8 @@ int parameters_init(struct multirotor_position_control_param_handles *h) int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { - param_get(h->takeoff_alt,&(p->takeoff_alt)); + param_get(h->takeoff_alt, &(p->takeoff_alt)); + param_get(h->takeoff_gap, &(p->takeoff_gap)); param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); param_get(h->z_p, &(p->z_p)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 79bc9b72bf..fc658dadb8 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -41,7 +41,8 @@ #include struct multirotor_position_control_params { - float takeoff_alt; + float takeoff_alt; + float takeoff_gap; float thr_min; float thr_max; float z_p; @@ -64,7 +65,8 @@ struct multirotor_position_control_params { }; struct multirotor_position_control_param_handles { - param_t takeoff_alt; + param_t takeoff_alt; + param_t takeoff_gap; param_t thr_min; param_t thr_max; param_t z_p; From 29c06e39431eb6e5112a2cefa9969d59fa2ff875 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 07:36:43 +0200 Subject: [PATCH 59/75] Set sane defaults for F330 --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 40 ++++++++++++++++++-------- 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index fb141e8a73..743dce9ef6 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -10,18 +10,34 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 + param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 1.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 2 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.10 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 param save fi From 3c0c11aec3c73d579659d81f9b68efbf7425846d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 07:49:57 +0200 Subject: [PATCH 60/75] v4 compatibility --- Tools/px_uploader.py | 2 +- src/drivers/px4io/uploader.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d2ebf16987..cffc316790 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -158,7 +158,7 @@ class uploader(object): INFO_BL_REV = chr(1) # bootloader protocol revision BL_REV_MIN = 2 # minimum supported bootloader protocol - BL_REV_MAX = 3 # maximum supported bootloader protocol + BL_REV_MAX = 4 # maximum supported bootloader protocol INFO_BOARD_ID = chr(2) # board type INFO_BOARD_REV = chr(3) # board revision INFO_FLASH_SIZE = chr(4) # max firmware size in bytes diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 135202dd1b..22387a3e23 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -69,7 +69,7 @@ private: PROTO_REBOOT = 0x30, INFO_BL_REV = 1, /**< bootloader protocol revision */ - BL_REV = 3, /**< supported bootloader protocol */ + BL_REV = 4, /**< supported bootloader protocol */ INFO_BOARD_ID = 2, /**< board type */ INFO_BOARD_REV = 3, /**< board revision */ INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ From 791e22f44245676a743af5d73dd159f92fb1c159 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 07:50:27 +0200 Subject: [PATCH 61/75] MAVLink workaround --- src/modules/mavlink/mavlink.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a8ca19d7a4..052ce7948c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,6 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ + usleep(1000); mavlink_logbuffer_init(&lb, 10); int ch; From 2457013bbba3e15e3fbfcc45f07428f006d56dcd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 08:17:22 +0200 Subject: [PATCH 62/75] Hotfix for USB: Starting MAVLink only on USB if connected. Needs rewrite of MAVLink and delay / retries for correct approach --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++---- src/modules/mavlink/mavlink.c | 1 - 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 9bb8e4a494..8c79a035a0 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -63,9 +63,8 @@ then if sercon then echo "USB connected" - else - # second attempt - sercon & + sleep 3 + mavlink start -d /dev/ttyACM0 -b 230400 fi # @@ -105,7 +104,7 @@ then fi # Try to get an USB console - nshterm /dev/ttyACM0 & + #nshterm /dev/ttyACM0 & # # Upgrade PX4IO firmware @@ -219,5 +218,6 @@ then gps start fi + # End of autostart fi diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 052ce7948c..a8ca19d7a4 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,7 +516,6 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - usleep(1000); mavlink_logbuffer_init(&lb, 10); int ch; From 811e1151fa05520f6f937518f3c7d5f6305b9fd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 16:41:29 +0200 Subject: [PATCH 63/75] Fix UART buf sizes so that MAVLink transfers are not corrupted for all serial ports intended for MAVLink --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- nuttx-configs/px4fmu-v2/nsh/defconfig | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 68ab4da32d..5be528f8c9 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -509,8 +509,8 @@ CONFIG_USART2_OFLOWCONTROL=y # # UART5 Configuration # -CONFIG_UART5_RXBUFSIZE=32 -CONFIG_UART5_TXBUFSIZE=32 +CONFIG_UART5_RXBUFSIZE=512 +CONFIG_UART5_TXBUFSIZE=512 CONFIG_UART5_BAUD=57600 CONFIG_UART5_BITS=8 CONFIG_UART5_PARITY=0 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e507c89baf..0615950a2d 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -608,8 +608,8 @@ CONFIG_USART6_2STOP=0 # # UART7 Configuration # -CONFIG_UART7_RXBUFSIZE=256 -CONFIG_UART7_TXBUFSIZE=256 +CONFIG_UART7_RXBUFSIZE=512 +CONFIG_UART7_TXBUFSIZE=512 CONFIG_UART7_BAUD=57600 CONFIG_UART7_BITS=8 CONFIG_UART7_PARITY=0 @@ -620,8 +620,8 @@ CONFIG_UART7_2STOP=0 # # UART8 Configuration # -CONFIG_UART8_RXBUFSIZE=256 -CONFIG_UART8_TXBUFSIZE=256 +CONFIG_UART8_RXBUFSIZE=512 +CONFIG_UART8_TXBUFSIZE=512 CONFIG_UART8_BAUD=57600 CONFIG_UART8_BITS=8 CONFIG_UART8_PARITY=0 From e301bb4d9425bbe460384afcc029dac6fa63f3e1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Sep 2013 17:07:41 +0200 Subject: [PATCH 64/75] Reset baudrate after px4io update --- src/drivers/px4io/px4io_uploader.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 7db28ecad0..fe8561a0b8 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -110,6 +110,10 @@ PX4IO_Uploader::upload(const char *filenames[]) return -errno; } + /* save initial uart configuration to reset after the update */ + struct termios t_original; + tcgetattr(_io_fd, &t_original); + /* adjust line speed to match bootloader */ struct termios t; tcgetattr(_io_fd, &t); @@ -122,6 +126,7 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -EIO; @@ -130,6 +135,7 @@ PX4IO_Uploader::upload(const char *filenames[]) struct stat st; if (stat(filename, &st) != 0) { log("Failed to stat %s - %d\n", filename, (int)errno); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -errno; @@ -137,6 +143,7 @@ PX4IO_Uploader::upload(const char *filenames[]) fw_size = st.st_size; if (_fw_fd == -1) { + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -ENOENT; @@ -151,6 +158,7 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -EIO; @@ -164,6 +172,7 @@ PX4IO_Uploader::upload(const char *filenames[]) log("found bootloader revision: %d", bl_rev); } else { log("found unsupported bootloader revision %d, exiting", bl_rev); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return OK; @@ -199,6 +208,9 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret != OK) { log("reboot failed"); + tcsetattr(_io_fd, TCSANOW, &t_original); + close(_io_fd); + _io_fd = -1; return ret; } @@ -208,6 +220,9 @@ PX4IO_Uploader::upload(const char *filenames[]) break; } + /* reset uart to previous/default baudrate */ + tcsetattr(_io_fd, TCSANOW, &t_original); + close(_fw_fd); close(_io_fd); _io_fd = -1; From 175020cb5b9b5bb4162a765f8d924830105fad58 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 22:45:33 +0200 Subject: [PATCH 65/75] Hotfix: Identified mavlink issue source, workaround, working on a fix --- ROMFS/px4fmu_common/init.d/rcS | 4 +--- src/modules/mavlink/mavlink.c | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8c79a035a0..32fb67a454 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -63,8 +63,6 @@ then if sercon then echo "USB connected" - sleep 3 - mavlink start -d /dev/ttyACM0 -b 230400 fi # @@ -104,7 +102,7 @@ then fi # Try to get an USB console - #nshterm /dev/ttyACM0 & + nshterm /dev/ttyACM0 & # # Upgrade PX4IO firmware diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a8ca19d7a4..5eb7cba9b2 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -739,7 +739,7 @@ int mavlink_thread_main(int argc, char *argv[]) tcsetattr(uart, TCSANOW, &uart_config_original); /* destroy log buffer */ - mavlink_logbuffer_destroy(&lb); + //mavlink_logbuffer_destroy(&lb); thread_running = false; From 5030a27f48b4ff4a5bb65e118ae85bada09dc85a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 3 Sep 2013 23:16:32 +0200 Subject: [PATCH 66/75] Tabs replaced by spaces in autostart scripts --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 56 ++++++++++----------- ROMFS/px4fmu_common/init.d/11_dji_f450 | 24 ++++----- ROMFS/px4fmu_common/init.d/15_tbs_discovery | 24 ++++----- ROMFS/px4fmu_common/init.d/16_3dr_iris | 24 ++++----- ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 30 +++++------ 5 files changed, 79 insertions(+), 79 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 743dce9ef6..709e170830 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -10,34 +10,34 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 1.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 2 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.10 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 + param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 2 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.10 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 param save fi diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 1827a2c114..73af1465c6 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -10,18 +10,18 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 + param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 param save fi diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index 1fcde3e27f..e4b9aee345 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -10,18 +10,18 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 5.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.2 + param set MC_ATTRATE_D 0.006 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 5.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 param save fi diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index fa2cb1feb0..3fbc848990 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -10,18 +10,18 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 5.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.2 + param set MC_ATTRATE_D 0.006 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 5.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 param save fi diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index 938663f0c2..ae41f2a014 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -8,21 +8,21 @@ echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 - param set MC_ATT_D 0 - param set MC_YAWPOS_D 0 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.08 - param set RC_SCALE_PITCH 1 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_YAW 3 + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 param set SYS_AUTOCONFIG 0 param save From 0c7995fd03bff63e765862910eaafff1ffb3197c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Sep 2013 08:12:51 +0200 Subject: [PATCH 67/75] Avoid unneccesary cast --- src/modules/systemlib/mavlink_log.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 03ca71375f..f321f9ceba 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -51,7 +51,7 @@ __EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) lb->size = size; lb->start = 0; lb->count = 0; - lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); + lb->elems = calloc(lb->size, sizeof(struct mavlink_logmessage)); } __EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb) From 59373399ff8fe08dfd0fb1cee59ed8c73ec75810 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Sep 2013 13:30:00 +0200 Subject: [PATCH 68/75] Uploader works now with Python 2.7 and 3.3 --- Tools/px_uploader.py | 71 +++++++++++++++++++++++++++----------------- 1 file changed, 44 insertions(+), 27 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 8a7e5e2501..c2ba6d0342 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -137,31 +137,31 @@ class uploader(object): '''Uploads a firmware file to the PX FMU bootloader''' # protocol bytes - INSYNC = chr(0x12) - EOC = chr(0x20) + INSYNC = b'\x12' + EOC = b'\x20' # reply bytes - OK = chr(0x10) - FAILED = chr(0x11) - INVALID = chr(0x13) # rev3+ + OK = b'\x10' + FAILED = b'\x11' + INVALID = b'\x13' # rev3+ # command bytes - NOP = chr(0x00) # guaranteed to be discarded by the bootloader - GET_SYNC = chr(0x21) - GET_DEVICE = chr(0x22) - CHIP_ERASE = chr(0x23) - CHIP_VERIFY = chr(0x24) # rev2 only - PROG_MULTI = chr(0x27) - READ_MULTI = chr(0x28) # rev2 only - GET_CRC = chr(0x29) # rev3+ - REBOOT = chr(0x30) + NOP = b'\x00' # guaranteed to be discarded by the bootloader + GET_SYNC = b'\x21' + GET_DEVICE = b'\x22' + CHIP_ERASE = b'\x23' + CHIP_VERIFY = b'\x24' # rev2 only + PROG_MULTI = b'\x27' + READ_MULTI = b'\x28' # rev2 only + GET_CRC = b'\x29' # rev3+ + REBOOT = b'\x30' - INFO_BL_REV = chr(1) # bootloader protocol revision + INFO_BL_REV = b'\x01' # bootloader protocol revision BL_REV_MIN = 2 # minimum supported bootloader protocol BL_REV_MAX = 4 # maximum supported bootloader protocol - INFO_BOARD_ID = chr(2) # board type - INFO_BOARD_REV = chr(3) # board revision - INFO_FLASH_SIZE = chr(4) # max firmware size in bytes + INFO_BOARD_ID = b'\x02' # board type + INFO_BOARD_REV = b'\x03' # board revision + INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 @@ -176,7 +176,7 @@ class uploader(object): def __send(self, c): # print("send " + binascii.hexlify(c)) - self.port.write(str(c)) + self.port.write(c) def __recv(self, count=1): c = self.port.read(count) @@ -192,9 +192,9 @@ class uploader(object): def __getSync(self): self.port.flush() - c = self.__recv() - if c is not self.INSYNC: - raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c)) + c = bytes(self.__recv()) + if c != self.INSYNC: + raise RuntimeError("unexpected %s instead of INSYNC" % c) c = self.__recv() if c == self.INVALID: raise RuntimeError("bootloader reports INVALID OPERATION") @@ -249,17 +249,29 @@ class uploader(object): # send a PROG_MULTI command to write a collection of bytes def __program_multi(self, data): - self.__send(uploader.PROG_MULTI - + chr(len(data))) + + if runningPython3 == True: + length = len(data).to_bytes(1, byteorder='big') + else: + length = chr(len(data)) + + self.__send(uploader.PROG_MULTI) + self.__send(length) self.__send(data) self.__send(uploader.EOC) self.__getSync() # verify multiple bytes in flash def __verify_multi(self, data): - self.__send(uploader.READ_MULTI - + chr(len(data)) - + uploader.EOC) + + if runningPython3 == True: + length = len(data).to_bytes(1, byteorder='big') + else: + length = chr(len(data)) + + self.__send(uploader.READ_MULTI) + self.__send(length) + self.__send(uploader.EOC) self.port.flush() programmed = self.__recv(len(data)) if programmed != data: @@ -351,6 +363,11 @@ class uploader(object): self.__reboot() self.port.close() +# Detect python version +if sys.version_info[0] < 3: + runningPython3 = False +else: + runningPython3 = True # Parse commandline arguments parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") From d30ba44f463045aee47fe9ac06436d28c5b4800b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Sep 2013 14:54:16 +0200 Subject: [PATCH 69/75] The python uploader can now reboot the board --- Tools/px_uploader.py | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index c2ba6d0342..cab3c2fd0e 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -165,6 +165,12 @@ class uploader(object): PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 + + NSH_INIT = bytearray(b'\x0d\x0d\x0d') + NSH_REBOOT_BL = b"reboot -b\n" + NSH_REBOOT = b"reboot\n" + MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') + MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly @@ -362,6 +368,18 @@ class uploader(object): print("done, rebooting.") self.__reboot() self.port.close() + + def send_reboot(self): + # try reboot via NSH first + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT_BL) + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT) + # then try MAVLINK command + self.__send(uploader.MAVLINK_REBOOT_ID1) + self.__send(uploader.MAVLINK_REBOOT_ID0) + + # Detect python version if sys.version_info[0] < 3: @@ -414,7 +432,11 @@ while True: print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) except: - # most probably a timeout talking to the port, no bootloader + # most probably a timeout talking to the port, no bootloader, try to reboot the board + print("attempting reboot on %s..." % port) + up.send_reboot() + # wait for the reboot, without we might run into Serial I/O Error 5 + time.sleep(1.5) continue try: From 09db74da0a88908aa761940a3de8383c0c64d1e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Sep 2013 10:27:35 +0200 Subject: [PATCH 70/75] Hotfix: Minor param adjustments based on flight testing --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 709e170830..32209d11c9 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -30,12 +30,12 @@ then param set MPC_XY_P 0.5 param set MPC_XY_VEL_D 0 param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 2 + param set MPC_XY_VEL_MAX 3 param set MPC_XY_VEL_P 0.2 param set MPC_Z_D 0 param set MPC_Z_P 1 param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.10 + param set MPC_Z_VEL_I 0.1 param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 From 1d7f3d0aa5f886d56bd69018fe7759a7df1ef6ef Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 5 Sep 2013 12:20:03 +0200 Subject: [PATCH 71/75] position_estimator_inav: land detector bug fixed --- .../position_estimator_inav_main.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 88057b3237..932f610886 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include #include #include @@ -181,7 +181,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) uint32_t baro_counter = 0; /* declare and safely initialize all structs */ - struct actuator_controls_effective_s actuator; + struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); struct actuator_armed_s armed; memset(&armed, 0, sizeof(armed)); @@ -200,7 +200,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -337,7 +337,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* actuator */ if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); } /* armed */ @@ -546,7 +546,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) alt_disp = alt_disp * alt_disp; float land_disp2 = params.land_disp * params.land_disp; /* get actual thrust output */ - float thrust = armed.armed ? actuator.control_effective[3] : 0.0f; + float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { if (alt_disp > land_disp2 && thrust > params.land_thr) { From aa785b0d2b339a8fcc730e11b63264b1ff8d146d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Sep 2013 13:24:21 +0200 Subject: [PATCH 72/75] Hotfix: Better error reporting, fixed sched param setup --- src/modules/commander/commander.cpp | 25 ++++++++++++++++++++---- src/modules/commander/rc_calibration.cpp | 14 +++++++------ src/modules/systemlib/param/param.c | 4 ++-- 3 files changed, 31 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8e8226f091..333fe30ae2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -599,6 +600,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; + (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); @@ -1655,13 +1657,13 @@ void *commander_low_prio_loop(void *arg) if (((int)(cmd.param1)) == 1) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(1000000); + usleep(100000); /* reboot */ systemreset(false); } else if (((int)(cmd.param1)) == 3) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(1000000); + usleep(100000); /* reboot to bootloader */ systemreset(true); @@ -1704,6 +1706,7 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + calib_ret = do_rc_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ @@ -1729,22 +1732,36 @@ void *commander_low_prio_loop(void *arg) case VEHICLE_CMD_PREFLIGHT_STORAGE: { if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { + int ret = param_load_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) + ret = -ret; + + if (ret < 1000) + mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { + int ret = param_save_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) + ret = -ret; + + if (ret < 1000) + mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 9fc1d64700..90ede499a1 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -57,14 +57,16 @@ int do_rc_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "trim calibration starting"); - /* XXX fix this */ - // if (current_status.rc_signal) { - // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - // return; - // } - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp; + bool changed; + orb_check(sub_man, &changed); + + if (!changed) { + mavlink_log_critical(mavlink_fd, "no manual control, aborting"); + return ERROR; + } + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 24b52d1a92..c69de52b70 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -520,7 +520,7 @@ param_save_default(void) if (fd < 0) { warn("opening '%s' for writing failed", param_get_default_file()); - return -1; + return fd; } result = param_export(fd, false); @@ -529,7 +529,7 @@ param_save_default(void) if (result != 0) { warn("error exporting parameters to '%s'", param_get_default_file()); unlink(param_get_default_file()); - return -2; + return result; } return 0; From 5dbe53877ab543728d4f2d288716d4989464237a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Sep 2013 13:24:37 +0200 Subject: [PATCH 73/75] Fixed sched param setup in MAVLink app --- src/modules/mavlink/mavlink_receiver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index af43542da4..4674f7a248 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -747,6 +747,7 @@ receive_start(int uart) fcntl(uart, F_SETFL, flags | O_NONBLOCK); struct sched_param param; + (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); From e8c309fb1442f281051874ca6d82af9b52605c3b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 5 Sep 2013 15:57:09 +0200 Subject: [PATCH 74/75] Workaround to prevent crash during mag calibration --- src/drivers/hmc5883/hmc5883.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index a5229b2371..3ede90a174 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -962,11 +962,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) warnx("sampling 500 samples for scaling offset"); /* set the queue depth to 10 */ - if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { - warn("failed to set queue depth"); - ret = 1; - goto out; - } + /* don't do this for now, it can lead to a crash in start() respectively work_queue() */ +// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { +// warn("failed to set queue depth"); +// ret = 1; +// goto out; +// } /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { From 373a74adb9abc218ca8084fc3f59c0e50daf9bf4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Sep 2013 17:39:11 +0200 Subject: [PATCH 75/75] Setting correct battery scaling for FMU-only setups --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 1 + ROMFS/px4fmu_common/init.d/11_dji_f450 | 1 + ROMFS/px4fmu_common/init.d/15_tbs_discovery | 1 + ROMFS/px4fmu_common/init.d/16_3dr_iris | 1 + 4 files changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 32209d11c9..b8fe5fc31c 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -69,6 +69,7 @@ else mavlink start -d /dev/ttyS0 usleep 5000 fmu mode_pwm + param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 73af1465c6..388f40a477 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -53,6 +53,7 @@ else mavlink start -d /dev/ttyS0 usleep 5000 fmu mode_pwm + param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index e4b9aee345..cbfde6d3c0 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -53,6 +53,7 @@ else mavlink start -d /dev/ttyS0 usleep 5000 fmu mode_pwm + param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index 3fbc848990..b6c5fbdea4 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -53,6 +53,7 @@ else mavlink start -d /dev/ttyS0 usleep 5000 fmu mode_pwm + param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi