Lorenz Meier
12 years ago
66 changed files with 1869 additions and 1296 deletions
@ -1,86 +1,45 @@
@@ -1,86 +1,45 @@
|
||||
#!nsh |
||||
# |
||||
# Flight startup script for PX4FMU on PX4IOAR carrier board. |
||||
# |
||||
|
||||
echo "[init] doing PX4IOAR startup..." |
||||
|
||||
# |
||||
# Start the ORB |
||||
# |
||||
uorb start |
||||
|
||||
echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" |
||||
|
||||
# |
||||
# Load microSD params |
||||
# Load default params for this platform |
||||
# |
||||
echo "[init] loading microSD params" |
||||
param select /fs/microsd/params |
||||
if [ -f /fs/microsd/params ] |
||||
if param compare SYS_AUTOCONFIG 1 |
||||
then |
||||
param load /fs/microsd/params |
||||
# 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 1 = fixed wing, 2 = quadrotor, 13 = hexarotor |
||||
# see https://pixhawk.ethz.ch/mavlink/ |
||||
# MAV_TYPE 2 = quadrotor |
||||
# |
||||
param set MAV_TYPE 2 |
||||
|
||||
# |
||||
# Configure PX4FMU for operation with PX4IOAR |
||||
# |
||||
fmu mode_gpio_serial |
||||
|
||||
# |
||||
# Start the sensors. |
||||
# |
||||
sh /etc/init.d/rc.sensors |
||||
|
||||
# |
||||
# Start MAVLink |
||||
# |
||||
mavlink start -d /dev/ttyS0 -b 57600 |
||||
usleep 5000 |
||||
|
||||
# |
||||
# Start the commander. |
||||
# |
||||
commander start |
||||
|
||||
# |
||||
# Start the attitude estimator |
||||
# |
||||
attitude_estimator_ekf start |
||||
|
||||
|
||||
# |
||||
# Fire up the multi rotor attitude controller |
||||
# Configure PX4FMU for operation with PX4IOAR |
||||
# |
||||
multirotor_att_control start |
||||
fmu mode_gpio_serial |
||||
|
||||
# |
||||
# Fire up the AR.Drone interface. |
||||
# |
||||
ardrone_interface start -d /dev/ttyS1 |
||||
|
||||
# |
||||
# Start logging once armed |
||||
# |
||||
sdlog2 start -r 50 -a -b 14 |
||||
|
||||
# |
||||
# Start GPS capture |
||||
# |
||||
gps start |
||||
|
||||
# |
||||
# startup is done; we don't want the shell because we |
||||
# use the same UART for telemetry |
||||
# Start common for all multirotors apps |
||||
# |
||||
echo "[init] startup done" |
||||
|
||||
# Try to get an USB console |
||||
nshterm /dev/ttyACM0 & |
||||
|
||||
sh /etc/init.d/rc.multirotor |
||||
|
||||
# Exit, because /dev/ttyS0 is needed for MAVLink |
||||
exit |
||||
|
||||
@ -0,0 +1,64 @@
@@ -0,0 +1,64 @@
|
||||
#!nsh |
||||
|
||||
echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 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 /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/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 |
||||
@ -0,0 +1,23 @@
@@ -0,0 +1,23 @@
|
||||
# |
||||
# Start PX4IO interface (depends on orb, commander) |
||||
# |
||||
if px4io start |
||||
then |
||||
# |
||||
# Allow PX4IO to recover from midair restarts. |
||||
# this is very unlikely, but quite safe and robust. |
||||
px4io recovery |
||||
|
||||
# |
||||
# Disable px4io topic limiting |
||||
# |
||||
if [ $BOARD == fmuv1 ] |
||||
then |
||||
px4io limit 200 |
||||
else |
||||
px4io limit 400 |
||||
fi |
||||
else |
||||
# SOS |
||||
tone_alarm 6 |
||||
fi |
||||
@ -0,0 +1,44 @@
@@ -0,0 +1,44 @@
|
||||
#!nsh |
||||
# |
||||
# Standard everything needed for multirotors except mixer, output and mavlink |
||||
# |
||||
|
||||
# |
||||
# Start the sensors and test them. |
||||
# |
||||
sh /etc/init.d/rc.sensors |
||||
|
||||
# |
||||
# Start logging (depends on sensors) |
||||
# |
||||
sh /etc/init.d/rc.logging |
||||
|
||||
# |
||||
# Start the commander. |
||||
# |
||||
commander start |
||||
|
||||
# |
||||
# Start GPS interface (depends on orb) |
||||
# |
||||
gps start |
||||
|
||||
# |
||||
# Start the attitude estimator |
||||
# |
||||
attitude_estimator_ekf start |
||||
|
||||
# |
||||
# Start position estimator |
||||
# |
||||
position_estimator_inav start |
||||
|
||||
# |
||||
# Start attitude control |
||||
# |
||||
multirotor_att_control start |
||||
|
||||
# |
||||
# Start position control |
||||
# |
||||
multirotor_pos_control start |
||||
@ -0,0 +1,55 @@
@@ -0,0 +1,55 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved. |
||||
* Author: Simon Wilks <sjwilks@gmail.com> |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file commander_tests.cpp |
||||
* Commander unit tests. Run the tests as follows: |
||||
* nsh> commander_tests |
||||
* |
||||
*/ |
||||
|
||||
#include <systemlib/err.h> |
||||
|
||||
#include "state_machine_helper_test.h" |
||||
|
||||
extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); |
||||
|
||||
|
||||
int commander_tests_main(int argc, char *argv[]) |
||||
{ |
||||
state_machine_helper_test(); |
||||
//state_machine_test();
|
||||
|
||||
return 0; |
||||
} |
||||
@ -0,0 +1,41 @@
@@ -0,0 +1,41 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# System state machine tests.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = commander_tests
|
||||
SRCS = commander_tests.cpp \
|
||||
state_machine_helper_test.cpp \
|
||||
../state_machine_helper.cpp
|
||||
@ -0,0 +1,247 @@
@@ -0,0 +1,247 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved. |
||||
* Author: Simon Wilks <sjwilks@gmail.com> |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file state_machine_helper_test.cpp |
||||
* System state machine unit test. |
||||
* |
||||
*/ |
||||
|
||||
#include "state_machine_helper_test.h" |
||||
|
||||
#include "../state_machine_helper.h" |
||||
#include <unit_test/unit_test.h> |
||||
|
||||
class StateMachineHelperTest : public UnitTest |
||||
{ |
||||
public: |
||||
StateMachineHelperTest(); |
||||
virtual ~StateMachineHelperTest(); |
||||
|
||||
virtual const char* run_tests(); |
||||
|
||||
private: |
||||
const char* arming_state_transition_test(); |
||||
const char* arming_state_transition_arm_disarm_test(); |
||||
const char* main_state_transition_test(); |
||||
const char* is_safe_test(); |
||||
}; |
||||
|
||||
StateMachineHelperTest::StateMachineHelperTest() { |
||||
} |
||||
|
||||
StateMachineHelperTest::~StateMachineHelperTest() { |
||||
} |
||||
|
||||
const char* |
||||
StateMachineHelperTest::arming_state_transition_test() |
||||
{ |
||||
struct vehicle_status_s status; |
||||
struct safety_s safety; |
||||
arming_state_t new_arming_state; |
||||
struct actuator_armed_s armed; |
||||
|
||||
// Identical states.
|
||||
status.arming_state = ARMING_STATE_INIT; |
||||
new_arming_state = ARMING_STATE_INIT; |
||||
mu_assert("no transition: identical states", |
||||
TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); |
||||
|
||||
// INIT to STANDBY.
|
||||
armed.armed = false; |
||||
armed.ready_to_arm = false; |
||||
status.arming_state = ARMING_STATE_INIT; |
||||
status.condition_system_sensors_initialized = true; |
||||
new_arming_state = ARMING_STATE_STANDBY; |
||||
mu_assert("transition: init to standby", |
||||
TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); |
||||
mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state); |
||||
mu_assert("not armed", !armed.armed); |
||||
mu_assert("ready to arm", armed.ready_to_arm); |
||||
|
||||
// INIT to STANDBY, sensors not initialized.
|
||||
armed.armed = false; |
||||
armed.ready_to_arm = false; |
||||
status.arming_state = ARMING_STATE_INIT; |
||||
status.condition_system_sensors_initialized = false; |
||||
new_arming_state = ARMING_STATE_STANDBY; |
||||
mu_assert("no transition: sensors not initialized", |
||||
TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed)); |
||||
mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state); |
||||
mu_assert("not armed", !armed.armed); |
||||
mu_assert("not ready to arm", !armed.ready_to_arm); |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
const char* |
||||
StateMachineHelperTest::arming_state_transition_arm_disarm_test() |
||||
{ |
||||
struct vehicle_status_s status; |
||||
struct safety_s safety; |
||||
arming_state_t new_arming_state; |
||||
struct actuator_armed_s armed; |
||||
|
||||
// TODO(sjwilks): ARM then DISARM.
|
||||
return 0; |
||||
} |
||||
|
||||
const char* |
||||
StateMachineHelperTest::main_state_transition_test() |
||||
{ |
||||
struct vehicle_status_s current_state; |
||||
main_state_t new_main_state; |
||||
|
||||
// Identical states.
|
||||
current_state.main_state = MAIN_STATE_MANUAL; |
||||
new_main_state = MAIN_STATE_MANUAL; |
||||
mu_assert("no transition: identical states", |
||||
TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); |
||||
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
|
||||
|
||||
// AUTO to MANUAL.
|
||||
current_state.main_state = MAIN_STATE_AUTO; |
||||
new_main_state = MAIN_STATE_MANUAL; |
||||
mu_assert("transition changed: auto to manual", |
||||
TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); |
||||
mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); |
||||
|
||||
// MANUAL to SEATBELT.
|
||||
current_state.main_state = MAIN_STATE_MANUAL; |
||||
current_state.condition_local_altitude_valid = true; |
||||
new_main_state = MAIN_STATE_SEATBELT; |
||||
mu_assert("tranisition: manual to seatbelt",
|
||||
TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); |
||||
mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); |
||||
|
||||
// MANUAL to SEATBELT, invalid local altitude.
|
||||
current_state.main_state = MAIN_STATE_MANUAL; |
||||
current_state.condition_local_altitude_valid = false; |
||||
new_main_state = MAIN_STATE_SEATBELT; |
||||
mu_assert("no transition: invalid local altitude", |
||||
TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); |
||||
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); |
||||
|
||||
// MANUAL to EASY.
|
||||
current_state.main_state = MAIN_STATE_MANUAL; |
||||
current_state.condition_local_position_valid = true; |
||||
new_main_state = MAIN_STATE_EASY; |
||||
mu_assert("transition: manual to easy", |
||||
TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); |
||||
mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); |
||||
|
||||
// MANUAL to EASY, invalid local position.
|
||||
current_state.main_state = MAIN_STATE_MANUAL; |
||||
current_state.condition_local_position_valid = false; |
||||
new_main_state = MAIN_STATE_EASY; |
||||
mu_assert("no transition: invalid position", |
||||
TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); |
||||
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); |
||||
|
||||
// MANUAL to AUTO.
|
||||
current_state.main_state = MAIN_STATE_MANUAL; |
||||
current_state.condition_global_position_valid = true; |
||||
new_main_state = MAIN_STATE_AUTO; |
||||
mu_assert("transition: manual to auto", |
||||
TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); |
||||
mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); |
||||
|
||||
// MANUAL to AUTO, invalid global position.
|
||||
current_state.main_state = MAIN_STATE_MANUAL; |
||||
current_state.condition_global_position_valid = false; |
||||
new_main_state = MAIN_STATE_AUTO; |
||||
mu_assert("no transition: invalid global position", |
||||
TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); |
||||
mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
const char* |
||||
StateMachineHelperTest::is_safe_test() |
||||
{ |
||||
struct vehicle_status_s current_state; |
||||
struct safety_s safety; |
||||
struct actuator_armed_s armed; |
||||
|
||||
armed.armed = false; |
||||
armed.lockdown = false; |
||||
safety.safety_switch_available = true; |
||||
safety.safety_off = false; |
||||
mu_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); |
||||
|
||||
armed.armed = false; |
||||
armed.lockdown = true; |
||||
safety.safety_switch_available = true; |
||||
safety.safety_off = true; |
||||
mu_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); |
||||
|
||||
armed.armed = true; |
||||
armed.lockdown = false; |
||||
safety.safety_switch_available = true; |
||||
safety.safety_off = true; |
||||
mu_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); |
||||
|
||||
armed.armed = true; |
||||
armed.lockdown = false; |
||||
safety.safety_switch_available = true; |
||||
safety.safety_off = false; |
||||
mu_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); |
||||
|
||||
armed.armed = true; |
||||
armed.lockdown = false; |
||||
safety.safety_switch_available = false; |
||||
safety.safety_off = false; |
||||
mu_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
const char* |
||||
StateMachineHelperTest::run_tests() |
||||
{ |
||||
mu_run_test(arming_state_transition_test); |
||||
mu_run_test(arming_state_transition_arm_disarm_test); |
||||
mu_run_test(main_state_transition_test); |
||||
mu_run_test(is_safe_test); |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
void |
||||
state_machine_helper_test() |
||||
{ |
||||
StateMachineHelperTest* test = new StateMachineHelperTest(); |
||||
test->UnitTest::print_results(test->run_tests()); |
||||
} |
||||
@ -0,0 +1,44 @@
@@ -0,0 +1,44 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved. |
||||
* Author: Simon Wilks <sjwilks@gmail.com> |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file state_machine_helper_test.h |
||||
*/ |
||||
|
||||
#ifndef STATE_MACHINE_HELPER_TEST_H_ |
||||
#define STATE_MACHINE_HELPER_TEST_ |
||||
|
||||
void state_machine_helper_test(); |
||||
|
||||
#endif /* STATE_MACHINE_HELPER_TEST_H_ */ |
||||
@ -0,0 +1,148 @@
@@ -0,0 +1,148 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 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 |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file rc_check.c |
||||
* |
||||
* RC calibration check |
||||
*/ |
||||
|
||||
#include <nuttx/config.h> |
||||
|
||||
#include <stdio.h> |
||||
#include <fcntl.h> |
||||
|
||||
#include <systemlib/rc_check.h> |
||||
#include <systemlib/param/param.h> |
||||
#include <mavlink/mavlink_log.h> |
||||
#include <uORB/topics/rc_channels.h> |
||||
|
||||
int rc_calibration_check(void) { |
||||
|
||||
char nbuf[20]; |
||||
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, |
||||
_parameter_handles_rev, _parameter_handles_dz; |
||||
|
||||
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); |
||||
|
||||
float param_min, param_max, param_trim, param_rev, param_dz; |
||||
|
||||
/* first check channel mappings */ |
||||
/* check which map param applies */ |
||||
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
|
||||
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
|
||||
// /* give system time to flush error message in case there are more */
|
||||
// usleep(100000);
|
||||
// count++;
|
||||
// }
|
||||
|
||||
|
||||
|
||||
for (int i = 0; i < RC_CHANNELS_MAX; i++) { |
||||
/* should the channel be enabled? */ |
||||
uint8_t count = 0; |
||||
|
||||
/* min values */ |
||||
sprintf(nbuf, "RC%d_MIN", i + 1); |
||||
_parameter_handles_min = param_find(nbuf); |
||||
param_get(_parameter_handles_min, ¶m_min); |
||||
|
||||
/* trim values */ |
||||
sprintf(nbuf, "RC%d_TRIM", i + 1); |
||||
_parameter_handles_trim = param_find(nbuf); |
||||
param_get(_parameter_handles_trim, ¶m_trim); |
||||
|
||||
/* max values */ |
||||
sprintf(nbuf, "RC%d_MAX", i + 1); |
||||
_parameter_handles_max = param_find(nbuf); |
||||
param_get(_parameter_handles_max, ¶m_max); |
||||
|
||||
/* channel reverse */ |
||||
sprintf(nbuf, "RC%d_REV", i + 1); |
||||
_parameter_handles_rev = param_find(nbuf); |
||||
param_get(_parameter_handles_rev, ¶m_rev); |
||||
|
||||
/* channel deadzone */ |
||||
sprintf(nbuf, "RC%d_DZ", i + 1); |
||||
_parameter_handles_dz = param_find(nbuf); |
||||
param_get(_parameter_handles_dz, ¶m_dz); |
||||
|
||||
/* assert min..center..max ordering */ |
||||
if (param_min < 500) { |
||||
count++; |
||||
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); |
||||
/* give system time to flush error message in case there are more */ |
||||
usleep(100000); |
||||
} |
||||
if (param_max > 2500) { |
||||
count++; |
||||
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); |
||||
/* give system time to flush error message in case there are more */ |
||||
usleep(100000); |
||||
} |
||||
if (param_trim < param_min) { |
||||
count++; |
||||
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); |
||||
/* 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); |
||||
/* give system time to flush error message in case there are more */ |
||||
usleep(100000); |
||||
} |
||||
|
||||
/* assert deadzone is sane */ |
||||
if (param_dz > 500) { |
||||
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); |
||||
/* give system time to flush error message in case there are more */ |
||||
usleep(100000); |
||||
count++; |
||||
} |
||||
|
||||
/* check which map param applies */ |
||||
// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
|
||||
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
|
||||
// /* give system time to flush error message in case there are more */
|
||||
// usleep(100000);
|
||||
// count++;
|
||||
// }
|
||||
|
||||
/* sanity checks pass, enable channel */ |
||||
if (count) { |
||||
mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); |
||||
usleep(100000); |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,52 @@
@@ -0,0 +1,52 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 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 |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file rc_check.h |
||||
* |
||||
* RC calibration check |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
__BEGIN_DECLS |
||||
|
||||
/**
|
||||
* Check the RC calibration |
||||
* |
||||
* @return 0 / OK if RC calibration is ok, index + 1 of the first |
||||
* channel that failed else (so 1 == first channel failed) |
||||
*/ |
||||
__EXPORT int rc_calibration_check(void); |
||||
|
||||
__END_DECLS |
||||
@ -1,4 +0,0 @@
@@ -1,4 +0,0 @@
|
||||
int test_main(void) |
||||
{ |
||||
return 0; |
||||
} |
||||
@ -1,4 +0,0 @@
@@ -1,4 +0,0 @@
|
||||
|
||||
MODULE_NAME = test
|
||||
SRCS = foo.c
|
||||
|
||||
@ -0,0 +1,39 @@
@@ -0,0 +1,39 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Makefile to build the unit test library.
|
||||
#
|
||||
|
||||
SRCS = unit_test.cpp
|
||||
|
||||
@ -0,0 +1,65 @@
@@ -0,0 +1,65 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved. |
||||
* Author: Simon Wilks <sjwilks@gmail.com> |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file unit_test.cpp |
||||
* A unit test library. |
||||
* |
||||
*/ |
||||
|
||||
#include "unit_test.h" |
||||
|
||||
#include <systemlib/err.h> |
||||
|
||||
|
||||
UnitTest::UnitTest() |
||||
{ |
||||
} |
||||
|
||||
UnitTest::~UnitTest() |
||||
{ |
||||
} |
||||
|
||||
void |
||||
UnitTest::print_results(const char* result) |
||||
{ |
||||
if (result != 0) { |
||||
warnx("Failed: %s:%d", mu_last_test(), mu_line()); |
||||
warnx("%s", result); |
||||
} else { |
||||
warnx("ALL TESTS PASSED"); |
||||
warnx(" Tests run : %d", mu_tests_run()); |
||||
warnx(" Assertion : %d", mu_assertion()); |
||||
} |
||||
} |
||||
@ -0,0 +1,93 @@
@@ -0,0 +1,93 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved. |
||||
* Author: Simon Wilks <sjwilks@gmail.com> |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file unit_test.h |
||||
* A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html).
|
||||
* |
||||
*/ |
||||
|
||||
#ifndef UNIT_TEST_H_ |
||||
#define UNIT_TEST_ |
||||
|
||||
#include <systemlib/err.h> |
||||
|
||||
|
||||
class __EXPORT UnitTest |
||||
{ |
||||
|
||||
public: |
||||
#define xstr(s) str(s) |
||||
#define str(s) #s |
||||
#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; } |
||||
|
||||
INLINE_GLOBAL(int, mu_tests_run) |
||||
INLINE_GLOBAL(int, mu_assertion) |
||||
INLINE_GLOBAL(int, mu_line) |
||||
INLINE_GLOBAL(const char*, mu_last_test) |
||||
|
||||
#define mu_assert(message, test) \ |
||||
do \
|
||||
{ \
|
||||
if (!(test)) \
|
||||
return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \
|
||||
else \
|
||||
mu_assertion()++; \
|
||||
} while (0) |
||||
|
||||
|
||||
#define mu_run_test(test) \ |
||||
do \
|
||||
{ \
|
||||
const char *message; \
|
||||
mu_last_test() = #test; \
|
||||
mu_line() = __LINE__; \
|
||||
message = test(); \
|
||||
mu_tests_run()++; \
|
||||
if (message) \
|
||||
return message; \
|
||||
} while (0) |
||||
|
||||
|
||||
public: |
||||
UnitTest(); |
||||
virtual ~UnitTest(); |
||||
|
||||
virtual const char* run_tests() = 0; |
||||
virtual void print_results(const char* result); |
||||
}; |
||||
|
||||
|
||||
|
||||
#endif /* UNIT_TEST_H_ */ |
||||
Loading…
Reference in new issue