diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 11:40:16 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 11:40:42 +0200 |
commit | a1b99a3f03a5908ea7e263658343451440326aea (patch) | |
tree | 45b3d287c854c99d0655934dbbd4ca65238703b8 | |
parent | a69c55f671b1a2116c0e6c497906844a81ce6c74 (diff) | |
download | px4-firmware-a1b99a3f03a5908ea7e263658343451440326aea.tar.gz px4-firmware-a1b99a3f03a5908ea7e263658343451440326aea.tar.bz2 px4-firmware-a1b99a3f03a5908ea7e263658343451440326aea.zip |
Kicked out mix_and_link, deleted old MPU driver, disabled (still needed for reference) old HMC and MS5611 drivers. Removed driver init from up_nsh.c. Reworked fixedwing_control to be closer to up-to-date api, still more clean up needed. Fixed a bug that limited the motor thrust for multirotor control
-rw-r--r-- | apps/fixedwing_control/fixedwing_control.c | 239 | ||||
-rw-r--r-- | apps/mix_and_link/Makefile | 38 | ||||
-rw-r--r-- | apps/mix_and_link/mix_and_link.c | 81 | ||||
-rw-r--r-- | apps/mix_and_link/mix_and_link.h | 115 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 11 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/include/drv_mpu6000.h | 92 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/nsh/appconfig | 2 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/Makefile | 4 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/drv_mpu6000.c | 433 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/up_cpuload.c | 1 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/up_nsh.c | 95 |
11 files changed, 86 insertions, 1025 deletions
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 1d13b1c6a..73a72185d 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -52,14 +52,15 @@ #include <arch/board/board.h> #include <drivers/drv_pwm_output.h> #include <nuttx/spi.h> -#include "../mix_and_link/mix_and_link.h" //TODO: add to Makefile #include <uORB/uORB.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/fixedwing_control.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> #include <systemlib/param/param.h> __EXPORT int fixedwing_control_main(int argc, char *argv[]); @@ -710,19 +711,38 @@ int fixedwing_control_main(int argc, char *argv[]) /* welcome user */ printf("[fixedwing control] started\n"); - /* Set up to publish fixed wing control messages */ - struct fixedwing_control_s control; - orb_advert_t fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control); + /* output structs */ + struct actuator_controls_s actuators; + struct actuator_armed_s armed; + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + + /* publish actuator controls */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + actuators.control[i] = 0.0f; + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + armed.armed = false; + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); /* Subscribe to global position, attitude and rc */ struct vehicle_global_position_s global_pos; int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_attitude_s att; - int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - struct rc_channels_s rc; - int rc_sub = orb_subscribe(ORB_ID(rc_channels)); struct vehicle_global_position_setpoint_s global_setpoint; int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + /* declare and safely initialize all structs */ + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + struct vehicle_attitude_s att; + memset(&att_sp, 0, sizeof(att_sp)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + + /* subscribe to attitude, motor setpoints and system state */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* Mainloop setup */ unsigned int loopcounter = 0; @@ -734,44 +754,6 @@ int fixedwing_control_main(int argc, char *argv[]) /* Servo setup */ - int fd; - servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; - - fd = open("/dev/pwm_servo", O_RDWR); - - if (fd < 0) { - printf("[fixedwing control] Failed opening /dev/pwm_servo\n"); - } - - ioctl(fd, PWM_SERVO_ARM, 0); - - int16_t buffer_rc[3]; - int16_t buffer_servo[3]; - mixer_data_t mixer_buffer; - mixer_buffer.input = buffer_rc; - mixer_buffer.output = buffer_servo; - - mixer_conf_t mixers[3]; - - mixers[0].source = PITCH; - mixers[0].nr_actuators = 2; - mixers[0].dest[0] = AIL_1; - mixers[0].dest[1] = AIL_2; - mixers[0].dual_rate[0] = 1; - mixers[0].dual_rate[1] = 1; - - mixers[1].source = ROLL; - mixers[1].nr_actuators = 2; - mixers[1].dest[0] = AIL_1; - mixers[1].dest[1] = AIL_2; - mixers[1].dual_rate[0] = 1; - mixers[1].dual_rate[1] = -1; - - mixers[2].source = THROTTLE; - mixers[2].nr_actuators = 1; - mixers[2].dest[0] = MOT; - mixers[2].dual_rate[0] = 1; - /* * Main control, navigation and servo routine */ @@ -786,8 +768,15 @@ int fixedwing_control_main(int argc, char *argv[]) // XXX add error checking orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att); - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + + /* get a local copy of system state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + /* get a local copy of attitude setpoint */ + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); /* scaling factors are defined by the data from the APM Planner * TODO: ifdef for other parameters (HIL/Real world switch) @@ -810,20 +799,19 @@ int fixedwing_control_main(int argc, char *argv[]) plane_data.yawspeed = att.yawspeed; /* parameter values */ - get_parameters(&plane_data); + if (loopcounter % 20 == 0) { + get_parameters(&plane_data); + } /* Attitude control part */ if (verbose && loopcounter % 20 == 0) { /******************************** DEBUG OUTPUT ************************************************************/ - printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att, + printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i\n", (int)plane_data.Kp_att, (int)plane_data.Ki_att, (int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos, (int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed, - (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z, - (int)global_data_parameter_storage->pm.param_values[PARAM_WPLON], - (int)global_data_parameter_storage->pm.param_values[PARAM_WPLAT], - (int)global_data_parameter_storage->pm.param_values[PARAM_WPALT]); + (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z); printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint)); printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint)); @@ -840,20 +828,13 @@ int fixedwing_control_main(int argc, char *argv[]) // printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n", // (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r)); - printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n", - (int)(10000.0f * control_outputs.roll_ailerons), (int)(10000.0f * control_outputs.pitch_elevator), - (int)(10000.0f * control_outputs.yaw_rudder), (int)(10000.0f * control_outputs.throttle)); + printf("\nCalculated outputs: \n R: %8.4f\n P: %8.4f\n Y: %8.4f\n T: %8.4f \n", + att_sp.roll_body, att_sp.pitch_body, + att_sp.yaw_body, att_sp.thrust); /************************************************************************************************************/ } - /* - * Computation section - * - * The function calls to compute the required control values happen - * in this section. - */ - /* Set plane mode */ set_plane_mode(); @@ -864,123 +845,37 @@ int fixedwing_control_main(int argc, char *argv[]) /* Calculate the body frame angles of the aircraft */ //calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw); - /* Calculate the output values */ - control_outputs.roll_ailerons = calc_roll_ail(); - control_outputs.pitch_elevator = calc_pitch_elev(); - //control_outputs.yaw_rudder = calc_yaw_rudder(); - control_outputs.throttle = calc_throttle(); - - if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode + // if (manual.override_mode_switch < XXX) { - /* - * TAKEOFF hack for HIL - */ - if (plane_data.mode == TAKEOFF) { - control.attitude_control_output[ROLL] = 0; - control.attitude_control_output[PITCH] = 5000; - control.attitude_control_output[THROTTLE] = 10000; - //global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder); - } - - if (plane_data.mode == CRUISE) { - control.attitude_control_output[ROLL] = control_outputs.roll_ailerons; - control.attitude_control_output[PITCH] = control_outputs.pitch_elevator; - control.attitude_control_output[THROTTLE] = control_outputs.throttle; - //control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder); - } - - control.counter++; - control.timestamp = hrt_absolute_time(); - } /* Navigation part */ - // Get GPS Waypoint - - // if(global_data_wait(&global_data_position_setpoint->access_conf) == 0) - // { - // plane_data.wp_x = global_data_position_setpoint->x; - // plane_data.wp_y = global_data_position_setpoint->y; - // plane_data.wp_z = global_data_position_setpoint->z; - // } - // global_data_unlock(&global_data_position_setpoint->access_conf); - - if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // AUTO mode - // AUTO/HYBRID switch - - if (rc.chan[rc.function[OVERRIDE]].scale < AUTO) { - plane_data.roll_setpoint = calc_roll_setpoint(); - plane_data.pitch_setpoint = calc_pitch_setpoint(); - plane_data.throttle_setpoint = calc_throttle_setpoint(); - - } else { - plane_data.roll_setpoint = rc.chan[rc.function[ROLL]].scale / 200; - plane_data.pitch_setpoint = rc.chan[rc.function[PITCH]].scale / 200; - plane_data.throttle_setpoint = rc.chan[rc.function[THROTTLE]].scale / 200; - } - - //control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg); - - } else { - control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale/10000; - control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale/10000; - control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale/10000; - // since we don't have a yaw rudder - //control->attitude_control_output[3] = global_data_rc_channels->chan[YAW].scale; - - control.counter++; - control.timestamp = hrt_absolute_time(); - } - - /* publish the control data */ + // Get Waypoint - orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control); + // XXX FIXME - /* Servo part */ + //else if (manual.override_mode_switch > MANUAL) { // AUTO mode + + /* calculate setpoint based on current position and waypoint */ + att_sp.roll_body = calc_roll_setpoint(); + att_sp.pitch_body = calc_pitch_setpoint(); + att_sp.thrust = calc_throttle_setpoint(); - buffer_rc[ROLL] = (int16_t)(10000*control.attitude_control_output[ROLL]); - buffer_rc[PITCH] = (int16_t)(10000*control.attitude_control_output[PITCH]); - buffer_rc[THROTTLE] = (int16_t)(10000*control.attitude_control_output[THROTTLE]); + /* calculate the control outputs based on roll / pitch / yaw setpoints */ + actuators.control[0] = calc_roll_ail(); + actuators.control[1] = calc_pitch_elev(); + actuators.control[2] = calc_yaw_rudder(att.yaw); + actuators.control[3] = calc_throttle(); - //mix_and_link(mixers, 3, 2, &mixer_buffer); + /* publish attitude setpoint (for MAVLink) */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - // Scaling and saturation of servo outputs happens here + /* publish actuator setpoints (for mixer) */ - data[AIL_1] = buffer_servo[AIL_1] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE] - + global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM]; - - if (data[AIL_1] > SERVO_MAX) - data[AIL_1] = SERVO_MAX; - - if (data[AIL_1] < SERVO_MIN) - data[AIL_1] = SERVO_MIN; - - data[AIL_2] = buffer_servo[AIL_2] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE] - + global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM]; - - if (data[AIL_2] > SERVO_MAX) - data[AIL_2] = SERVO_MAX; - - if (data[AIL_2] < SERVO_MIN) - data[AIL_2] = SERVO_MIN; - - data[MOT] = buffer_servo[MOT] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE] - + global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM]; - - if (data[MOT] > SERVO_MAX) - data[MOT] = SERVO_MAX; - - if (data[MOT] < SERVO_MIN) - data[MOT] = SERVO_MIN; - - int result = write(fd, &data, sizeof(data)); - - if (result != sizeof(data)) { - if (failcounter < 10 || failcounter % 20 == 0) { - printf("[fixedwing_control] failed writing servo outputs\n"); - } - failcounter++; - } + /* arming state depends on commander arming state */ + armed.armed = state.flag_system_armed; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); loopcounter++; diff --git a/apps/mix_and_link/Makefile b/apps/mix_and_link/Makefile deleted file mode 100644 index a907277c7..000000000 --- a/apps/mix_and_link/Makefile +++ /dev/null @@ -1,38 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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 mix-and-link functions -# - -include $(APPDIR)/mk/app.mk diff --git a/apps/mix_and_link/mix_and_link.c b/apps/mix_and_link/mix_and_link.c deleted file mode 100644 index 495f336b1..000000000 --- a/apps/mix_and_link/mix_and_link.c +++ /dev/null @@ -1,81 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Nils Wenzler <wenzlern@ethz.ch> - * - * 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 Mixing / linking of RC channels - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdint.h> - -#include "mix_and_link.h" - -/* - * Library function that mixes the abstract functions like ROLL, PITCH, THROTTLE - * to one or several actuators. - * @param *mixers is a pointer to the configuration struct, declared in mix_and_link.h - * @param nr_mixers is a integer that denotes the number of mixers that should be processed - * @param nr_actuators is a integer that denotes the max number of actuators that get linked to one source - * @param *data is a pointer to the mixer data struct, declared in mix_and_link.h - */ -void mix_and_link(const mixer_conf_t *mixers, uint8_t nr_mixers, uint8_t nr_actuators, mixer_data_t *data) -{ - - /* Reset the Output Array */ - uint8_t i, j; - - for (i = 0; i < nr_actuators; i++) { - data->output[i] = 0; - } - - /* Loop throug the given mixers */ - for (i = 0; i < nr_mixers; i++) { - - /* The actual mixing */ - for (j = 0; j < mixers[i].nr_actuators; j++) { - data->output[mixers[i].dest[j]] += (int16_t)(data->input[mixers[i].source] - * mixers[i].dual_rate[j]); - - /* Saturate to +-10000 */ - if (data->output[mixers[i].dest[j]] > 10000) - data->output[mixers[i].dest[j]] = 10000; - else if (data->output[mixers[i].dest[j]] < -10000) - data->output[mixers[i].dest[j]] = -10000; - - } - } -} - - diff --git a/apps/mix_and_link/mix_and_link.h b/apps/mix_and_link/mix_and_link.h deleted file mode 100644 index 0a374fd85..000000000 --- a/apps/mix_and_link/mix_and_link.h +++ /dev/null @@ -1,115 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Nils Wenzler <wenzlern@ethz.ch> - * - * 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. - * - ****************************************************************************/ - - -#ifndef _MIX_AND_LINK_H_ -#define _MIX_AND_LINK_H_ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdint.h> - -/**< The maximum number of actuators that get linked to one abstract function*/ -#define NR_MAX_ACTUATORS 2 - -/** - * The struct mixer_conf_t contains all the necessary information for one mixer. - * Make an array of type mixer_conf_t for several mix functions. - */ -typedef struct { - uint8_t source; /**< source RC channel index 0..n */ - uint8_t nr_actuators; /**< number of actuators to output to */ - uint8_t dest[NR_MAX_ACTUATORS]; /**< Which actuators to output to */ - float dual_rate[NR_MAX_ACTUATORS]; /**< Direction and rate of mixing. Range [-1,1] */ -} mixer_conf_t; /**< Setup for one mixer */ - -/* - * The struct mixer_data contains two int16_t arrays with the abstract source functions - * (Throttle, Roll,...) in the input, and an empty array output of the desired - * size for the results. - */ -typedef struct { - int16_t *input; - int16_t *output; -} mixer_data_t; - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/* - * Mixes the functions to the actuators. - * e.g. Throttle, Roll,... - * - * Each mixer_conf struct in the mixers array contains the source function (ex. THROTTLE), the number - * of actuators, an array with the desired actuators, Dual Rate determines which - * percentage of the source gets mixed to the destination and sets the direction. - * It's range is between -1 and 1. - * - * EXAMPLE: Deltamix for a wing plane: - * - * mixer_data_t mixer_buffer; - * mixer_buffer.input = buffer_rc; - * mixer_buffer.output = buffer_servo; - * - * - * mixer_conf_t mixers[3]; - * - * mixers[0].source = PITCH; - * mixers[0].nr_actuators = 2; - * mixers[0].dest[0] = AIL_1; - * mixers[0].dest[1] = AIL_2; - * mixers[0].dual_rate[0] = 1; - * mixers[0].dual_rate[1] = 1; - * - * mixers[1].source = ROLL; - * mixers[1].nr_actuators = 2; - * mixers[1].dest[0] = AIL_1; - * mixers[1].dest[1] = AIL_2; - * mixers[1].dual_rate[0] = 1; - * mixers[1].dual_rate[0] = -1; - * - * mixers[2].source = THROTTLE; - * mixers[2].nr_actuators = 1; - * mixers[2].dest[0] = MOT; - * mixers[2].dual_rate[0] = 1; - * - * @param mixers pointer to the mixer struct array - * @param nr_mixers number of mixers in struct array - * @param nr_actuators number of actuators - * @param data pointer to the mixer_data struct. - */ -void mix_and_link(const mixer_conf_t *mixers, uint8_t nr_mixers, uint8_t nr_actuators, mixer_data_t *data); - -#endif diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 33e7ab7e7..9a23e8290 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -96,8 +96,6 @@ mc_thread_main(int argc, char *argv[]) memset(&raw, 0, sizeof(raw)); struct ardrone_motors_setpoint_s setpoint; memset(&setpoint, 0, sizeof(setpoint)); - struct actuator_controls_s actuator_controls; - memset(&actuator_controls, 0, sizeof(actuator_controls)); struct actuator_controls_s actuators; struct actuator_armed_s armed; @@ -153,12 +151,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = 0.0f; att_sp.thrust = 0.1f; } else { - if (state.state_machine == SYSTEM_STATE_MANUAL || - state.state_machine == SYSTEM_STATE_GROUND_READY || - state.state_machine == SYSTEM_STATE_STABILIZED || - state.state_machine == SYSTEM_STATE_AUTO || - state.state_machine == SYSTEM_STATE_MISSION_ABORT || - state.state_machine == SYSTEM_STATE_EMCY_LANDING) { + if (state.flag_system_armed) { att_sp.thrust = manual.throttle; armed.armed = true; @@ -173,7 +166,7 @@ mc_thread_main(int argc, char *argv[]) } - multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode); + multirotor_control_attitude(&att_sp, &att, &state, &actuators, motor_test_mode); /* publish the result */ orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); diff --git a/nuttx/configs/px4fmu/include/drv_mpu6000.h b/nuttx/configs/px4fmu/include/drv_mpu6000.h deleted file mode 100644 index 0a5a48b70..000000000 --- a/nuttx/configs/px4fmu/include/drv_mpu6000.h +++ /dev/null @@ -1,92 +0,0 @@ -/* - * Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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. - */ - -/* - * Driver for the Invense MPU-6000 gyroscope - */ - -/* IMPORTANT NOTES: - * - * SPI max. clock frequency: 10 Mhz - * CS has to be high before transfer, - * go low right before transfer and - * go high again right after transfer - * - */ - -#include <sys/ioctl.h> - -#define _MPU6000BASE 0x7600 -#define MPU6000C(_x) _IOC(_MPU6000BASE, _x) - -/* - * Sets the sensor internal sampling rate, and if a buffer - * has been configured, the rate at which entries will be - * added to the buffer. - */ -#define MPU6000_SETRATE MPU6000C(1) - -#define MPU6000_RATE_95HZ_LP_12_5HZ ((0<<7) | (0<<6) | (0<<5) | (0<<4)) - -/* - * Sets the sensor internal range. - */ -#define MPU6000_SETRANGE MPU6000C(2) - -#define MPU6000_RANGE_250DPS (0<<4) - -#define MPU6000_RATE_95HZ ((0<<6) | (0<<4)) - - - -/* - * Sets the address of a shared MPU6000_buffer - * structure that is maintained by the driver. - * - * If zero is passed as the address, disables - * the buffer updating. - */ -#define MPU6000_SETBUFFER MPU6000C(3) - -struct MPU6000_buffer { - uint32_t size; /* number of entries in the samples[] array */ - uint32_t next; /* the next entry that will be populated */ - struct { - uint16_t x; - uint16_t y; - uint16_t z; - uint16_t roll; - uint16_t pitch; - uint16_t yaw; - } samples[]; -}; - -extern int mpu6000_attach(struct spi_dev_s *spi, int spi_id); diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index a99b5f6ad..834b18165 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -71,12 +71,10 @@ CONFIGURED_APPS += gps CONFIGURED_APPS += commander #CONFIGURED_APPS += sdlog CONFIGURED_APPS += sensors -CONFIGURED_APPS += ardrone_control CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += px4/attitude_estimator_bm CONFIGURED_APPS += fixedwing_control -CONFIGURED_APPS += mix_and_link CONFIGURED_APPS += position_estimator CONFIGURED_APPS += attitude_estimator_ekf diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile index 2e3138aaa..5a60b7d19 100644 --- a/nuttx/configs/px4fmu/src/Makefile +++ b/nuttx/configs/px4fmu/src/Makefile @@ -42,9 +42,9 @@ AOBJS = $(ASRCS:.S=$(OBJEXT)) CSRCS = up_boot.c up_leds.c up_spi.c up_hrt.c \ drv_gpio.c drv_bma180.c drv_l3gd20.c \ - drv_led.c drv_hmc5833l.c drv_ms5611.c drv_eeprom.c \ + drv_led.c drv_eeprom.c \ drv_tone_alarm.c up_pwm_servo.c up_usbdev.c \ - up_cpuload.c drv_mpu6000.c + up_cpuload.c ifeq ($(CONFIG_NSH_ARCHINIT),y) CSRCS += up_nsh.c diff --git a/nuttx/configs/px4fmu/src/drv_mpu6000.c b/nuttx/configs/px4fmu/src/drv_mpu6000.c deleted file mode 100644 index aded70318..000000000 --- a/nuttx/configs/px4fmu/src/drv_mpu6000.c +++ /dev/null @@ -1,433 +0,0 @@ -/* - * Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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. - */ - -/* - * Driver for the ST mpu6000 MEMS gyroscope - */ - -#include <nuttx/config.h> - -#include <stdint.h> -#include <stdbool.h> -#include <debug.h> -#include <errno.h> -#include <unistd.h> -#include <stdio.h> - -#include <nuttx/spi.h> -#include <arch/board/board.h> -#include <nuttx/arch.h> - -#include "chip.h" -#include "stm32_internal.h" -#include "px4fmu-internal.h" - -#include <arch/board/drv_mpu6000.h> - -#define DIR_READ (0x80) -#define DIR_WRITE (0<<7) -#define ADDR_INCREMENT (1<<6) - -#define WHO_I_AM 0xD4 - -// MPU 6000 registers -#define MPUREG_WHOAMI 0x75 // -#define MPUREG_SMPLRT_DIV 0x19 // -#define MPUREG_CONFIG 0x1A // -#define MPUREG_GYRO_CONFIG 0x1B -#define MPUREG_ACCEL_CONFIG 0x1C -#define MPUREG_FIFO_EN 0x23 -#define MPUREG_INT_PIN_CFG 0x37 -#define MPUREG_INT_ENABLE 0x38 -#define MPUREG_INT_STATUS 0x3A -#define MPUREG_ACCEL_XOUT_H 0x3B // -#define MPUREG_ACCEL_XOUT_L 0x3C // -#define MPUREG_ACCEL_YOUT_H 0x3D // -#define MPUREG_ACCEL_YOUT_L 0x3E // -#define MPUREG_ACCEL_ZOUT_H 0x3F // -#define MPUREG_ACCEL_ZOUT_L 0x40 // -#define MPUREG_TEMP_OUT_H 0x41// -#define MPUREG_TEMP_OUT_L 0x42// -#define MPUREG_GYRO_XOUT_H 0x43 // -#define MPUREG_GYRO_XOUT_L 0x44 // -#define MPUREG_GYRO_YOUT_H 0x45 // -#define MPUREG_GYRO_YOUT_L 0x46 // -#define MPUREG_GYRO_ZOUT_H 0x47 // -#define MPUREG_GYRO_ZOUT_L 0x48 // -#define MPUREG_USER_CTRL 0x6A // -#define MPUREG_PWR_MGMT_1 0x6B // -#define MPUREG_PWR_MGMT_2 0x6C // -#define MPUREG_FIFO_COUNTH 0x72 -#define MPUREG_FIFO_COUNTL 0x73 -#define MPUREG_FIFO_R_W 0x74 -#define MPUREG_PRODUCT_ID 0x0C // Product ID Register - - -// Configuration bits MPU 3000 and MPU 6000 (not revised)? -#define BIT_SLEEP 0x40 -#define BIT_H_RESET 0x80 -#define BITS_CLKSEL 0x07 -#define MPU_CLK_SEL_PLLGYROX 0x01 -#define MPU_CLK_SEL_PLLGYROZ 0x03 -#define MPU_EXT_SYNC_GYROX 0x02 -#define BITS_FS_250DPS 0x00 -#define BITS_FS_500DPS 0x08 -#define BITS_FS_1000DPS 0x10 -#define BITS_FS_2000DPS 0x18 -#define BITS_FS_MASK 0x18 -#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 -#define BITS_DLPF_CFG_188HZ 0x01 -#define BITS_DLPF_CFG_98HZ 0x02 -#define BITS_DLPF_CFG_42HZ 0x03 -#define BITS_DLPF_CFG_20HZ 0x04 -#define BITS_DLPF_CFG_10HZ 0x05 -#define BITS_DLPF_CFG_5HZ 0x06 -#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 -#define BITS_DLPF_CFG_MASK 0x07 -#define BIT_INT_ANYRD_2CLEAR 0x10 -#define BIT_RAW_RDY_EN 0x01 -#define BIT_I2C_IF_DIS 0x10 -#define BIT_INT_STATUS_DATA 0x01 - // Product ID Description for MPU6000 - // high 4 bits low 4 bits - // Product Name Product Revision -#define MPU6000ES_REV_C4 0x14 // 0001 0100 -#define MPU6000ES_REV_C5 0x15 // 0001 0101 -#define MPU6000ES_REV_D6 0x16 // 0001 0110 -#define MPU6000ES_REV_D7 0x17 // 0001 0111 -#define MPU6000ES_REV_D8 0x18 // 0001 1000 -#define MPU6000_REV_C4 0x54 // 0101 0100 -#define MPU6000_REV_C5 0x55 // 0101 0101 -#define MPU6000_REV_D6 0x56 // 0101 0110 -#define MPU6000_REV_D7 0x57 // 0101 0111 -#define MPU6000_REV_D8 0x58 // 0101 1000 -#define MPU6000_REV_D9 0x59 // 0101 1001 -#define MPU6000_REV_D10 0x5A // 0101 1010 - -static FAR struct mpu6000_dev_s mpu6000_dev; - -static ssize_t mpu6000_read(struct file *filp, FAR char *buffer, size_t buflen); -static int mpu6000_ioctl(struct file *filp, int cmd, unsigned long arg); - -static const struct file_operations mpu6000_fops = { - .open = 0, - .close = 0, - .read = mpu6000_read, - .write = 0, - .seek = 0, - .ioctl = mpu6000_ioctl, -#ifndef CONFIG_DISABLE_POLL - .poll = 0 -#endif -}; - -struct mpu6000_dev_s -{ - struct spi_dev_s *spi; - int spi_id; - uint8_t rate; - struct mpu6000_buffer *buffer; -}; - -static void mpu6000_write_reg(uint8_t address, uint8_t data); -static uint8_t mpu6000_read_reg(uint8_t address); - -static void -mpu6000_write_reg(uint8_t address, uint8_t data) -{ - uint8_t cmd[2] = { address | DIR_WRITE, data }; - - SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true); - SPI_SNDBLOCK(mpu6000_dev.spi, &cmd, sizeof(cmd)); - SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false); -} - -static uint8_t -mpu6000_read_reg(uint8_t address) -{ - uint8_t cmd[2] = {address | DIR_READ, 0}; - uint8_t data[2]; - - SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true); - SPI_EXCHANGE(mpu6000_dev.spi, cmd, data, sizeof(cmd)); - SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false); - - return data[1]; -} - -static int16_t -mpu6000_read_int16(uint8_t address) -{ - uint8_t cmd[3] = {address | DIR_READ, 0, 0}; - uint8_t data[3]; - - SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true); - SPI_EXCHANGE(mpu6000_dev.spi, cmd, data, sizeof(cmd)); - SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false); - - return (((int16_t)data[1])<<8) | data[2]; -} - -static int -mpu6000_set_range(uint8_t range) -{ -// /* mask out illegal bit positions */ -// uint8_t write_range = range & REG4_RANGE_MASK; -// /* immediately return if user supplied invalid value */ -// if (write_range != range) return EINVAL; -// /* set remaining bits to a sane value */ -// write_range |= REG4_BDU; -// /* write to device */ -// write_reg(ADDR_CTRL_REG4, write_range); -// /* return 0 if register value is now written value, 1 if unchanged */ -// return !(read_reg(ADDR_CTRL_REG4) == write_range); -} - -static int -mpu6000_set_rate(uint8_t rate) -{ -// /* mask out illegal bit positions */ -// uint8_t write_rate = rate & REG1_RATE_LP_MASK; -// /* immediately return if user supplied invalid value */ -// if (write_rate != rate) return EINVAL; -// /* set remaining bits to a sane value */ -// write_rate |= REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; -// /* write to device */ -// write_reg(ADDR_CTRL_REG1, write_rate); -// /* return 0 if register value is now written value, 1 if unchanged */ -// return !(read_reg(ADDR_CTRL_REG1) == write_rate); -} - -static int -mpu6000_read_fifo(int16_t *data) -{ -// struct { /* status register and data as read back from the device */ -// uint8_t cmd; -// uint8_t temp; -// uint8_t status; -// int16_t x; -// int16_t y; -// int16_t z; -// } __attribute__((packed)) report; -// -// report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; -// -// /* exchange the report structure with the device */ -// SPI_LOCK(mpu6000_dev.spi, true); -// -// SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true); -// -// read_reg(ADDR_WHO_AM_I); -// -// SPI_EXCHANGE(mpu6000_dev.spi, &report, &report, sizeof(report)); -// SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false); -// -// SPI_LOCK(mpu6000_dev.spi, false); -// -// -// - // - - // Device has MSB first at lower address (big endian) - - - struct { /* status register and data as read back from the device */ - // uint8_t cmd; - // uint8_t int_status; - int16_t xacc; - int16_t yacc; - int16_t zacc; - int16_t temp; - int16_t rollspeed; - int16_t pitchspeed; - int16_t yawspeed; - } report; - - uint8_t cmd[sizeof(report)]; - cmd[0] = MPUREG_ACCEL_XOUT_H | DIR_READ; // was addr_incr - - SPI_LOCK(mpu6000_dev.spi, true); - SPI_SELECT(mpu6000_dev.spi, PX4_SPIDEV_MPU, true); - report.xacc = mpu6000_read_int16(MPUREG_ACCEL_XOUT_H); - report.yacc = mpu6000_read_int16(MPUREG_ACCEL_YOUT_H); - report.zacc = mpu6000_read_int16(MPUREG_ACCEL_ZOUT_H); - report.rollspeed = mpu6000_read_int16(MPUREG_GYRO_XOUT_H); - report.pitchspeed = mpu6000_read_int16(MPUREG_GYRO_YOUT_H); - report.yawspeed = mpu6000_read_int16(MPUREG_GYRO_ZOUT_H); - SPI_SELECT(mpu6000_dev.spi, PX4_SPIDEV_MPU, false); - SPI_LOCK(mpu6000_dev.spi, false); - - data[0] = report.xacc; - data[1] = report.yacc; - data[2] = report.zacc; - data[3] = report.rollspeed; - data[4] = report.pitchspeed; - data[5] = report.yawspeed; - - return 1;//(report.int_status & 0x01); -} - -static ssize_t -mpu6000_read(struct file *filp, char *buffer, size_t buflen) -{ - /* if the buffer is large enough, and data are available, return success */ - if (buflen >= 12) { - if (mpu6000_read_fifo((int16_t *)buffer)) - return 12; - - /* no data */ - return 0; - } - - /* buffer too small */ - errno = ENOSPC; - return ERROR; -} - -static int -mpu6000_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int result = ERROR; - - switch (cmd) { - case MPU6000_SETRATE: - if ((arg & 0x00/* XXX REG MASK MISSING */) == arg) { - SPI_LOCK(mpu6000_dev.spi, true); - mpu6000_set_rate(arg); - SPI_LOCK(mpu6000_dev.spi, false); - result = 0; - mpu6000_dev.rate = arg; - } - break; - - case MPU6000_SETRANGE: - if ((arg & 0x00/* XXX REG MASK MISSING */) == arg) { - SPI_LOCK(mpu6000_dev.spi, true); - mpu6000_set_range(arg); - SPI_LOCK(mpu6000_dev.spi, false); - result = 0; - } - break; - - case MPU6000_SETBUFFER: - mpu6000_dev.buffer = (struct mpu6000_buffer *)arg; - result = 0; - break; - } - - if (result) - errno = EINVAL; - return result; -} - -int -mpu6000_attach(struct spi_dev_s *spi, int spi_id) -{ - int result = ERROR; - - mpu6000_dev.spi = spi; - mpu6000_dev.spi_id = spi_id; - - SPI_LOCK(mpu6000_dev.spi, true); - - // Set sensor-specific SPI mode - SPI_SETFREQUENCY(mpu6000_dev.spi, 10000000); // 500 KHz - SPI_SETBITS(mpu6000_dev.spi, 8); - // Either mode 1 or mode 3 - SPI_SETMODE(mpu6000_dev.spi, SPIDEV_MODE3); - - // Chip reset - mpu6000_write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - up_udelay(10000); - // Wake up device and select GyroZ clock (better performance) - mpu6000_write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); - up_udelay(1000); - // Disable I2C bus (recommended on datasheet) - mpu6000_write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); - up_udelay(1000); - // SAMPLE RATE - mpu6000_write_reg(MPUREG_SMPLRT_DIV,0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz - usleep(1000); - // FS & DLPF FS=2000¼/s, DLPF = 98Hz (low pass filter) - mpu6000_write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ); - usleep(1000); - mpu6000_write_reg(MPUREG_GYRO_CONFIG,BITS_FS_2000DPS); // Gyro scale 2000¼/s - usleep(1000); - - uint8_t _product_id = mpu6000_read_reg(MPUREG_PRODUCT_ID); - printf("MPU-6000 product id: %d\n", (int)_product_id); - - if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5) || - (_product_id == MPU6000_REV_C4) || (_product_id == MPU6000_REV_C5)){ - // Accel scale 8g (4096 LSB/g) - // Rev C has different scaling than rev D - mpu6000_write_reg(MPUREG_ACCEL_CONFIG,1<<3); - } else { - // Accel scale 8g (4096 LSB/g) - mpu6000_write_reg(MPUREG_ACCEL_CONFIG,2<<3); - } - usleep(1000); - - // INT CFG => Interrupt on Data Ready - mpu6000_write_reg(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN); // INT: Raw data ready - usleep(1000); - mpu6000_write_reg(MPUREG_INT_PIN_CFG,BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read - usleep(1000); - // Oscillator set - // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); - usleep(1000); - - /* revert back to normal bus mode */ - SPI_SETFREQUENCY(mpu6000_dev.spi, 10000000); - SPI_SETBITS(mpu6000_dev.spi, 8); - SPI_SETMODE(mpu6000_dev.spi, SPIDEV_MODE3); - - /* verify that the device is attached and functioning */ - if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5) || - (_product_id == MPU6000_REV_C4) || (_product_id == MPU6000_REV_C5) || - (_product_id == MPU6000_REV_D7) || (_product_id == MPU6000_REV_D8) || - (_product_id == MPU6000_REV_D9) || (_product_id == MPU6000_REV_D10)){ - - /* make ourselves available */ - register_driver("/dev/mpu6000", &mpu6000_fops, 0666, NULL); - - result = OK; - } else { - - errno = EIO; - } - - SPI_LOCK(mpu6000_dev.spi, false); - - SPI_LOCK(mpu6000_dev.spi, false); - - return result; -} diff --git a/nuttx/configs/px4fmu/src/up_cpuload.c b/nuttx/configs/px4fmu/src/up_cpuload.c index 750ec4852..bf867ae8b 100644 --- a/nuttx/configs/px4fmu/src/up_cpuload.c +++ b/nuttx/configs/px4fmu/src/up_cpuload.c @@ -45,6 +45,7 @@ #include <debug.h> #include <sys/time.h> +#include <sched.h> #include <arch/board/board.h> #include <arch/board/up_hrt.h> diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c index d185bf7f1..25f3c2795 100644 --- a/nuttx/configs/px4fmu/src/up_nsh.c +++ b/nuttx/configs/px4fmu/src/up_nsh.c @@ -1,5 +1,5 @@ /**************************************************************************** - * config/stm3210e_eval/src/up_nsh.c + * config/px4fmu/src/up_nsh.c * arch/arm/src/board/up_nsh.c * * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. @@ -63,11 +63,8 @@ #include <arch/board/board.h> #include <arch/board/drv_bma180.h> #include <arch/board/drv_l3gd20.h> -#include <arch/board/drv_hmc5883l.h> -#include <arch/board/drv_mpu6000.h> -#include <arch/board/drv_ms5611.h> -#include <arch/board/drv_eeprom.h> #include <arch/board/drv_led.h> +#include <arch/board/drv_eeprom.h> /**************************************************************************** * Pre-Processor Definitions @@ -96,20 +93,6 @@ ****************************************************************************/ /**************************************************************************** - * Name: multiport_setup - * - * Description: - * Perform setup of the PX4FMU's multi function ports - * - ****************************************************************************/ -int multiport_setup(void) -{ - int result = OK; - - return result; -} - -/**************************************************************************** * Public Functions ****************************************************************************/ @@ -221,7 +204,7 @@ int nsh_archinitialize(void) up_udelay(1000); } - if (gyro_fail) message("[boot] FAILED to attach L3GD20 gyro\r\n"); + if (!gyro_fail) message("[boot] Found L3GD20 gyro\n"); int acc_attempts = 0; int acc_fail = 0; @@ -234,13 +217,13 @@ int nsh_archinitialize(void) up_udelay(1000); } - if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n"); + if (!acc_fail) message("[boot] Found BMA180 accelerometer\n"); /* initialize I2C2 bus */ i2c2 = up_i2cinitialize(2); if (!i2c2) { - message("[boot] FAILED to initialize I2C bus 2\r\n"); + message("[boot] FAILED to initialize I2C bus 2\n"); up_ledon(LED_AMBER); return -ENODEV; } @@ -251,7 +234,7 @@ int nsh_archinitialize(void) i2c3 = up_i2cinitialize(3); if (!i2c3) { - message("[boot] FAILED to initialize I2C bus 3\r\n"); + message("[boot] FAILED to initialize I2C bus 3\n"); up_ledon(LED_AMBER); return -ENODEV; } @@ -259,91 +242,41 @@ int nsh_archinitialize(void) /* set I2C3 speed */ I2C_SETFREQUENCY(i2c3, 400000); - int mag_attempts = 0; - int mag_fail = 0; - - while (mag_attempts < 5) - { - mag_fail = hmc5883l_attach(i2c2); - mag_attempts++; - if (mag_fail == 0) break; - up_udelay(1000); - } - - if (mag_fail) message("[boot] FAILED to attach HMC5883L magnetometer\r\n"); - - int baro_attempts = 0; - int baro_fail = 0; - while (baro_attempts < 5) - { - baro_fail = ms5611_attach(i2c2); - baro_attempts++; - if (baro_fail == 0) break; - up_udelay(1000); - } - - if (baro_fail) message("[boot] FAILED to attach MS5611 baro at addr #1 or #2 (0x76 or 0x77)\r\n"); - /* try to attach, don't fail if device is not responding */ (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS, FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES, FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES, FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1); -#if 0 - int eeprom_attempts = 0; - int eeprom_fail; - while (eeprom_attempts < 5) - { - /* try to attach, fail if device does not respond */ - eeprom_fail = eeprom_attach(i2c2, FMU_ONBOARD_EEPROM_ADDRESS, - FMU_ONBOARD_EEPROM_TOTAL_SIZE_BYTES, - FMU_ONBOARD_EEPROM_PAGE_SIZE_BYTES, - FMU_ONBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/eeprom", 1); - eeprom_attempts++; - if (eeprom_fail == OK) break; - up_udelay(1000); - } - - if (eeprom_fail) message("[boot] FAILED to attach FMU EEPROM\r\n"); - -#endif - - /* Report back sensor status */ - if (gyro_fail || mag_fail || baro_fail/* || eeprom_fail*/) - { - up_ledon(LED_AMBER); - } - #if defined(CONFIG_STM32_SPI3) /* Get the SPI port */ - message("[boot] Initializing SPI port 3\r\n"); + message("[boot] Initializing SPI port 3\n"); spi3 = up_spiinitialize(3); if (!spi3) { - message("[boot] FAILED to initialize SPI port 3\r\n"); + message("[boot] FAILED to initialize SPI port 3\n"); up_ledon(LED_AMBER); return -ENODEV; } - message("[boot] Successfully initialized SPI port 3\r\n"); + message("[boot] Successfully initialized SPI port 3\n"); /* Now bind the SPI interface to the MMCSD driver */ result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); if (result != OK) { - message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\r\n"); + message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); up_ledon(LED_AMBER); return -ENODEV; } - message("[boot] Successfully bound SPI port 3 to the MMCSD driver\r\n"); + message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); #endif /* SPI3 */ /* initialize I2C1 bus */ i2c1 = up_i2cinitialize(1); if (!i2c1) { - message("[boot] FAILED to initialize I2C bus 1\r\n"); + message("[boot] FAILED to initialize I2C bus 1\n"); up_ledon(LED_AMBER); return -ENODEV; } @@ -355,7 +288,7 @@ int nsh_archinitialize(void) /* Get board information if available */ - /* Initialize the user GPIOs */ + /* Initialize the user GPIOs */ px4fmu_gpio_init(); #ifdef CONFIG_ADC @@ -367,7 +300,7 @@ int nsh_archinitialize(void) if (adc_state != OK) { /* Give up */ - message("[boot] FAILED adc_devinit: %d\r\n", adc_state); + message("[boot] FAILED adc_devinit: %d\n", adc_state); return -ENODEV; } } |