Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • opensource/ardupilot
  • opensource/baitboat
2 results
Show changes
Commits on Source (19)
Showing
with 351 additions and 320 deletions
language: cpp
before_install:
- cd .. && ./ardupilot/Tools/scripts/install-prereqs-ubuntu.sh -y && . ~/.profile
- APMDIR=$(pwd) && pushd .. && $APMDIR/Tools/scripts/install-prereqs-ubuntu.sh -y && . ~/.profile && popd
script:
- cd ./ardupilot && Tools/scripts/build_all_travis.sh
- Tools/scripts/build_all_travis.sh
notifications:
webhooks:
......
......@@ -449,13 +449,6 @@ static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4, g.single_servo_1, g.si
static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4);
#endif
////////////////////////////////////////////////////////////////////////////////
// PIDs
////////////////////////////////////////////////////////////////////////////////
// This is used to hold radio tuning values for in-flight CH6 tuning
float tuning_value;
////////////////////////////////////////////////////////////////////////////////
// GPS variables
////////////////////////////////////////////////////////////////////////////////
......@@ -1116,8 +1109,8 @@ static void three_hz_loop()
update_events();
if(g.radio_tuning > 0)
tuning();
// update ch6 in flight tuning
tuning();
}
// one_hz_loop - runs at 1Hz
......@@ -1340,197 +1333,5 @@ static void update_altitude()
}
}
static void tuning(){
// exit immediately when radio failsafe is invoked so tuning values are not set to zero
if (failsafe.radio || failsafe.radio_counter != 0) {
return;
}
tuning_value = (float)g.rc_6.control_in / 1000.0f;
g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); // 0 to 1
switch(g.radio_tuning) {
// Roll, Pitch tuning
case CH6_STABILIZE_ROLL_PITCH_KP:
g.p_stabilize_roll.kP(tuning_value);
g.p_stabilize_pitch.kP(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KP:
g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KI:
g.pid_rate_roll.kI(tuning_value);
g.pid_rate_pitch.kI(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KD:
g.pid_rate_roll.kD(tuning_value);
g.pid_rate_pitch.kD(tuning_value);
break;
// Yaw tuning
case CH6_STABILIZE_YAW_KP:
g.p_stabilize_yaw.kP(tuning_value);
break;
case CH6_YAW_RATE_KP:
g.pid_rate_yaw.kP(tuning_value);
break;
case CH6_YAW_RATE_KD:
g.pid_rate_yaw.kD(tuning_value);
break;
// Altitude and throttle tuning
case CH6_ALTITUDE_HOLD_KP:
g.p_alt_hold.kP(tuning_value);
break;
case CH6_THROTTLE_RATE_KP:
g.p_throttle_rate.kP(tuning_value);
break;
case CH6_THROTTLE_ACCEL_KP:
g.pid_throttle_accel.kP(tuning_value);
break;
case CH6_THROTTLE_ACCEL_KI:
g.pid_throttle_accel.kI(tuning_value);
break;
case CH6_THROTTLE_ACCEL_KD:
g.pid_throttle_accel.kD(tuning_value);
break;
// Loiter and navigation tuning
case CH6_LOITER_POSITION_KP:
g.p_loiter_pos.kP(tuning_value);
break;
case CH6_LOITER_RATE_KP:
g.pid_loiter_rate_lon.kP(tuning_value);
g.pid_loiter_rate_lat.kP(tuning_value);
break;
case CH6_LOITER_RATE_KI:
g.pid_loiter_rate_lon.kI(tuning_value);
g.pid_loiter_rate_lat.kI(tuning_value);
break;
case CH6_LOITER_RATE_KD:
g.pid_loiter_rate_lon.kD(tuning_value);
g.pid_loiter_rate_lat.kD(tuning_value);
break;
case CH6_WP_SPEED:
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
wp_nav.set_speed_xy(g.rc_6.control_in);
break;
// Acro roll pitch gain
case CH6_ACRO_RP_KP:
g.acro_rp_p = tuning_value;
break;
// Acro yaw gain
case CH6_ACRO_YAW_KP:
g.acro_yaw_p = tuning_value;
break;
#if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO:
motors.ext_gyro_gain(g.rc_6.control_in);
break;
case CH6_RATE_PITCH_FF:
g.pid_rate_pitch.ff(tuning_value);
break;
case CH6_RATE_ROLL_FF:
g.pid_rate_roll.ff(tuning_value);
break;
case CH6_RATE_YAW_FF:
g.pid_rate_yaw.ff(tuning_value);
break;
#endif
case CH6_AHRS_YAW_KP:
ahrs._kp_yaw.set(tuning_value);
break;
case CH6_AHRS_KP:
ahrs._kp.set(tuning_value);
break;
case CH6_DECLINATION:
// set declination to +-20degrees
compass.set_declination(ToRad((2.0f * g.rc_6.control_in - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
break;
case CH6_CIRCLE_RATE:
// set circle rate
circle_nav.set_rate(g.rc_6.control_in/25-20); // allow approximately 45 degree turn rate in either direction
break;
case CH6_SONAR_GAIN:
// set sonar gain
g.sonar_gain.set(tuning_value);
break;
#if 0
// disabled for now - we need accessor functions
case CH6_EKF_VERTICAL_POS:
// EKF's baro vs accel (higher rely on accels more, baro impact is reduced)
ahrs.get_NavEKF()._gpsVertPosNoise = tuning_value;
break;
case CH6_EKF_HORIZONTAL_POS:
// EKF's gps vs accel (higher rely on accels more, gps impact is reduced)
ahrs.get_NavEKF()._gpsHorizPosNoise = tuning_value;
break;
case CH6_EKF_ACCEL_NOISE:
// EKF's accel noise (lower means trust accels more, gps & baro less)
ahrs.get_NavEKF()._accNoise = tuning_value;
break;
#endif
case CH6_RC_FEEL_RP:
// roll-pitch input smoothing
g.rc_feel_rp = g.rc_6.control_in / 10;
break;
case CH6_RATE_PITCH_KP:
g.pid_rate_pitch.kP(tuning_value);
break;
case CH6_RATE_PITCH_KI:
g.pid_rate_pitch.kI(tuning_value);
break;
case CH6_RATE_PITCH_KD:
g.pid_rate_pitch.kD(tuning_value);
break;
case CH6_RATE_ROLL_KP:
g.pid_rate_roll.kP(tuning_value);
break;
case CH6_RATE_ROLL_KI:
g.pid_rate_roll.kI(tuning_value);
break;
case CH6_RATE_ROLL_KD:
g.pid_rate_roll.kD(tuning_value);
break;
}
}
AP_HAL_MAIN();
......@@ -5,7 +5,10 @@
*/
#ifndef DRIFT_SPEEDGAIN
# define DRIFT_SPEEDGAIN 14.0f
# define DRIFT_SPEEDGAIN 8.0f
#endif
#ifndef DRIFT_SPEEDLIMIT
# define DRIFT_SPEEDLIMIT 560.0f
#endif
#ifndef DRIFT_THR_ASSIST_GAIN
......@@ -38,6 +41,7 @@ static bool drift_init(bool ignore_checks)
static void drift_run()
{
static float breaker = 0.0;
static float roll_input = 0.0;
int16_t target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
......@@ -63,16 +67,21 @@ static void drift_run()
float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel
float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel
float pitch_vel2 = min(fabs(pitch_vel), 800);
// gain sceduling for Yaw
float pitch_vel2 = min(fabs(pitch_vel), 2000);
target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p;
// simple gain scheduling for yaw input
target_yaw_rate = (float)(target_roll/2.0f) * (1.0f - (pitch_vel2 / 2400.0f)) * g.acro_yaw_p;
roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
roll_input = roll_input * .96 + (float)g.rc_4.control_in * .04;
roll_vel = constrain_float(roll_vel, -322, 322);
pitch_vel = constrain_float(pitch_vel, -322, 322);
//convert user input into desired roll velocity
float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN);
// always limit roll
target_roll = roll_vel * -DRIFT_SPEEDGAIN;
// Roll velocity is feed into roll acceleration to minimize slip
target_roll = roll_vel_error * -DRIFT_SPEEDGAIN;
target_roll = constrain_int16(target_roll, -4500, 4500);
// If we let go of sticks, bring us to a stop
if(target_pitch == 0){
......
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// counter to verify landings
static uint16_t land_detector = LAND_DETECTOR_TRIGGER; // we assume we are landed
static bool land_with_gps;
static uint32_t land_start_time;
......@@ -198,46 +196,6 @@ static float get_land_descent_speed()
}
}
// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
static bool land_complete_maybe()
{
return (ap.land_complete || ap.land_complete_maybe);
}
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at 50hz
static void update_land_detector()
{
// detect whether we have landed by watching for low climb rate, motors hitting their lower limit, overall low throttle and low rotation rate
if ((abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) &&
(abs(baro_climbrate) < LAND_DETECTOR_BARO_CLIMBRATE_MAX) &&
motors.limit.throttle_lower &&
#if FRAME_CONFIG != HELI_FRAME
(motors.get_throttle_out() < get_non_takeoff_throttle()) &&
#endif
(ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX)) {
if (!ap.land_complete) {
// increase counter until we hit the trigger then set land complete flag
if( land_detector < LAND_DETECTOR_TRIGGER) {
land_detector++;
}else{
set_land_complete(true);
land_detector = LAND_DETECTOR_TRIGGER;
}
}
} else {
// we've sensed movement up or down so reset land_detector
land_detector = 0;
// if throttle output is high then clear landing flag
if (motors.get_throttle_out() > get_non_takeoff_throttle()) {
set_land_complete(false);
}
}
// set land maybe flag
set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER);
}
// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
// called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
// has no effect if we are not already in LAND mode
......
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// counter to verify landings
static uint16_t land_detector = LAND_DETECTOR_TRIGGER; // we assume we are landed
// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
static bool land_complete_maybe()
{
return (ap.land_complete || ap.land_complete_maybe);
}
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at 50hz
static void update_land_detector()
{
bool climb_rate_low = (abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) && (abs(baro_climbrate) < LAND_DETECTOR_BARO_CLIMBRATE_MAX);
bool target_climb_rate_low = !pos_control.is_active_z() || (pos_control.get_desired_velocity().z < LAND_SPEED);
bool motor_at_lower_limit = motors.limit.throttle_lower;
bool throttle_low = (FRAME_CONFIG == HELI_FRAME) || (motors.get_throttle_out() < get_non_takeoff_throttle());
bool not_rotating_fast = (ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX);
if (climb_rate_low && target_climb_rate_low && motor_at_lower_limit && throttle_low && not_rotating_fast) {
if (!ap.land_complete) {
// increase counter until we hit the trigger then set land complete flag
if( land_detector < LAND_DETECTOR_TRIGGER) {
land_detector++;
}else{
set_land_complete(true);
land_detector = LAND_DETECTOR_TRIGGER;
}
}
} else {
// we've sensed movement up or down so reset land_detector
land_detector = 0;
// if throttle output is high then clear landing flag
if (motors.get_throttle_out() > get_non_takeoff_throttle()) {
set_land_complete(false);
}
}
// set land maybe flag
set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER);
}
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* tuning.pde - function to update various parameters in flight using the ch6 tuning knob
* This should not be confused with the AutoTune feature which can bve found in control_autotune.pde
*/
// tuning - updates parameters based on the ch6 tuning knob's position
// should be called at 3.3hz
static void tuning() {
// exit immediately if not tuning of when radio failsafe is invoked so tuning values are not set to zero
if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0) {
return;
}
float tuning_value = (float)g.rc_6.control_in / 1000.0f;
g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high);
switch(g.radio_tuning) {
// Roll, Pitch tuning
case CH6_STABILIZE_ROLL_PITCH_KP:
g.p_stabilize_roll.kP(tuning_value);
g.p_stabilize_pitch.kP(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KP:
g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KI:
g.pid_rate_roll.kI(tuning_value);
g.pid_rate_pitch.kI(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KD:
g.pid_rate_roll.kD(tuning_value);
g.pid_rate_pitch.kD(tuning_value);
break;
// Yaw tuning
case CH6_STABILIZE_YAW_KP:
g.p_stabilize_yaw.kP(tuning_value);
break;
case CH6_YAW_RATE_KP:
g.pid_rate_yaw.kP(tuning_value);
break;
case CH6_YAW_RATE_KD:
g.pid_rate_yaw.kD(tuning_value);
break;
// Altitude and throttle tuning
case CH6_ALTITUDE_HOLD_KP:
g.p_alt_hold.kP(tuning_value);
break;
case CH6_THROTTLE_RATE_KP:
g.p_throttle_rate.kP(tuning_value);
break;
case CH6_THROTTLE_ACCEL_KP:
g.pid_throttle_accel.kP(tuning_value);
break;
case CH6_THROTTLE_ACCEL_KI:
g.pid_throttle_accel.kI(tuning_value);
break;
case CH6_THROTTLE_ACCEL_KD:
g.pid_throttle_accel.kD(tuning_value);
break;
// Loiter and navigation tuning
case CH6_LOITER_POSITION_KP:
g.p_loiter_pos.kP(tuning_value);
break;
case CH6_LOITER_RATE_KP:
g.pid_loiter_rate_lon.kP(tuning_value);
g.pid_loiter_rate_lat.kP(tuning_value);
break;
case CH6_LOITER_RATE_KI:
g.pid_loiter_rate_lon.kI(tuning_value);
g.pid_loiter_rate_lat.kI(tuning_value);
break;
case CH6_LOITER_RATE_KD:
g.pid_loiter_rate_lon.kD(tuning_value);
g.pid_loiter_rate_lat.kD(tuning_value);
break;
case CH6_WP_SPEED:
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
wp_nav.set_speed_xy(g.rc_6.control_in);
break;
// Acro roll pitch gain
case CH6_ACRO_RP_KP:
g.acro_rp_p = tuning_value;
break;
// Acro yaw gain
case CH6_ACRO_YAW_KP:
g.acro_yaw_p = tuning_value;
break;
#if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO:
motors.ext_gyro_gain(g.rc_6.control_in);
break;
case CH6_RATE_PITCH_FF:
g.pid_rate_pitch.ff(tuning_value);
break;
case CH6_RATE_ROLL_FF:
g.pid_rate_roll.ff(tuning_value);
break;
case CH6_RATE_YAW_FF:
g.pid_rate_yaw.ff(tuning_value);
break;
#endif
case CH6_AHRS_YAW_KP:
ahrs._kp_yaw.set(tuning_value);
break;
case CH6_AHRS_KP:
ahrs._kp.set(tuning_value);
break;
case CH6_DECLINATION:
// set declination to +-20degrees
compass.set_declination(ToRad((2.0f * g.rc_6.control_in - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
break;
case CH6_CIRCLE_RATE:
// set circle rate
circle_nav.set_rate(g.rc_6.control_in/25-20); // allow approximately 45 degree turn rate in either direction
break;
case CH6_SONAR_GAIN:
// set sonar gain
g.sonar_gain.set(tuning_value);
break;
#if 0
// disabled for now - we need accessor functions
case CH6_EKF_VERTICAL_POS:
// EKF's baro vs accel (higher rely on accels more, baro impact is reduced)
ahrs.get_NavEKF()._gpsVertPosNoise = tuning_value;
break;
case CH6_EKF_HORIZONTAL_POS:
// EKF's gps vs accel (higher rely on accels more, gps impact is reduced)
ahrs.get_NavEKF()._gpsHorizPosNoise = tuning_value;
break;
case CH6_EKF_ACCEL_NOISE:
// EKF's accel noise (lower means trust accels more, gps & baro less)
ahrs.get_NavEKF()._accNoise = tuning_value;
break;
#endif
case CH6_RC_FEEL_RP:
// roll-pitch input smoothing
g.rc_feel_rp = g.rc_6.control_in / 10;
break;
case CH6_RATE_PITCH_KP:
g.pid_rate_pitch.kP(tuning_value);
break;
case CH6_RATE_PITCH_KI:
g.pid_rate_pitch.kI(tuning_value);
break;
case CH6_RATE_PITCH_KD:
g.pid_rate_pitch.kD(tuning_value);
break;
case CH6_RATE_ROLL_KP:
g.pid_rate_roll.kP(tuning_value);
break;
case CH6_RATE_ROLL_KI:
g.pid_rate_roll.kI(tuning_value);
break;
case CH6_RATE_ROLL_KD:
g.pid_rate_roll.kD(tuning_value);
break;
}
}
......@@ -154,7 +154,6 @@ static void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
////////////////////////////////////////////////////////////////////////////////
// DataFlash
////////////////////////////////////////////////////////////////////////////////
#if LOGGING_ENABLED == ENABLED
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
static DataFlash_APM1 DataFlash;
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
......@@ -165,7 +164,6 @@ static DataFlash_File DataFlash(HAL_BOARD_LOG_DIRECTORY);
// no dataflash driver
DataFlash_Empty DataFlash;
#endif
#endif
// has a log download started?
static bool in_log_download;
......
......@@ -1084,7 +1084,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// run pre_arm_checks and arm_checks and display failures
if (arming.arm(AP_Arming::MAVLINK)) {
//only log if arming was successful
channel_throttle->enable_out();
channel_throttle->enable_out();
Log_Arm_Disarm();
result = MAV_RESULT_ACCEPTED;
} else {
......
......@@ -654,9 +654,11 @@ static void Log_Write_IMU() {}
static void Log_Write_RC() {}
static void Log_Write_Airspeed(void) {}
static void Log_Write_Baro(void) {}
static void Log_Write_Sonar() {}
#if OPTFLOW == ENABLED
static void Log_Write_Optflow() {}
#endif
static void Log_Arm_Disarm() {}
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) {
return 0;
......
......@@ -672,7 +672,9 @@ static bool should_log(uint32_t mask)
// we have to set in_mavlink_delay to prevent logging while
// writing headers
in_mavlink_delay = true;
#if LOGGING_ENABLED == ENABLED
start_logging();
#endif
in_mavlink_delay = false;
}
return ret;
......
......@@ -20,7 +20,7 @@
/* the pin header uses */
// "P9.27", /* pru0: pr1_pru0_pru_r30_5 */
"P8.15", /* pru0: pr1_pru0_pru_r30_15 */
"P8.15", /* pru0: pr1_pru0_pru_r30_15, PPM-sum, SBUS, DSM */
// "P8.12", /* pru0: pr1_pru0_pru_r30_14 */
// "P9.25", /* pru0: pr1_pru0_pru_r30_7 */
// "P9.41", /* pru0: pr1_pru0_pru_r30_6 */
......@@ -39,18 +39,18 @@
//"P8.20", /* pru1: pr1_pru1_pru_r30_13 */
//"P8.21", /* pru1: pr1_pru1_pru_r30_12 */
"P8.27", /* pru1: pr1_pru1_pru_r30_8 */
"P8.28", /* pru1: pr1_pru1_pru_r30_10 */
"P8.29", /* pru1: pr1_pru1_pru_r30_9 */
"P8.30", /* pru1: pr1_pru1_pru_r30_11 */
"P8.39", /* pru1: pr1_pru1_pru_r30_6 */
"P8.40", /* pru1: pr1_pru1_pru_r30_7 */
"P8.41", /* pru1: pr1_pru1_pru_r30_4 */
"P8.42", /* pru1: pr1_pru1_pru_r30_5 */
"P8.43", /* pru1: pr1_pru1_pru_r30_2 */
"P8.44", /* pru1: pr1_pru1_pru_r30_3 */
"P8.45", /* pru1: pr1_pru1_pru_r30_0 */
"P8.46", /* pru1: pr1_pru1_pru_r30_1 */
"P8.27", /* pru1: pr1_pru1_pru_r30_8, CH_2 */
"P8.28", /* pru1: pr1_pru1_pru_r30_10, CH_1 */
"P8.29", /* pru1: pr1_pru1_pru_r30_9, CH_4 */
"P8.30", /* pru1: pr1_pru1_pru_r30_11, CH_3 */
"P8.39", /* pru1: pr1_pru1_pru_r30_6, CH_6 */
"P8.40", /* pru1: pr1_pru1_pru_r30_7, CH_5 */
"P8.41", /* pru1: pr1_pru1_pru_r30_4, CH_8 */
"P8.42", /* pru1: pr1_pru1_pru_r30_5, CH_7 */
"P8.43", /* pru1: pr1_pru1_pru_r30_2, CH_10 */
"P8.44", /* pru1: pr1_pru1_pru_r30_3, CH_9 */
"P8.45", /* pru1: pr1_pru1_pru_r30_0, CH_12 */
"P8.46", /* pru1: pr1_pru1_pru_r30_1, CH_11 */
/* pru1: pr1_pru1_pru_r30_14 is on UART0_RXD */
/* pru1: pr1_pru1_pru_r30_15 is on UART0_TXD */
/* the hardware IP uses */
......@@ -70,7 +70,7 @@
pru_pru_pins: pinmux_pru_pru_pins {
pinctrl-single,pins = <
// 0x1a4 0x25 /* mcasp0_fsr.pr1_pru0_pru_r30_5, MODE5 | OUTPUT | PRU */
0x03c 0x2E /* gpmc_ad13.pr1_pru0_pru_r30_15, MODE6 | INPUT | PRU */
0x03c 0x2E /* gpmc_ad13.pr1_pru0_pru_r30_15, MODE6 | INPUT | PRU, PPM-sum, SBUS, DSM */
// 0x030 0x26 /* gpmc_ad12.pr1_pru0_pru_r30_14, MODE6 | OUTPUT | PRU */
// 0x1ac 0x25 /* mcasp0_ahclkx.pr1_pru0_pru_r30_7, MODE5 | OUTPUT | PRU */
// 0x1a8 0x25 /* mcasp0_axr1.pr1_pru0_pru_r30_6, MODE5 | OUTPUT | PRU */
......@@ -82,18 +82,18 @@
//0x084 0x25 /* gpmc_csn2.pr1_pru1_pru_r30_13, MODE5 | OUTPUT | PRU */
//0x080 0x25 /* gpmc_csn1.pr1_pru1_pru_r30_12, MODE5 | OUTPUT | PRU */
0x0e0 0x25 /* lcd_vsync.pr1_pru1_pru_r30_8, MODE5 | OUTPUT | PRU */
0x0e8 0x25 /* lcd_pclk.pr1_pru1_pru_r30_10, MODE5 | OUTPUT | PRU */
0x0e4 0x25 /* lcd_hsync.pr1_pru1_pru_r30_9, MODE5 | OUTPUT | PRU */
0x0ec 0x25 /* lcd_ac_bias_en.pr1_pru1_pru_r30_11, MODE5 | OUTPUT | PRU */
0x0b8 0x25 /* pr1_pru1_pru_r30_6, MODE5 | OUTPUT | PRU */
0x0bc 0x25 /* lcd_data7.pr1_pru1_pru_r30_7, MODE5 | OUTPUT | PRU */
0x0b0 0x25 /* lcd_data4.pr1_pru1_pru_r30_4, MODE5 | OUTPUT | PRU */
0x0b4 0x25 /* lcd_data5.pr1_pru1_pru_r30_5, MODE5 | OUTPUT | PRU */
0x0a8 0x25 /* pr1_pru1_pru_r31_2, MODE5 | OUTPUT | PRU */
0x0ac 0x25 /* lcd_data3.pr1_pru1_pru_r30_3, MODE5 | OUTPUT | PRU */
0x0a0 0x25 /* lcd_data0.pr1_pru1_pru_r30_0, MODE5 | OUTPUT | PRU */
0x0a4 0x25 /* lcd_data1.pr1_pru1_pru_r30_1, MODE5 | OUTPUT | PRU */
0x0e0 0x25 /* lcd_vsync.pr1_pru1_pru_r30_8, MODE5 | OUTPUT | PRU, CH_2 */
0x0e8 0x25 /* lcd_pclk.pr1_pru1_pru_r30_10, MODE5 | OUTPUT | PRU, CH_1 */
0x0e4 0x25 /* lcd_hsync.pr1_pru1_pru_r30_9, MODE5 | OUTPUT | PRU, CH_4 */
0x0ec 0x25 /* lcd_ac_bias_en.pr1_pru1_pru_r30_11, MODE5 | OUTPUT | PRU, CH_3 */
0x0b8 0x25 /* lcd_data6.pr1_pru1_pru_r30_6, MODE5 | OUTPUT | PRU, CH_6 */
0x0bc 0x25 /* lcd_data7.pr1_pru1_pru_r30_7, MODE5 | OUTPUT | PRU, CH_5 */
0x0b0 0x25 /* lcd_data4.pr1_pru1_pru_r30_4, MODE5 | OUTPUT | PRU, CH_8 */
0x0b4 0x25 /* lcd_data5.pr1_pru1_pru_r30_5, MODE5 | OUTPUT | PRU, CH_7 */
0x0a8 0x25 /* lcd_data2.pr1_pru1_pru_r30_2, MODE5 | OUTPUT | PRU, CH_10 */
0x0ac 0x25 /* lcd_data3.pr1_pru1_pru_r30_3, MODE5 | OUTPUT | PRU, CH_9 */
0x0a0 0x25 /* lcd_data0.pr1_pru1_pru_r30_0, MODE5 | OUTPUT | PRU, CH_12 */
0x0a4 0x25 /* lcd_data1.pr1_pru1_pru_r30_1, MODE5 | OUTPUT | PRU, CH_11 */
>;
};
};
......
......@@ -18,22 +18,24 @@
/* state the resources this cape uses */
exclusive-use =
/* the pin header uses */
/* PRU */
/* PRU RCInput */
"P8.11", /* pru0: pr1_pru0_pru_r30_15 */
"P8.15", /* pru0: pr1_pru0_pru_r31_15*/
"P8.15", /* pru0: pr1_pru0_pru_r31_15, PPM-sum, SBUS, DSM */
"P8.27", /* pru1: pr1_pru1_pru_r30_8 */
"P8.28", /* pru1: pr1_pru1_pru_r30_10 */
"P8.29", /* pru1: pr1_pru1_pru_r30_9 */
"P8.30", /* pru1: pr1_pru1_pru_r30_11 */
"P8.39", /* pru1: pr1_pru1_pru_r30_6 */
"P8.40", /* pru1: pr1_pru1_pru_r30_7 */
"P8.41", /* pru1: pr1_pru1_pru_r30_4 */
"P8.42", /* pru1: pr1_pru1_pru_r30_5 */
"P8.43", /* pru1: pr1_pru1_pru_r30_2 */
"P8.44", /* pru1: pr1_pru1_pru_r30_3 */
"P8.45", /* pru1: pr1_pru1_pru_r30_0 */
"P8.46", /* pru1: pr1_pru1_pru_r30_1 */
/* PRU RCOutput */
"P8.27", /* pru1: pr1_pru1_pru_r30_8, CH_2 */
"P8.28", /* pru1: pr1_pru1_pru_r30_10, CH_1 */
"P8.29", /* pru1: pr1_pru1_pru_r30_9, CH_4 */
"P8.30", /* pru1: pr1_pru1_pru_r30_11, CH_3 */
"P8.39", /* pru1: pr1_pru1_pru_r30_6, CH_6 */
"P8.40", /* pru1: pr1_pru1_pru_r30_7, CH_5 */
"P8.41", /* pru1: pr1_pru1_pru_r30_4, CH_8 */
"P8.42", /* pru1: pr1_pru1_pru_r30_5, CH_7 */
"P8.43", /* pru1: pr1_pru1_pru_r30_2, CH_10 */
"P8.44", /* pru1: pr1_pru1_pru_r30_3, CH_9 */
"P8.45", /* pru1: pr1_pru1_pru_r30_0, CH_12 */
"P8.46", /* pru1: pr1_pru1_pru_r30_1, CH_11 */
/* SPI0 */
"P9.22", /* SPI0_SCLK */
......@@ -74,20 +76,20 @@
pru_pru_pins: pinmux_pru_pru_pins {
pinctrl-single,pins = <
0x034 0x26 /* gpmc_ad13.pr1_pru0_pru_r30_15, MODE6 | OUTPUT | PRU */
0x03c 0x2e /* gpmc_ad15.pr1_pru0_pru_r30_15, MODE 6 | INPUT | PRU */
0x0e0 0x25 /* lcd_vsync.pr1_pru1_pru_r30_8, MODE5 | OUTPUT | PRU */
0x0e8 0x25 /* lcd_pclk.pr1_pru1_pru_r30_10, MODE5 | OUTPUT | PRU */
0x0e4 0x25 /* lcd_hsync.pr1_pru1_pru_r30_9, MODE5 | OUTPUT | PRU */
0x0ec 0x25 /* lcd_ac_bias_en.pr1_pru1_pru_r30_11, MODE5 | OUTPUT | PRU */
0x0b8 0x25 /* pr1_pru1_pru_r30_6, MODE5 | OUTPUT | PRU */
0x0bc 0x25 /* lcd_data7.pr1_pru1_pru_r30_7, MODE5 | OUTPUT | PRU */
0x0b0 0x25 /* lcd_data4.pr1_pru1_pru_r30_4, MODE5 | OUTPUT | PRU */
0x0b4 0x25 /* lcd_data5.pr1_pru1_pru_r30_5, MODE5 | OUTPUT | PRU */
0x0a8 0x25 /* pr1_pru1_pru_r31_2, MODE5 | OUTPUT | PRU */
0x0ac 0x25 /* lcd_data3.pr1_pru1_pru_r30_3, MODE5 | OUTPUT | PRU */
0x0a0 0x25 /* lcd_data0.pr1_pru1_pru_r30_0, MODE5 | OUTPUT | PRU */
0x0a4 0x25 /* lcd_data1.pr1_pru1_pru_r30_1, MODE5 | OUTPUT | PRU */
0x03c 0x2e /* gpmc_ad15.pr1_pru0_pru_r31_15, MODE6 | INPUT | PRU, PPM-sum, SBUS, DSM */
0x0e0 0x25 /* lcd_vsync.pr1_pru1_pru_r30_8, MODE5 | OUTPUT | PRU, CH_2 */
0x0e8 0x25 /* lcd_pclk.pr1_pru1_pru_r30_10, MODE5 | OUTPUT | PRU, CH_1 */
0x0e4 0x25 /* lcd_hsync.pr1_pru1_pru_r30_9, MODE5 | OUTPUT | PRU, CH_4 */
0x0ec 0x25 /* lcd_ac_bias_en.pr1_pru1_pru_r30_11, MODE5 | OUTPUT | PRU, CH_3 */
0x0b8 0x25 /* lcd_data6.pr1_pru1_pru_r30_6, MODE5 | OUTPUT | PRU, CH_6 */
0x0bc 0x25 /* lcd_data7.pr1_pru1_pru_r30_7, MODE5 | OUTPUT | PRU, CH_5 */
0x0b0 0x25 /* lcd_data4.pr1_pru1_pru_r30_4, MODE5 | OUTPUT | PRU, CH_8 */
0x0b4 0x25 /* lcd_data5.pr1_pru1_pru_r30_5, MODE5 | OUTPUT | PRU, CH_7 */
0x0a8 0x25 /* lcd_data2.pr1_pru1_pru_r30_2, MODE5 | OUTPUT | PRU, CH_10 */
0x0ac 0x25 /* lcd_data3.pr1_pru1_pru_r30_3, MODE5 | OUTPUT | PRU, CH_9 */
0x0a0 0x25 /* lcd_data0.pr1_pru1_pru_r30_0, MODE5 | OUTPUT | PRU, CH_12 */
0x0a4 0x25 /* lcd_data1.pr1_pru1_pru_r30_1, MODE5 | OUTPUT | PRU, CH_11 */
>;
};
spi0_pins_s0: spi0_pins_s0 {
......
......@@ -160,7 +160,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
save_wp(mavproxy, mav)
# switch back to stabilize mode
mavproxy.send('rc 3 1450\n')
mavproxy.send('rc 3 1430\n')
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
......@@ -615,7 +615,7 @@ def fly_simple(mavproxy, mav, side=50, timeout=120):
# switch to stabilize mode
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1450\n')
mavproxy.send('rc 3 1430\n')
# fly south 50m
print("# Flying south %u meters" % side)
......@@ -680,7 +680,7 @@ def fly_super_simple(mavproxy, mav, timeout=45):
# switch to stabilize mode
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1450\n')
mavproxy.send('rc 3 1430\n')
# start copter yawing slowly
mavproxy.send('rc 4 1550\n')
......
......@@ -16,6 +16,8 @@ for b in all apm2 sitl linux; do
make clean
make $b -j4
done
make clean
make apm1-nologging
popd
for d in ArduCopter APMrover2 ArduPlane AntennaTracker; do
......
......@@ -117,6 +117,8 @@ void AC_PosControl::set_accel_z(float accel_cmss)
void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
{
float alt_change = alt_cm-_pos_target.z;
_vel_desired.z = constrain_float(alt_change * dt, _speed_down_cms, _speed_up_cms);
// adjust desired alt if motors have not hit their limits
if ((alt_change<0 && !_motors.limit.throttle_lower) || (alt_change>0 && !_motors.limit.throttle_upper)) {
......@@ -139,6 +141,8 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d
if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) {
_pos_target.z += climb_rate_cms * dt;
}
_vel_desired.z = climb_rate_cms;
}
// get_alt_error - returns altitude error in cm
......@@ -299,7 +303,6 @@ void AC_PosControl::rate_to_accel_z()
// reset last velocity target to current target
if (_flags.reset_rate_to_accel_z) {
_vel_last.z = _vel_target.z;
_flags.reset_rate_to_accel_z = false;
}
// feed forward desired acceleration calculation
......@@ -322,7 +325,6 @@ void AC_PosControl::rate_to_accel_z()
// Reset Filter
_vel_error.z = 0;
_vel_error_filter.reset(0);
desired_accel = 0;
_flags.reset_rate_to_accel_z = false;
} else {
// calculate rate error and filter with cut off frequency of 2 Hz
......
......@@ -72,7 +72,7 @@ void loop()
(int)gps.ground_course_cd() / 100,
gps.num_sats(),
gps.time_week(),
gps.time_week_ms(),
(unsigned long)gps.time_week_ms(),
gps.status());
}
hal.scheduler->delay(10);
......
......@@ -288,6 +288,9 @@ bool AP_InertialSensor_MPU9250::update( void )
// way up, and PWM pins on NavIO are at the back of the aircraft
accel.rotate(ROTATION_ROLL_180_YAW_90);
gyro.rotate(ROTATION_ROLL_180_YAW_90);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
accel.rotate(ROTATION_ROLL_180);
gyro.rotate(ROTATION_ROLL_180);
#endif
_rotate_and_offset_gyro(_gyro_instance, gyro);
......
......@@ -921,11 +921,12 @@ void NavEKF::SelectFlowFusion()
// check if fusion should be delayed to spread load. Setting fuseMeNow to true disables this load spreading feature.
bool delayFusion = ((covPredStep || magFusePerformed) && !(flowFuseNow || inhibitLoadLeveling));
// if we don't have valid flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in position hold mode
if (!flowDataValid && !constPosMode && PV_AidingMode == AID_RELATIVE) {
// if we don't have valid flow measurements and are relying on them, dead reckon using current velocity vector
// If the flow measurements have been rejected for too long and we are relying on them, then reset the velocities to an estimate based on the flow and range data
if (!flowDataValid && PV_AidingMode == AID_RELATIVE) {
constVelMode = true;
constPosMode = false; // always clear constant position mode if constant velocity mode is active
} else if (flowDataValid && flowFusionTimeout) {
} else if (flowDataValid && flowFusionTimeout && PV_AidingMode == AID_RELATIVE) {
// we need to reset the velocities to a value estimated from the flow data
// estimate the range
float initPredRng = max((terrainState - state.position[2]),0.1f) / Tnb_flow.c.z;
......
......@@ -36,7 +36,7 @@ extern const AP_HAL::HAL& hal;
bool ToneAlarm_Linux::init()
{
// open the tone alarm device
err = hal.util->toneAlarm_init();
err = !hal.util->toneAlarm_init();
if (err) {
hal.console->printf("AP_Notify: Failed to initialise ToneAlarm");
return false;
......
......@@ -16,6 +16,12 @@ include $(MK_DIR)/configure.mk
else
# short-circuit build for the help target
ifeq ($(MAKECMDGOALS),help)
include $(MK_DIR)/help.mk
else
# common makefile components
include $(MK_DIR)/targets.mk
include $(MK_DIR)/sketch_sources.mk
......@@ -53,3 +59,5 @@ endif
endif
endif
endif