This commit is contained in:
Michael Oborne 2011-10-30 07:35:10 +08:00
commit 9465617b02
17 changed files with 88 additions and 412 deletions

View File

@ -1,3 +1,4 @@
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduBoat/ArduBoat.pde"
// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
@ -13,13 +14,12 @@
#include <APM_BMP085.h>
#include <ModeFilter.h>
#include <APO.h>
#include <WProgram.h>
// Vehicle Configuration
#include "BoatGeneric.h"
// ArduPilotOne Default Setup
#include "APO_DefaultSetup.h"
#include <WProgram.h>; int main(void) {init();setup();for(;;) loop(); return 0; }
// vim:ts=4:sw=4:expandtab
#line 1 "autogenerated"
#include "WProgram.h"

View File

@ -13,7 +13,6 @@
#include <APM_BMP085.h>
#include <ModeFilter.h>
#include <APO.h>
#include <WProgram.h>
// Vehicle Configuration
#include "BoatGeneric.h"

View File

@ -1028,8 +1028,12 @@ void update_throttle_mode(void)
case THROTTLE_MANUAL:
if (g.rc_3.control_in > 0){
angle_boost = get_angle_boost(g.rc_3.control_in);
g.rc_3.servo_out = g.rc_3.control_in + angle_boost;
#if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.rc_3.control_in));
#else
angle_boost = get_angle_boost(g.rc_3.control_in);
g.rc_3.servo_out = g.rc_3.control_in + angle_boost;
#endif
}else{
g.pi_stabilize_roll.reset_I();
g.pi_stabilize_pitch.reset_I();
@ -1058,8 +1062,12 @@ void update_throttle_mode(void)
// clear the new data flag
invalid_throttle = false;
}
angle_boost = get_angle_boost(g.throttle_cruise);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost;
#if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle);
#else
angle_boost = get_angle_boost(g.throttle_cruise);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost;
#endif
break;
}
}
@ -1331,6 +1339,13 @@ static void tuning(){
g.pi_nav_lat.kP(tuning_value);
g.pi_nav_lon.kP(tuning_value);
break;
#if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO:
g.rc_6.set_range(1000,2000);
g.heli_ext_gyro_gain = tuning_value * 1000;
break;
#endif
}
}

View File

@ -1,165 +0,0 @@
#=============================================================================#
# Author: Niklaa Goddemeier & Sebastian Rohde #
# Date: 04.09.2011 #
#=============================================================================#
set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../")
set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain
#include(ArduinoProcessing)
set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde)
message(STATUS "DIR: ${CMAKE_SOURCE_DIR}")
cmake_minimum_required(VERSION 2.8)
#====================================================================#
# Setup Project #
#====================================================================#
project(ArduCopter C CXX)
find_package(Arduino 22 REQUIRED)
if (NOT DEFINED BOARD)
message(STATUS "board not defined, assuming mega, use cmake -DBOARD=mega2560 , etc. to specify")
set(BOARD "mega")
endif()
message(STATUS "Board configured as: ${BOARD}")
add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs")
#add_subdirectory(${CMAKE_SOURCE_DIR}/ArduCopter)
#add_subdirectory(testtool)
PRINT_BOARD_SETTINGS(${BOARD})
#=============================================================================#
# Author: Niklas Goddemeier & Sebastian Rohde #
# Date: 04.09.2011 #
#=============================================================================#
#====================================================================#
# Settings #
#====================================================================#
set(FIRMWARE_NAME arducopter)
set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board
set(${FIRMWARE_NAME}_SKETCHES
ArduCopter.pde
Attitude.pde
Camera.pde
commands.pde
commands_logic.pde
commands_process.pde
control_modes.pde
events.pde
flip.pde
GCS.pde
GCS_Ardupilot.pde
#GCS_IMU_output.pde
GCS_Jason_text.pde
GCS_Mavlink.pde
GCS_Standard.pde
GCS_Xplane.pde
heli.pde
HIL_Xplane.pde
leds.pde
Log.pde
motors_hexa.pde
motors_octa.pde
motors_octa_quad.pde
motors_quad.pde
motors_tri.pde
motors_y6.pde
navigation.pde
planner.pde
radio.pde
read_commands.pde
sensors.pde
setup.pde
system.pde
test.pde
) # Firmware sketches
#create dummy sourcefile
file(WRITE ${FIRMWARE_NAME}.cpp "// Do not edit")
set(${FIRMWARE_NAME}_SRCS
#test.cpp
${FIRMWARE_NAME}.cpp
) # Firmware sources
set(${FIRMWARE_NAME}_HDRS
APM_Config.h
APM_Config_mavlink_hil.h
APM_Config_xplane.h
config.h
defines.h
GCS.h
HIL.h
Mavlink_Common.h
Parameters.h
) # Firmware sources
set(${FIRMWARE_NAME}_LIBS
DataFlash
AP_Math
PID
RC_Channel
AP_OpticalFlow
ModeFilter
memcheck
#AP_PID
APM_PI
#APO
FastSerial
AP_Common
GCS_MAVLink
AP_GPS
APM_RC
AP_DCM
AP_ADC
AP_Compass
AP_IMU
AP_RangeFinder
APM_BMP085
c
m
)
#${CONSOLE_PORT}
set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port
set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd
include_directories(
${CMAKE_SOURCE_DIR}/libraries/DataFlash
${CMAKE_SOURCE_DIR}/libraries/AP_Math
${CMAKE_SOURCE_DIR}/libraries/PID
${CMAKE_SOURCE_DIR}/libraries/AP_Common
${CMAKE_SOURCE_DIR}/libraries/RC_Channel
${CMAKE_SOURCE_DIR}/libraries/AP_OpticalFlow
${CMAKE_SOURCE_DIR}/libraries/ModeFilter
${CMAKE_SOURCE_DIR}/libraries/memcheck
#${CMAKE_SOURCE_DIR}/libraries/AP_PID
${CMAKE_SOURCE_DIR}/libraries/APM_PI
${CMAKE_SOURCE_DIR}/libraries/FastSerial
${CMAKE_SOURCE_DIR}/libraries/AP_Compass
${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder
${CMAKE_SOURCE_DIR}/libraries/AP_GPS
${CMAKE_SOURCE_DIR}/libraries/AP_IMU
${CMAKE_SOURCE_DIR}/libraries/AP_ADC
${CMAKE_SOURCE_DIR}/libraries/AP_DCM
${CMAKE_SOURCE_DIR}/libraries/APM_RC
${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink
${CMAKE_SOURCE_DIR}/libraries/APM_BMP085
)
#====================================================================#
# Target generation #
#====================================================================#
generate_arduino_firmware(${FIRMWARE_NAME})

View File

@ -607,68 +607,35 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((int)wp_distance); // 1
DataFlash.WriteByte(wp_verify_byte); // 2
DataFlash.WriteInt((int)(target_bearing/100)); // 3
DataFlash.WriteInt((int)long_error); // 4
DataFlash.WriteInt((int)lat_error); // 5
DataFlash.WriteInt((int)wp_distance); // 1
DataFlash.WriteInt((int)(target_bearing/100)); // 2
DataFlash.WriteInt((int)long_error); // 3
DataFlash.WriteInt((int)lat_error); // 4
DataFlash.WriteInt((int)nav_lon); // 5
DataFlash.WriteInt((int)nav_lat); // 6
DataFlash.WriteInt((int)g.pi_nav_lon.get_integrator()); // 7
DataFlash.WriteInt((int)g.pi_nav_lat.get_integrator()); // 8
DataFlash.WriteInt((int)g.pi_loiter_lon.get_integrator()); // 9
DataFlash.WriteInt((int)g.pi_loiter_lat.get_integrator()); // 10
/*
DataFlash.WriteInt((int)long_error); // 5
DataFlash.WriteInt((int)lat_error); // 6
DataFlash.WriteInt(x_rate_error); // 4
DataFlash.WriteInt(y_rate_error); // 4
DataFlash.WriteInt((int)nav_lon); // 7
DataFlash.WriteInt((int)nav_lat); // 8
DataFlash.WriteInt((int)g.pi_nav_lon.get_integrator()); // 7
DataFlash.WriteInt((int)g.pi_nav_lat.get_integrator()); // 8
DataFlash.WriteInt((int)g.pi_loiter_lon.get_integrator()); // 7
DataFlash.WriteInt((int)g.pi_loiter_lat.get_integrator()); // 8
*/
DataFlash.WriteByte(END_BYTE);
}
static void Log_Read_Nav_Tuning()
{
Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d\n"),
DataFlash.ReadInt(), // distance
DataFlash.ReadByte(), // wp_verify_byte
DataFlash.ReadInt(), // target_bearing
DataFlash.ReadInt(), // long_error
DataFlash.ReadInt()); // lat_error
/*
// 1 2 3 4
Serial.printf_P(PSTR( "NTUN, %d, %d, %d, %d, "
"%d, %d, %d, %d, "
"%d, %d, %d, %d, "
"%d, %d\n"),
DataFlash.ReadInt(), //distance
DataFlash.ReadByte(), //bitmask
DataFlash.ReadInt(), //target bearing
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt()); //nav bearing
*/
// 1 2 3 4 5 6 7 8 9 10
Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
DataFlash.ReadInt(), // 1
DataFlash.ReadInt(), // 2
DataFlash.ReadInt(), // 3
DataFlash.ReadInt(), // 4
DataFlash.ReadInt(), // 5
DataFlash.ReadInt(), // 6
DataFlash.ReadInt(), // 7
DataFlash.ReadInt(), // 8
DataFlash.ReadInt(), // 9
DataFlash.ReadInt()); // 10
}
@ -769,8 +736,8 @@ static void Log_Write_Performance()
// Read a performance packet
static void Log_Read_Performance()
{ //1 2 3 4 5 6
Serial.printf_P(PSTR( "PM, %d, %d, %d, %d, %d, %ld\n"),
{ //1 2 3 4 5 6
Serial.printf_P(PSTR("PM, %d, %d, %d, %d, %d, %ld\n"),
// Control
//DataFlash.ReadLong(),

View File

@ -147,6 +147,7 @@
#define CH6_NAV_P 11
#define CH6_LOITER_P 12
#define CH6_HELI_EXTERNAL_GYRO 13
// nav byte mask
// -------------

View File

@ -6,6 +6,7 @@
#define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz
static int heli_manual_override = false;
static float heli_throttle_scaler = 0;
// heli_servo_averaging:
// 0 or 1 = no averaging, 250hz
@ -52,6 +53,9 @@ static void heli_init_swash()
g.heli_servo_3.radio_min = g.heli_coll_min - tilt_max[CH_3];
g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3];
// scaler for changing channel 3 radio input into collective range
heli_throttle_scaler = ((float)(g.heli_coll_max - g.heli_coll_min))/1000;
// reset the servo averaging
for( i=0; i<=3; i++ )
heli_servo_out[i] = 0;
@ -193,4 +197,25 @@ static void output_motor_test()
{
}
// heli_get_scaled_throttle - user's throttle scaled to collective range
// input is expected to be in the range of 0~1000 (ie. pwm)
// also does equivalent of angle_boost
static int heli_get_scaled_throttle(int throttle)
{
float scaled_throttle = (g.heli_coll_min - 1000) + throttle * heli_throttle_scaler;
return g.heli_coll_min - 1000 + (throttle * heli_throttle_scaler);
}
// heli_angle_boost - takes servo_out position
// adds a boost depending on roll/pitch values
// equivalent of quad's angle_boost function
// pwm_out value should be 0 ~ 1000
static int heli_get_angle_boost(int pwm_out)
{
float angle_boost_factor = cos_pitch_x * cos_roll_x;
angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0);
int throttle_above_center = max(1000 + pwm_out - g.heli_coll_mid,0);
return pwm_out + throttle_above_center*angle_boost_factor;
}
#endif // HELI_FRAME

View File

@ -18,7 +18,9 @@ static void init_rc_in()
g.rc_1.set_angle(4500);
g.rc_2.set_angle(4500);
g.rc_3.set_range(0,1000);
#if FRAME_CONFIG != HELI_FRAME
g.rc_3.scale_output = .9;
#endif
g.rc_4.set_angle(4500);
// reverse: CW = left

View File

@ -512,7 +512,11 @@ init_throttle_cruise()
// are we moving from manual throttle to auto_throttle?
if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
g.pi_throttle.reset_I();
g.throttle_cruise.set_and_save(g.rc_3.control_in);
#if FRAME_CONFIG == HELI_FRAME
g.throttle_cruise.set_and_save(heli_get_scaled_throttle(g.rc_3.control_in));
#else
g.throttle_cruise.set_and_save(g.rc_3.control_in);
#endif
}
}

View File

@ -1 +0,0 @@
ArduPlane.cpp

View File

@ -402,7 +402,6 @@ static unsigned long nav_loopTimer; // used to track the elapsed time for GP
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
static float load; // % MCU cycles used
RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
AP_Relay relay;
////////////////////////////////////////////////////////////////////////////////

View File

@ -1,168 +0,0 @@
#=============================================================================#
# Author: Niklaa Goddemeier & Sebastian Rohde #
# Date: 04.09.2011 #
#=============================================================================#
set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../")
set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain
#include(ArduinoProcessing)
set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde)
message(STATUS "DIR: ${CMAKE_SOURCE_DIR}")
cmake_minimum_required(VERSION 2.8)
#====================================================================#
# Setup Project #
#====================================================================#
project(ArduPlane C CXX)
find_package(Arduino 22 REQUIRED)
if (NOT DEFINED BOARD)
message(STATUS "board not defined, assuming mega, use cmake -DBOARD=mega2560 , etc. to specify")
set(BOARD "mega")
endif()
message(STATUS "Board configured as: ${BOARD}")
add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs")
#add_subdirectory(${CMAKE_SOURCE_DIR}/ArduCopter)
#add_subdirectory(testtool)
PRINT_BOARD_SETTINGS(${BOARD})
#=============================================================================#
# Author: Niklas Goddemeier & Sebastian Rohde #
# Date: 04.09.2011 #
#=============================================================================#
#====================================================================#
# Settings #
#====================================================================#
set(FIRMWARE_NAME ArduPlane)
set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board
set(${FIRMWARE_NAME}_SKETCHES
ArduPlane.pde
Attitude.pde
climb_rate.pde
commands.pde
commands_logic.pde
commands_process.pde
control_modes.pde
events.pde
#flip.pde
#GCS.pde
#GCS_Ardupilot.pde
#GCS_IMU_output.pde
#GCS_Jason_text.pde
GCS_Mavlink.pde
#GCS_Standard.pde
#GCS_Xplane.pde
#heli.pde
#HIL_Xplane.pde
#leds.pde
Log.pde
#motors_hexa.pde
#motors_octa.pde
#motors_octa_quad.pde
#motors_quad.pde
#motors_tri.pde
#motors_y6.pde
navigation.pde
planner.pde
radio.pde
#read_commands.pde
sensors.pde
setup.pde
system.pde
test.pde
) # Firmware sketches
#create dummy sourcefile
file(WRITE ${FIRMWARE_NAME}.cpp "// Do not edit")
set(${FIRMWARE_NAME}_SRCS
#test.cpp
${FIRMWARE_NAME}.cpp
) # Firmware sources
set(${FIRMWARE_NAME}_HDRS
APM_Config.h
APM_Config_mavlink_hil.h
#APM_Config_xplane.h
config.h
defines.h
GCS.h
#HIL.h
#Mavlink_Common.h
Parameters.h
) # Firmware sources
set(${FIRMWARE_NAME}_LIBS
DataFlash
AP_Math
PID
RC_Channel
AP_OpticalFlow
ModeFilter
memcheck
#AP_PID
APM_PI
#APO
FastSerial
AP_Common
GCS_MAVLink
AP_GPS
APM_RC
AP_DCM
AP_ADC
AP_Compass
AP_IMU
AP_RangeFinder
APM_BMP085
c
m
)
#${CONSOLE_PORT}
set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port
set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd
include_directories(
${CMAKE_SOURCE_DIR}/libraries/DataFlash
${CMAKE_SOURCE_DIR}/libraries/AP_Math
${CMAKE_SOURCE_DIR}/libraries/PID
${CMAKE_SOURCE_DIR}/libraries/AP_Common
${CMAKE_SOURCE_DIR}/libraries/RC_Channel
${CMAKE_SOURCE_DIR}/libraries/AP_OpticalFlow
${CMAKE_SOURCE_DIR}/libraries/ModeFilter
${CMAKE_SOURCE_DIR}/libraries/memcheck
#${CMAKE_SOURCE_DIR}/libraries/AP_PID
${CMAKE_SOURCE_DIR}/libraries/APM_PI
${CMAKE_SOURCE_DIR}/libraries/FastSerial
${CMAKE_SOURCE_DIR}/libraries/AP_Compass
${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder
${CMAKE_SOURCE_DIR}/libraries/AP_GPS
${CMAKE_SOURCE_DIR}/libraries/AP_IMU
${CMAKE_SOURCE_DIR}/libraries/AP_ADC
${CMAKE_SOURCE_DIR}/libraries/AP_DCM
${CMAKE_SOURCE_DIR}/libraries/APM_RC
${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink
${CMAKE_SOURCE_DIR}/libraries/APM_BMP085
#new
#${CMAKE_SOURCE_DIR}/libraries/Wire
#${CMAKE_SOURCE_DIR}/libraries/SPI
)
#====================================================================#
# Target generation #
#====================================================================#
generate_arduino_firmware(${FIRMWARE_NAME})

View File

@ -1,3 +1,4 @@
#line 1 "/home/jgoppert/Projects/ardupilotone/ArduRover/ArduRover.pde"
// Libraries
#include <Wire.h>
#include <FastSerial.h>
@ -18,9 +19,9 @@
// Vehicle Configuration
#include "CarStampede.h"
//#include "TankGeneric.h"
// ArduPilotOne Default Setup
#include "APO_DefaultSetup.h"
#include <WProgram.h>; int main(void) {init();setup();for(;;) loop(); return 0; }
// vim:ts=4:sw=4:expandtab
#line 1 "autogenerated"
#include "WProgram.h"

View File

@ -79,13 +79,8 @@ macro(apm_project PROJECT_NAME BOARD SRCS)
message(STATUS "creating apo project ${PROJECT_NAME}")
set(${PROJECT_NAME}_BOARD ${BOARD})
set(${PROJECT_NAME}_AFLAGS "-assembler-with-cpp")
file(GLOB HDRS ${PROJECT_NAME}/*.h)
file(GLOB PDE ${PROJECT_NAME}/*.pde)
set(${PROJECT_NAME}_SRCS ${SRCS} ${HDRS} ${PDE})
set(${PROJECT_NAME}_SRCS ${SRCS})
set(${PROJECT_NAME}_LIBS c)
message(STATUS "sources: ${SRCS}")
message(STATUS "headers: ${HDRS}")
message(STATUS "pde: ${PDE}")
generate_arduino_firmware(${PROJECT_NAME})
set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
install(FILES
@ -100,3 +95,5 @@ apm_project(ArduRover ${BOARD} ArduRover/ArduRover.cpp)
apm_project(ArduBoat ${BOARD} ArduBoat/ArduBoat.cpp)
#apm_project(ArduPlane ${BOARD} ArduPlane/ArduPlane.cpp)
#apm_project(ArduCopter ${BOARD} ArduCopter/ArduCopter.cpp)

View File

@ -14,7 +14,6 @@
#include <APM_BMP085.h>
#include <ModeFilter.h>
#include <APO.h>
#include <WProgram.h>
// Vehicle Configuration
//#include "QuadArducopter.h"

View File

@ -3,7 +3,7 @@
#include <APM_RC.h>
#include "RC_Channel_aux.h"
extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
// map a function to a servo channel and output it
void

View File

@ -44,5 +44,6 @@ public:
};
void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8);
extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
#endif /* RC_CHANNEL_AUX_H_ */