zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Cleaned up / simplified position control, attacking pos control implementation next

sbg
Lorenz Meier 13 years ago
parent
2a06b66845
commit
607e902b88
5 changed files with 310 additions and 230 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      apps/examples/px4_deamon_app/px4_deamon_app.c
  2. 112
      apps/multirotor_pos_control/multirotor_pos_control.c
  3. 410
      apps/multirotor_pos_control/position_control.c
  4. 12
      apps/multirotor_pos_control/position_control.h
  5. 1
      nuttx/configs/px4fmu/nsh/appconfig

5
apps/examples/px4_deamon_app/px4_deamon_app.c
Unescape View File

@ -97,7 +97,6 @@ int px4_deamon_app_main(int argc, char *argv[])
4096, 4096,
px4_deamon_thread_main, px4_deamon_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL); (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0); exit(0);
} }
@ -123,6 +122,8 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
printf("[deamon] starting\n"); printf("[deamon] starting\n");
thread_running = true;
while (!thread_should_exit) { while (!thread_should_exit) {
printf("Hello Deamon!\n"); printf("Hello Deamon!\n");
sleep(10); sleep(10);
@ -130,5 +131,7 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
printf("[deamon] exiting.\n"); printf("[deamon] exiting.\n");
thread_running = false;
return 0; return 0;
} }

112
apps/multirotor_pos_control/multirotor_pos_control.c
Unescape View File

@ -49,24 +49,92 @@
#include <time.h> #include <time.h>
#include <sys/prctl.h> #include <sys/prctl.h>
#include <arch/board/up_hrt.h> #include <arch/board/up_hrt.h>
#include "ardrone_control.h"
#include "attitude_control.h"
#include "rate_control.h"
#include "ardrone_motor_control.h"
#include "position_control.h"
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <systemlib/systemlib.h>
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
__EXPORT int multirotor_pos_control_main(int argc, char *argv[]); __EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
static bool thread_should_exit; /**
static bool thread_running = false; * Mainloop of position controller.
static int mpc_task; */
static int multirotor_pos_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn().
*/
int multirotor_pos_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("multirotor pos control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("multirotor pos control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 60,
4096,
multirotor_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tmultirotor pos control app is running\n");
} else {
printf("\tmultirotor pos control app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
static int static int
mpc_thread_main(int argc, char *argv[]) multirotor_pos_control_thread_main(int argc, char *argv[])
{ {
/* welcome user */ /* welcome user */
printf("[multirotor pos control] Control started, taking over position control\n"); printf("[multirotor pos control] Control started, taking over position control\n");
@ -91,6 +159,8 @@ mpc_thread_main(int argc, char *argv[])
/* publish attitude setpoint */ /* publish attitude setpoint */
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
thread_running = true;
while (1) { while (1) {
/* get a local copy of the vehicle state */ /* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state); orb_copy(ORB_ID(vehicle_status), state_sub, &state);
@ -103,11 +173,19 @@ mpc_thread_main(int argc, char *argv[])
/* get a local copy of local position setpoint */ /* get a local copy of local position setpoint */
orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
if (state.state_machine == SYSTEM_STATE_AUTO) { // if (state.state_machine == SYSTEM_STATE_AUTO) {
position_control(&state, &manual, &att, &local_pos, &local_pos_sp, &att_sp);
// XXX IMPLEMENT POSITION CONTROL HERE
att_sp.roll_body = 0.1f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.4f;
att_sp.timestamp = hrt_absolute_time();
/* publish new attitude setpoint */ /* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
} else if (state.state_machine == SYSTEM_STATE_STABILIZE) { // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
/* set setpoint to current position */ /* set setpoint to current position */
// XXX select pos reset channel on remote // XXX select pos reset channel on remote
/* reset setpoint to current position (position hold) */ /* reset setpoint to current position (position hold) */
@ -117,19 +195,17 @@ mpc_thread_main(int argc, char *argv[])
// local_pos_sp.z = local_pos.z; // local_pos_sp.z = local_pos.z;
// local_pos_sp.yaw = att.yaw; // local_pos_sp.yaw = att.yaw;
// } // }
} // }
/* run at approximately 50 Hz */ /* run at approximately 50 Hz */
usleep(20000); usleep(20000);
counter++;
} }
/* close uarts */ printf("[multirotor pos control] ending now...\n");
close(ardrone_write);
ar_multiplexing_deinit(gpios); thread_running = false;
printf("[multirotor pos control] ending now...\r\n");
fflush(stdout); fflush(stdout);
return 0; return 0;
} }

410
apps/multirotor_pos_control/position_control.c
Unescape View File

@ -1,235 +1,235 @@
/**************************************************************************** // /****************************************************************************
* // *
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. // * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch> // * Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch> // * @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch> // * @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch> // * @author Martin Rutschmann <rutmarti@student.ethz.ch>
* // *
* Redistribution and use in source and binary forms, with or without // * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions // * modification, are permitted provided that the following conditions
* are met: // * are met:
* // *
* 1. Redistributions of source code must retain the above copyright // * 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer. // * notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright // * 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in // * notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the // * the documentation and/or other materials provided with the
* distribution. // * distribution.
* 3. Neither the name PX4 nor the names of its contributors may be // * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software // * used to endorse or promote products derived from this software
* without specific prior written permission. // * without specific prior written permission.
* // *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS // * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE // * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, // * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, // * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS // * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED // * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT // * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN // * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE. // * POSSIBILITY OF SUCH DAMAGE.
* // *
****************************************************************************/ // ****************************************************************************/
/** // /**
* @file multirotor_position_control.c // * @file multirotor_position_control.c
* Implementation of the position control for a multirotor VTOL // * Implementation of the position control for a multirotor VTOL
*/ // */
#include <stdio.h> // #include <stdio.h>
#include <stdlib.h> // #include <stdlib.h>
#include <stdio.h> // #include <stdio.h>
#include <stdint.h> // #include <stdint.h>
#include <math.h> // #include <math.h>
#include <stdbool.h> // #include <stdbool.h>
#include <float.h> // #include <float.h>
#include <systemlib/pid/pid.h> // #include <systemlib/pid/pid.h>
#include "multirotor_position_control.h" // #include "multirotor_position_control.h"
void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, // void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, // const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) // const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
{ // {
static PID_t distance_controller; // static PID_t distance_controller;
static int read_ret; // static int read_ret;
static global_data_position_t position_estimated; // static global_data_position_t position_estimated;
static uint16_t counter; // static uint16_t counter;
static bool initialized; // static bool initialized;
static uint16_t pm_counter; // static uint16_t pm_counter;
static float lat_next; // static float lat_next;
static float lon_next; // static float lon_next;
static float pitch_current; // static float pitch_current;
static float thrust_total; // static float thrust_total;
if (initialized == false) { // if (initialized == false) {
pid_init(&distance_controller, // pid_init(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], // global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], // global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], // global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], // global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
PID_MODE_DERIVATIV_CALC, 150);//150 // PID_MODE_DERIVATIV_CALC, 150);//150
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; // // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; // // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
thrust_total = 0.0f; // thrust_total = 0.0f;
/* Position initialization */ // /* Position initialization */
/* Wait for new position estimate */ // /* Wait for new position estimate */
do { // do {
read_ret = read_lock_position(&position_estimated); // read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0); // } while (read_ret != 0);
lat_next = position_estimated.lat; // lat_next = position_estimated.lat;
lon_next = position_estimated.lon; // lon_next = position_estimated.lon;
/* attitude initialization */ // /* attitude initialization */
global_data_lock(&global_data_attitude->access_conf); // global_data_lock(&global_data_attitude->access_conf);
pitch_current = global_data_attitude->pitch; // pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf); // global_data_unlock(&global_data_attitude->access_conf);
initialized = true; // initialized = true;
} // }
/* load new parameters with 10Hz */ // /* load new parameters with 10Hz */
if (counter % 50 == 0) { // if (counter % 50 == 0) {
if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { // if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
/* check whether new parameters are available */ // /* check whether new parameters are available */
if (global_data_parameter_storage->counter > pm_counter) { // if (global_data_parameter_storage->counter > pm_counter) {
pid_set_parameters(&distance_controller, // pid_set_parameters(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], // global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], // global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], // global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); // global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
// // //
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; // // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; // // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
pm_counter = global_data_parameter_storage->counter; // pm_counter = global_data_parameter_storage->counter;
printf("Position controller changed pid parameters\n"); // printf("Position controller changed pid parameters\n");
} // }
} // }
global_data_unlock(&global_data_parameter_storage->access_conf); // global_data_unlock(&global_data_parameter_storage->access_conf);
} // }
/* Wait for new position estimate */ // /* Wait for new position estimate */
do { // do {
read_ret = read_lock_position(&position_estimated); // read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0); // } while (read_ret != 0);
/* Get next waypoint */ //TODO: add local copy // /* Get next waypoint */ //TODO: add local copy
if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { // if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
lat_next = global_data_position_setpoint->x; // lat_next = global_data_position_setpoint->x;
lon_next = global_data_position_setpoint->y; // lon_next = global_data_position_setpoint->y;
global_data_unlock(&global_data_position_setpoint->access_conf); // global_data_unlock(&global_data_position_setpoint->access_conf);
} // }
/* Get distance to waypoint */ // /* Get distance to waypoint */
float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); // float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
// if(counter % 5 == 0) // // if(counter % 5 == 0)
// printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); // // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
/* Get bearing to waypoint (direction on earth surface to next waypoint) */ // /* Get bearing to waypoint (direction on earth surface to next waypoint) */
float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); // float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
if (counter % 5 == 0) // if (counter % 5 == 0)
printf("bearing: %.4f\n", bearing); // printf("bearing: %.4f\n", bearing);
/* Calculate speed in direction of bearing (needed for controller) */ // /* Calculate speed in direction of bearing (needed for controller) */
float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); // float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
// if(counter % 5 == 0) // // if(counter % 5 == 0)
// printf("speed_norm: %.4f\n", speed_norm); // // printf("speed_norm: %.4f\n", speed_norm);
float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller // float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
/* Control Thrust in bearing direction */ // /* Control Thrust in bearing direction */
float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, // float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else // CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
// if(counter % 5 == 0) // // if(counter % 5 == 0)
// printf("horizontal thrust: %.4f\n", horizontal_thrust); // // printf("horizontal thrust: %.4f\n", horizontal_thrust);
/* Get total thrust (from remote for now) */ // /* Get total thrust (from remote for now) */
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { // if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? // thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
global_data_unlock(&global_data_rc_channels->access_conf); // global_data_unlock(&global_data_rc_channels->access_conf);
} // }
const float max_gas = 500.0f; // const float max_gas = 500.0f;
thrust_total *= max_gas / 20000.0f; //TODO: check this // thrust_total *= max_gas / 20000.0f; //TODO: check this
thrust_total += max_gas / 2.0f; // thrust_total += max_gas / 2.0f;
if (horizontal_thrust > thrust_total) { // if (horizontal_thrust > thrust_total) {
horizontal_thrust = thrust_total; // horizontal_thrust = thrust_total;
} else if (horizontal_thrust < -thrust_total) { // } else if (horizontal_thrust < -thrust_total) {
horizontal_thrust = -thrust_total; // horizontal_thrust = -thrust_total;
} // }
//TODO: maybe we want to add a speed controller later... // //TODO: maybe we want to add a speed controller later...
/* Calclulate thrust in east and north direction */ // /* Calclulate thrust in east and north direction */
float thrust_north = cosf(bearing) * horizontal_thrust; // float thrust_north = cosf(bearing) * horizontal_thrust;
float thrust_east = sinf(bearing) * horizontal_thrust; // float thrust_east = sinf(bearing) * horizontal_thrust;
if (counter % 10 == 0) { // if (counter % 10 == 0) {
printf("thrust north: %.4f\n", thrust_north); // printf("thrust north: %.4f\n", thrust_north);
printf("thrust east: %.4f\n", thrust_east); // printf("thrust east: %.4f\n", thrust_east);
fflush(stdout); // fflush(stdout);
} // }
/* Get current attitude */ // /* Get current attitude */
if (0 == global_data_trylock(&global_data_attitude->access_conf)) { // if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
pitch_current = global_data_attitude->pitch; // pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf); // global_data_unlock(&global_data_attitude->access_conf);
} // }
/* Get desired pitch & roll */ // /* Get desired pitch & roll */
float pitch_desired = 0.0f; // float pitch_desired = 0.0f;
float roll_desired = 0.0f; // float roll_desired = 0.0f;
if (thrust_total != 0) { // if (thrust_total != 0) {
float pitch_fraction = -thrust_north / thrust_total; // float pitch_fraction = -thrust_north / thrust_total;
float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); // float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
if (roll_fraction < -1) { // if (roll_fraction < -1) {
roll_fraction = -1; // roll_fraction = -1;
} else if (roll_fraction > 1) { // } else if (roll_fraction > 1) {
roll_fraction = 1; // roll_fraction = 1;
} // }
// if(counter % 5 == 0) // // if(counter % 5 == 0)
// { // // {
// printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); // // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
// fflush(stdout); // // fflush(stdout);
// } // // }
pitch_desired = asinf(pitch_fraction); // pitch_desired = asinf(pitch_fraction);
roll_desired = asinf(roll_fraction); // roll_desired = asinf(roll_fraction);
} // }
att_sp.roll = roll_desired; // att_sp.roll = roll_desired;
att_sp.pitch = pitch_desired; // att_sp.pitch = pitch_desired;
counter++; // counter++;
} // }

12
apps/multirotor_pos_control/position_control.h
Unescape View File

@ -40,11 +40,11 @@
* Definition of the position control for a multirotor VTOL * Definition of the position control for a multirotor VTOL
*/ */
#ifndef POSITION_CONTROL_H_ // #ifndef POSITION_CONTROL_H_
#define POSITION_CONTROL_H_ // #define POSITION_CONTROL_H_
void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, // void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, // const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); // const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
#endif /* POSITION_CONTROL_H_ */ // #endif /* POSITION_CONTROL_H_ */

1
nuttx/configs/px4fmu/nsh/appconfig
Unescape View File

@ -74,6 +74,7 @@ CONFIGURED_APPS += sdlog
CONFIGURED_APPS += sensors CONFIGURED_APPS += sensors
CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += ardrone_interface
CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_att_control
CONFIGURED_APPS += multirotor_pos_control
CONFIGURED_APPS += px4/attitude_estimator_bm CONFIGURED_APPS += px4/attitude_estimator_bm
CONFIGURED_APPS += fixedwing_control CONFIGURED_APPS += fixedwing_control
CONFIGURED_APPS += position_estimator CONFIGURED_APPS += position_estimator

Write Preview
Loading…
Cancel
Save