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 (2598)
2,498 additional commits have been omitted to prevent performance issues.
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 547 additions and 648 deletions
.editorconfig
View file @ 62e7778b
...@@ -13,8 +13,8 @@ indent_style = space ...@@ -13,8 +13,8 @@ indent_style = space
indent_size = 4 indent_size = 4
end_of_line = lf end_of_line = lf
charset = utf-8 charset = utf-8
trim_trailing_whitespace = true trim_trailing_whitespace = false # These are the correct rules for APM coding standards, but fixing up old files causes git spam
insert_final_newline = true insert_final_newline = false
[*.mk] [*.mk]
indent_style = tab indent_style = tab
......
.gitignore
View file @ 62e7778b
...@@ -3,11 +3,14 @@ ...@@ -3,11 +3,14 @@
*.d *.d
*.dll *.dll
*.elf *.elf
*.hex
*.dfu
*.exe *.exe
*.lst *.lst
*.o *.o
*.obj *.obj
*.px4 *.px4
*.vrx
*.pyc *.pyc
*.tlog *.tlog
*.tlog.raw *.tlog.raw
...@@ -48,20 +51,25 @@ ...@@ -48,20 +51,25 @@
/Tools/autotest/jsbsim/fgout.xml /Tools/autotest/jsbsim/fgout.xml
/Tools/autotest/jsbsim/rascal_test.xml /Tools/autotest/jsbsim/rascal_test.xml
ArduCopter/Debug/ ArduCopter/Debug/
ArduCopter/terrain/*.DAT
ArduCopter/test.ArduCopter/ ArduCopter/test.ArduCopter/
ArduCopter/test/* ArduCopter/test/*
ArduPlane/terrain/*.DAT
ArduPlane/test.ArduPlane/ ArduPlane/test.ArduPlane/
APMrover2/test.APMrover2/ APMrover2/test.APMrover2/
autotest.lck
build build
Build.ArduCopter/* Build.ArduCopter/*
Build.ArduPlane/* Build.ArduPlane/*
Build.APMrover2/* Build.APMrover2/*
Build.AntennaTracker/*
cmake_install.cmake cmake_install.cmake
CMakeCache.txt CMakeCache.txt
CMakeFiles CMakeFiles
config.mk config.mk
dataflash.bin dataflash.bin
eeprom.bin eeprom.bin
index.html
LASTLOG.TXT LASTLOG.TXT
Make.dep Make.dep
mav.log mav.log
...@@ -71,3 +79,4 @@ serialsent.raw ...@@ -71,3 +79,4 @@ serialsent.raw
status.txt status.txt
tags tags
test.ArduCopter/* test.ArduCopter/*
Thumbs.db
.travis.yml 0 → 100644
View file @ 62e7778b
language: cpp
before_install:
- APMDIR=$(pwd) && pushd .. && $APMDIR/Tools/scripts/install-prereqs-ubuntu.sh -y && . ~/.profile && popd
script:
- Tools/scripts/build_all_travis.sh
notifications:
webhooks:
urls:
- https://webhooks.gitter.im/e/e5e0b55e353e53945b4b
on_success: change # options: [always|never|change] default: always
on_failure: always # options: [always|never|change] default: always
on_start: false # default: false
APMrover2/APMrover2.pde
View file @ 62e7778b
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduRover v2.45" #define THISFIRMWARE "ArduRover v2.47"
/* /*
This program is free software: you can redistribute it and/or modify This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
...@@ -62,6 +62,7 @@ ...@@ -62,6 +62,7 @@
#include <AP_HAL.h> #include <AP_HAL.h>
#include <AP_Menu.h> #include <AP_Menu.h>
#include <AP_Param.h> #include <AP_Param.h>
#include <StorageManager.h>
#include <AP_GPS.h> // ArduPilot GPS library #include <AP_GPS.h> // ArduPilot GPS library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library #include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_ADC_AnalogSource.h> #include <AP_ADC_AnalogSource.h>
...@@ -72,6 +73,8 @@ ...@@ -72,6 +73,8 @@
#include <AP_AHRS.h> // ArduPilot Mega DCM Library #include <AP_AHRS.h> // ArduPilot Mega DCM Library
#include <AP_NavEKF.h> #include <AP_NavEKF.h>
#include <AP_Mission.h> // Mission command library #include <AP_Mission.h> // Mission command library
#include <AP_Rally.h>
#include <AP_Terrain.h>
#include <PID.h> // PID library #include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library #include <AP_RangeFinder.h> // Range finder library
...@@ -96,6 +99,7 @@ ...@@ -96,6 +99,7 @@
#include <APM_Control.h> #include <APM_Control.h>
#include <AP_L1_Control.h> #include <AP_L1_Control.h>
#include <AP_BoardConfig.h> #include <AP_BoardConfig.h>
#include <AP_Frsky_Telem.h>
#include <AP_HAL_AVR.h> #include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h> #include <AP_HAL_AVR_SITL.h>
...@@ -108,6 +112,7 @@ ...@@ -108,6 +112,7 @@
#include <AP_Notify.h> // Notify library #include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library #include <AP_BattMonitor.h> // Battery monitor library
#include <AP_OpticalFlow.h> // Optical Flow library
// Configuration // Configuration
#include "config.h" #include "config.h"
...@@ -127,7 +132,7 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; ...@@ -127,7 +132,7 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
// must be the first AP_Param variable declared to ensure its // must be the first AP_Param variable declared to ensure its
// constructor runs before the constructors of the other AP_Param // constructor runs before the constructors of the other AP_Param
// variables // variables
AP_Param param_loader(var_info, MISSION_START_BYTE); AP_Param param_loader(var_info);
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// the rate we run the main loop at // the rate we run the main loop at
...@@ -169,15 +174,8 @@ static void print_mode(AP_HAL::BetterStream *port, uint8_t mode); ...@@ -169,15 +174,8 @@ static void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
static DataFlash_APM1 DataFlash; static DataFlash_APM1 DataFlash;
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
static DataFlash_APM2 DataFlash; static DataFlash_APM2 DataFlash;
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL #elif defined(HAL_BOARD_LOG_DIRECTORY)
static DataFlash_File DataFlash("logs"); static DataFlash_File DataFlash(HAL_BOARD_LOG_DIRECTORY);
//static DataFlash_SITL DataFlash;
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
static DataFlash_File DataFlash("/fs/microsd/APM/LOGS");
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
static DataFlash_File DataFlash("logs");
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
static DataFlash_File DataFlash("/fs/microsd/APM/LOGS");
#else #else
DataFlash_Empty DataFlash; DataFlash_Empty DataFlash;
#endif #endif
...@@ -203,61 +201,28 @@ static AP_GPS gps; ...@@ -203,61 +201,28 @@ static AP_GPS gps;
// flight modes convenience array // flight modes convenience array
static AP_Int8 *modes = &g.mode1; static AP_Int8 *modes = &g.mode1;
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 static AP_Baro barometer;
static AP_ADC_ADS7844 adc;
#endif
#if CONFIG_COMPASS == AP_COMPASS_PX4 #if CONFIG_COMPASS == HAL_COMPASS_PX4
static AP_Compass_PX4 compass; static AP_Compass_PX4 compass;
#elif CONFIG_COMPASS == AP_COMPASS_VRBRAIN #elif CONFIG_COMPASS == HAL_COMPASS_VRBRAIN
static AP_Compass_VRBRAIN compass; static AP_Compass_VRBRAIN compass;
#elif CONFIG_COMPASS == AP_COMPASS_HMC5843 #elif CONFIG_COMPASS == HAL_COMPASS_HMC5843
static AP_Compass_HMC5843 compass; static AP_Compass_HMC5843 compass;
#elif CONFIG_COMPASS == AP_COMPASS_HIL #elif CONFIG_COMPASS == HAL_COMPASS_HIL
static AP_Compass_HIL compass; static AP_Compass_HIL compass;
#elif CONFIG_COMPASS == HAL_COMPASS_AK8963
static AP_Compass_AK8963_MPU9250 compass;
#else #else
#error Unrecognized CONFIG_COMPASS setting #error Unrecognized CONFIG_COMPASS setting
#endif #endif
#if CONFIG_INS_TYPE == CONFIG_INS_MPU6000 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
AP_InertialSensor_MPU6000 ins; AP_ADC_ADS7844 apm1_adc;
#elif CONFIG_INS_TYPE == CONFIG_INS_PX4
AP_InertialSensor_PX4 ins;
#elif CONFIG_INS_TYPE == CONFIG_INS_VRBRAIN
AP_InertialSensor_VRBRAIN ins;
#elif CONFIG_INS_TYPE == CONFIG_INS_HIL
AP_InertialSensor_HIL ins;
#elif CONFIG_INS_TYPE == CONFIG_INS_FLYMAPLE
AP_InertialSensor_Flymaple ins;
#elif CONFIG_INS_TYPE == CONFIG_INS_L3G4200D
AP_InertialSensor_L3G4200D ins;
#elif CONFIG_INS_TYPE == CONFIG_INS_OILPAN
AP_InertialSensor_Oilpan ins( &adc );
#else
#error Unrecognised CONFIG_INS_TYPE setting.
#endif // CONFIG_INS_TYPE
#if CONFIG_BARO == AP_BARO_BMP085
static AP_Baro_BMP085 barometer;
#elif CONFIG_BARO == AP_BARO_PX4
static AP_Baro_PX4 barometer;
#elif CONFIG_BARO == AP_BARO_VRBRAIN
static AP_Baro_VRBRAIN barometer;
#elif CONFIG_BARO == AP_BARO_HIL
static AP_Baro_HIL barometer;
#elif CONFIG_BARO == AP_BARO_MS5611
#if CONFIG_MS5611_SERIAL == AP_BARO_MS5611_SPI
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi);
#elif CONFIG_MS5611_SERIAL == AP_BARO_MS5611_I2C
static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::i2c);
#else
#error Unrecognized CONFIG_MS5611_SERIAL setting.
#endif
#else
#error Unrecognized CONFIG_BARO setting
#endif #endif
AP_InertialSensor ins;
// Inertial Navigation EKF // Inertial Navigation EKF
#if AP_AHRS_NAVEKF_AVAILABLE #if AP_AHRS_NAVEKF_AVAILABLE
AP_AHRS_NavEKF ahrs(ins, barometer, gps); AP_AHRS_NavEKF ahrs(ins, barometer, gps);
...@@ -280,7 +245,9 @@ static AP_SteerController steerController(ahrs); ...@@ -280,7 +245,9 @@ static AP_SteerController steerController(ahrs);
static bool start_command(const AP_Mission::Mission_Command& cmd); static bool start_command(const AP_Mission::Mission_Command& cmd);
static bool verify_command(const AP_Mission::Mission_Command& cmd); static bool verify_command(const AP_Mission::Mission_Command& cmd);
static void exit_mission(); static void exit_mission();
AP_Mission mission(ahrs, &start_command, &verify_command, &exit_mission, MISSION_START_BYTE, MISSION_END_BYTE); AP_Mission mission(ahrs, &start_command, &verify_command, &exit_mission);
static OpticalFlow optflow;
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
SITL sitl; SITL sitl;
...@@ -298,11 +265,8 @@ static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; ...@@ -298,11 +265,8 @@ static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
AP_HAL::AnalogSource *rssi_analog_source; AP_HAL::AnalogSource *rssi_analog_source;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// SONAR selection // SONAR
//////////////////////////////////////////////////////////////////////////////// static RangeFinder sonar;
//
static AP_RangeFinder_analog sonar;
static AP_RangeFinder_analog sonar2;
// relay support // relay support
AP_Relay relay; AP_Relay relay;
...@@ -446,6 +410,13 @@ static bool ch7_flag; ...@@ -446,6 +410,13 @@ static bool ch7_flag;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
static AP_BattMonitor battery; static AP_BattMonitor battery;
////////////////////////////////////////////////////////////////////////////////
// Battery Sensors
////////////////////////////////////////////////////////////////////////////////
#if FRSKY_TELEM_ENABLED == ENABLED
static AP_Frsky_Telem frsky_telemetry(ahrs, battery);
#endif
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Navigation control variables // Navigation control variables
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
...@@ -549,7 +520,10 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { ...@@ -549,7 +520,10 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ gcs_failsafe_check, 5, 600 }, { gcs_failsafe_check, 5, 600 },
{ compass_accumulate, 1, 900 }, { compass_accumulate, 1, 900 },
{ update_notify, 1, 300 }, { update_notify, 1, 300 },
{ one_second_loop, 50, 3000 } { one_second_loop, 50, 3000 },
#if FRSKY_TELEM_ENABLED == ENABLED
{ telemetry_send, 10, 100 }
#endif
}; };
...@@ -565,6 +539,7 @@ void setup() { ...@@ -565,6 +539,7 @@ void setup() {
// rover does not use arming nor pre-arm checks // rover does not use arming nor pre-arm checks
AP_Notify::flags.armed = true; AP_Notify::flags.armed = true;
AP_Notify::flags.pre_arm_check = true; AP_Notify::flags.pre_arm_check = true;
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.failsafe_battery = false; AP_Notify::flags.failsafe_battery = false;
notify.init(false); notify.init(false);
...@@ -585,9 +560,8 @@ void setup() { ...@@ -585,9 +560,8 @@ void setup() {
void loop() void loop()
{ {
// wait for an INS sample // wait for an INS sample
if (!ins.wait_for_sample(1000)) { ins.wait_for_sample();
return;
}
uint32_t timer = hal.scheduler->micros(); uint32_t timer = hal.scheduler->micros();
delta_us_fast_loop = timer - fast_loopTimer_us; delta_us_fast_loop = timer - fast_loopTimer_us;
...@@ -651,7 +625,7 @@ static void mount_update(void) ...@@ -651,7 +625,7 @@ static void mount_update(void)
static void update_alt() static void update_alt()
{ {
barometer.read(); barometer.update();
if (should_log(MASK_LOG_IMU)) { if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro(); Log_Write_Baro();
} }
......
APMrover2/GCS_Mavlink.pde
View file @ 62e7778b
This diff is collapsed.
APMrover2/Log.pde
View file @ 62e7778b
...@@ -202,37 +202,6 @@ static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd) ...@@ -202,37 +202,6 @@ static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd)
DataFlash.Log_Write_MavCmd(mission.num_commands(),mav_cmd); DataFlash.Log_Write_MavCmd(mission.num_commands(),mav_cmd);
} }
struct PACKED log_Camera {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint32_t gps_time;
uint16_t gps_week;
int32_t latitude;
int32_t longitude;
int16_t roll;
int16_t pitch;
uint16_t yaw;
};
// Write a Camera packet. Total length : 26 bytes
static void Log_Write_Camera()
{
#if CAMERA == ENABLED
struct log_Camera pkt = {
LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG),
time_ms : millis(),
gps_time : gps.time_week_ms(),
gps_week : gps.time_week(),
latitude : current_loc.lat,
longitude : current_loc.lng,
roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor,
yaw : (uint16_t)ahrs.yaw_sensor
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#endif
}
struct PACKED log_Steering { struct PACKED log_Steering {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint32_t time_ms; uint32_t time_ms;
...@@ -350,7 +319,11 @@ static void Log_Write_Attitude() ...@@ -350,7 +319,11 @@ static void Log_Write_Attitude()
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
#if AP_AHRS_NAVEKF_AVAILABLE #if AP_AHRS_NAVEKF_AVAILABLE
DataFlash.Log_Write_EKF(ahrs); #if OPTFLOW == ENABLED
DataFlash.Log_Write_EKF(ahrs,optflow.enabled());
#else
DataFlash.Log_Write_EKF(ahrs,false);
#endif
DataFlash.Log_Write_AHRS2(ahrs); DataFlash.Log_Write_AHRS2(ahrs);
#endif #endif
} }
...@@ -399,8 +372,8 @@ static void Log_Write_Sonar() ...@@ -399,8 +372,8 @@ static void Log_Write_Sonar()
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG), LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
time_ms : millis(), time_ms : millis(),
lateral_accel : lateral_acceleration, lateral_accel : lateral_acceleration,
sonar1_distance : (uint16_t)sonar.distance_cm(), sonar1_distance : (uint16_t)sonar.distance_cm(0),
sonar2_distance : (uint16_t)sonar2.distance_cm(), sonar2_distance : (uint16_t)sonar.distance_cm(1),
detected_count : obstacle.detected_count, detected_count : obstacle.detected_count,
turn_angle : (int8_t)obstacle.turn_angle, turn_angle : (int8_t)obstacle.turn_angle,
turn_time : turn_time, turn_time : turn_time,
...@@ -492,6 +465,27 @@ static void Log_Write_Compass() ...@@ -492,6 +465,27 @@ static void Log_Write_Compass()
DataFlash.WriteBlock(&pkt2, sizeof(pkt2)); DataFlash.WriteBlock(&pkt2, sizeof(pkt2));
} }
#endif #endif
#if COMPASS_MAX_INSTANCES > 2
if (compass.get_count() > 2) {
const Vector3f &mag3_offsets = compass.get_offsets(2);
const Vector3f &mag3_motor_offsets = compass.get_motor_offsets(2);
const Vector3f &mag3 = compass.get_field(2);
struct log_Compass pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS3_MSG),
time_ms : millis(),
mag_x : (int16_t)mag3.x,
mag_y : (int16_t)mag3.y,
mag_z : (int16_t)mag3.z,
offset_x : (int16_t)mag3_offsets.x,
offset_y : (int16_t)mag3_offsets.y,
offset_z : (int16_t)mag3_offsets.z,
motor_offset_x : (int16_t)mag3_motor_offsets.x,
motor_offset_y : (int16_t)mag3_motor_offsets.y,
motor_offset_z : (int16_t)mag3_motor_offsets.z
};
DataFlash.WriteBlock(&pkt3, sizeof(pkt3));
}
#endif
} }
...@@ -512,8 +506,6 @@ static const struct LogStructure log_structure[] PROGMEM = { ...@@ -512,8 +506,6 @@ static const struct LogStructure log_structure[] PROGMEM = {
"ATT", "IccC", "TimeMS,Roll,Pitch,Yaw" }, "ATT", "IccC", "TimeMS,Roll,Pitch,Yaw" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance), { LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "IIHIhhhBH", "TimeMS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" }, "PM", "IIHIhhhBH", "TimeMS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" },
{ LOG_CAMERA_MSG, sizeof(log_Camera),
"CAM", "IIHLLeccC", "TimeMS,GPSTime,GPSWeek,Lat,Lng,Alt,Roll,Pitch,Yaw" },
{ LOG_STARTUP_MSG, sizeof(log_Startup), { LOG_STARTUP_MSG, sizeof(log_Startup),
"STRT", "IBH", "TimeMS,SType,CTot" }, "STRT", "IBH", "TimeMS,SType,CTot" },
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning), { LOG_CTUN_MSG, sizeof(log_Control_Tuning),
......
APMrover2/Parameters.h
View file @ 62e7778b
...@@ -51,6 +51,9 @@ public: ...@@ -51,6 +51,9 @@ public:
// misc2 // misc2
k_param_log_bitmask = 40, k_param_log_bitmask = 40,
k_param_gps, k_param_gps,
k_param_serial0_baud,
k_param_serial1_baud,
k_param_serial2_baud,
// 110: Telemetry control // 110: Telemetry control
...@@ -59,12 +62,13 @@ public: ...@@ -59,12 +62,13 @@ public:
k_param_gcs1, // stream rates for uartC k_param_gcs1, // stream rates for uartC
k_param_sysid_this_mav, k_param_sysid_this_mav,
k_param_sysid_my_gcs, k_param_sysid_my_gcs,
k_param_serial0_baud, k_param_serial0_baud_old,
k_param_serial1_baud, k_param_serial1_baud_old,
k_param_telem_delay, k_param_telem_delay,
k_param_skip_gyro_cal, k_param_skip_gyro_cal,
k_param_gcs2, // stream rates for uartD k_param_gcs2, // stream rates for uartD
k_param_serial2_baud, k_param_serial2_baud_old,
k_param_serial2_protocol,
// //
// 130: Sensor parameters // 130: Sensor parameters
...@@ -126,12 +130,13 @@ public: ...@@ -126,12 +130,13 @@ public:
// obstacle control // obstacle control
k_param_sonar_enabled = 190, // deprecated, can be removed k_param_sonar_enabled = 190, // deprecated, can be removed
k_param_sonar, // sonar object k_param_sonar_old, // unused
k_param_sonar_trigger_cm, k_param_sonar_trigger_cm,
k_param_sonar_turn_angle, k_param_sonar_turn_angle,
k_param_sonar_turn_time, k_param_sonar_turn_time,
k_param_sonar2, // sonar2 object k_param_sonar2_old, // unused
k_param_sonar_debounce, k_param_sonar_debounce,
k_param_sonar, // sonar object
// //
// 210: driving modes // 210: driving modes
...@@ -205,10 +210,11 @@ public: ...@@ -205,10 +210,11 @@ public:
// //
AP_Int16 sysid_this_mav; AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs; AP_Int16 sysid_my_gcs;
AP_Int8 serial0_baud; AP_Int16 serial0_baud;
AP_Int8 serial1_baud; AP_Int16 serial1_baud;
#if MAVLINK_COMM_NUM_BUFFERS > 2 #if MAVLINK_COMM_NUM_BUFFERS > 2
AP_Int8 serial2_baud; AP_Int16 serial2_baud;
AP_Int8 serial2_protocol;
#endif #endif
AP_Int8 telem_delay; AP_Int8 telem_delay;
AP_Int8 skip_gyro_cal; AP_Int8 skip_gyro_cal;
......
APMrover2/Parameters.pde
View file @ 62e7778b
...@@ -58,26 +58,37 @@ const AP_Param::Info var_info[] PROGMEM = { ...@@ -58,26 +58,37 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: SERIAL0_BAUD // @Param: SERIAL0_BAUD
// @DisplayName: USB Console Baud Rate // @DisplayName: USB Console Baud Rate
// @Description: The baud rate used on the USB console // @Description: The baud rate used on the USB console. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard // @User: Standard
GSCALAR(serial0_baud, "SERIAL0_BAUD", 115), GSCALAR(serial0_baud, "SERIAL0_BAUD", SERIAL0_BAUD/1000),
// @Param: SERIAL1_BAUD // @Param: SERIAL1_BAUD
// @DisplayName: Telemetry Baud Rate // @DisplayName: Telemetry Baud Rate
// @Description: The baud rate used on the first telemetry port // @Description: The baud rate used on the first telemetry port. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard // @User: Standard
GSCALAR(serial1_baud, "SERIAL1_BAUD", 57), GSCALAR(serial1_baud, "SERIAL1_BAUD", SERIAL1_BAUD/1000),
#if MAVLINK_COMM_NUM_BUFFERS > 2 #if MAVLINK_COMM_NUM_BUFFERS > 2
// @Param: SERIAL2_BAUD // @Param: SERIAL2_BAUD
// @DisplayName: Telemetry Baud Rate // @DisplayName: Telemetry Baud Rate
// @Description: The baud rate used on the second telemetry port (where available) // @Description: The baud rate used on the second telemetry port. The APM2 can support all baudrates up to 115, and also can support 500. The PX4 can support rates of up to 1500. If you setup a rate you cannot support on APM2 and then can't connect to your board you should load a firmware from a different vehicle type. That will reset all your parameters to defaults.
// @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
// @User: Standard // @User: Standard
GSCALAR(serial2_baud, "SERIAL2_BAUD", 57), GSCALAR(serial2_baud, "SERIAL2_BAUD", SERIAL2_BAUD/1000),
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
// @Param: SERIAL2_PROTOCOL
// @DisplayName: SERIAL2 protocol selection
// @Description: Control what protocol telemetry 2 port should be used for
// @Values: 1:GCS Mavlink,2:Frsky D-PORT
// @User: Standard
GSCALAR(serial2_protocol, "SERIAL2_PROTOCOL", SERIAL2_MAVLINK),
#endif // FRSKY_TELEM_ENABLED
#endif // MAVLINK_COMM_NUM_BUFFERS
// @Param: TELEM_DELAY // @Param: TELEM_DELAY
// @DisplayName: Telemetry startup delay // @DisplayName: Telemetry startup delay
...@@ -270,12 +281,12 @@ const AP_Param::Info var_info[] PROGMEM = { ...@@ -270,12 +281,12 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: THR_SLEWRATE // @Param: THR_SLEWRATE
// @DisplayName: Throttle slew rate // @DisplayName: Throttle slew rate
// @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. A value of zero means no limit. // @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. A value of zero means no limit. A value of 100 means the throttle can change over its full range in one second. Note that for some NiMH powered rovers setting a lower value like 40 or 50 may be worthwhile as the sudden current demand on the battery of a big rise in throttle may cause a brownout.
// @Units: Percent // @Units: Percent
// @Range: 0 100 // @Range: 0 100
// @Increment: 1 // @Increment: 1
// @User: Standard // @User: Standard
GSCALAR(throttle_slewrate, "THR_SLEWRATE", 0), GSCALAR(throttle_slewrate, "THR_SLEWRATE", 100),
// @Param: SKID_STEER_OUT // @Param: SKID_STEER_OUT
// @DisplayName: Skid steering output // @DisplayName: Skid steering output
...@@ -327,40 +338,40 @@ const AP_Param::Info var_info[] PROGMEM = { ...@@ -327,40 +338,40 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard // @User: Standard
GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE", 0), GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE", 0),
// @Param: SONAR_TRIGGER_CM // @Param: RNGFND_TRIGGR_CM
// @DisplayName: Sonar trigger distance // @DisplayName: Rangefinder trigger distance
// @Description: The distance from an obstacle in centimeters at which the sonar triggers a turn to avoid the obstacle // @Description: The distance from an obstacle in centimeters at which the rangefinder triggers a turn to avoid the obstacle
// @Units: centimeters // @Units: centimeters
// @Range: 0 1000 // @Range: 0 1000
// @Increment: 1 // @Increment: 1
// @User: Standard // @User: Standard
GSCALAR(sonar_trigger_cm, "SONAR_TRIGGER_CM", 100), GSCALAR(sonar_trigger_cm, "RNGFND_TRIGGR_CM", 100),
// @Param: SONAR_TURN_ANGLE // @Param: RNGFND_TURN_ANGL
// @DisplayName: Sonar trigger angle // @DisplayName: Rangefinder trigger angle
// @Description: The course deviation in degrees to apply while avoiding an obstacle detected with the sonar. A positive number means to turn right, and a negative angle means to turn left. // @Description: The course deviation in degrees to apply while avoiding an obstacle detected with the rangefinder. A positive number means to turn right, and a negative angle means to turn left.
// @Units: centimeters // @Units: centimeters
// @Range: -45 45 // @Range: -45 45
// @Increment: 1 // @Increment: 1
// @User: Standard // @User: Standard
GSCALAR(sonar_turn_angle, "SONAR_TURN_ANGLE", 45), GSCALAR(sonar_turn_angle, "RNGFND_TURN_ANGL", 45),
// @Param: SONAR_TURN_TIME // @Param: RNGFND_TURN_TIME
// @DisplayName: Sonar turn time // @DisplayName: Rangefinder turn time
// @Description: The amount of time in seconds to apply the SONAR_TURN_ANGLE after detecting an obstacle. // @Description: The amount of time in seconds to apply the RNGFND_TURN_ANGL after detecting an obstacle.
// @Units: seconds // @Units: seconds
// @Range: 0 100 // @Range: 0 100
// @Increment: 0.1 // @Increment: 0.1
// @User: Standard // @User: Standard
GSCALAR(sonar_turn_time, "SONAR_TURN_TIME", 1.0f), GSCALAR(sonar_turn_time, "RNGFND_TURN_TIME", 1.0f),
// @Param: SONAR_DEBOUNCE // @Param: RNGFND_DEBOUNCE
// @DisplayName: Sonar debounce count // @DisplayName: Rangefinder debounce count
// @Description: The number of 50Hz sonar hits needed to trigger an obstacle avoidance event. If you get a lot of false sonar events then raise this number, but if you make it too large then it will cause lag in detecting obstacles, which could cause you go hit the obstacle. // @Description: The number of 50Hz rangefinder hits needed to trigger an obstacle avoidance event. If you get a lot of false sonar events then raise this number, but if you make it too large then it will cause lag in detecting obstacles, which could cause you go hit the obstacle.
// @Range: 1 100 // @Range: 1 100
// @Increment: 1 // @Increment: 1
// @User: Standard // @User: Standard
GSCALAR(sonar_debounce, "SONAR_DEBOUNCE", 2), GSCALAR(sonar_debounce, "RNGFND_DEBOUNCE", 2),
// @Param: LEARN_CH // @Param: LEARN_CH
// @DisplayName: Learning channel // @DisplayName: Learning channel
...@@ -482,13 +493,9 @@ const AP_Param::Info var_info[] PROGMEM = { ...@@ -482,13 +493,9 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
GOBJECT(L1_controller, "NAVL1_", AP_L1_Control), GOBJECT(L1_controller, "NAVL1_", AP_L1_Control),
// @Group: SONAR_ // @Group: RNGFND
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
GOBJECT(sonar, "SONAR_", AP_RangeFinder_analog), GOBJECT(sonar, "RNGFND", RangeFinder),
// @Group: SONAR2_
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
GOBJECT(sonar2, "SONAR2_", AP_RangeFinder_analog),
// @Group: INS_ // @Group: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
...@@ -518,7 +525,7 @@ const AP_Param::Info var_info[] PROGMEM = { ...@@ -518,7 +525,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Group: BATT_ // @Group: BATT_
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
GOBJECT(battery, "BATT_", AP_BattMonitor), GOBJECT(battery, "BATT", AP_BattMonitor),
// @Group: BRD_ // @Group: BRD_
// @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp
......
APMrover2/Steering.pde
View file @ 62e7778b
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
static void throttle_slew_limit(int16_t last_throttle) static void throttle_slew_limit(int16_t last_throttle)
{ {
// if slew limit rate is set to zero then do not slew limit // if slew limit rate is set to zero then do not slew limit
if (g.throttle_slewrate) { if (g.throttle_slewrate && last_throttle != 0) {
// limit throttle change by the given percentage per second // limit throttle change by the given percentage per second
float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min); float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
// allow a minimum change of 1 PWM per cycle // allow a minimum change of 1 PWM per cycle
...@@ -109,7 +109,7 @@ static void calc_throttle(float target_speed) ...@@ -109,7 +109,7 @@ static void calc_throttle(float target_speed)
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) { if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
// in auto-modes we reduce speed when approaching waypoints // in auto-modes we reduce speed when approaching waypoints
float reduction2 = 1.0 - speed_turn_reduction*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist); float reduction2 = 1.0 - speed_turn_reduction;
if (reduction2 < reduction) { if (reduction2 < reduction) {
reduction = reduction2; reduction = reduction2;
} }
...@@ -143,7 +143,7 @@ static void calc_throttle(float target_speed) ...@@ -143,7 +143,7 @@ static void calc_throttle(float target_speed)
float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1); float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain; int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
channel_throttle->servo_out = constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min); channel_throttle->servo_out = constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min);
// temporarily set us in reverse to allow the PWM setting to // temporarily set us in reverse to allow the PWM setting to
// go negative // go negative
set_reverse(true); set_reverse(true);
...@@ -208,13 +208,12 @@ static void calc_nav_steer() ...@@ -208,13 +208,12 @@ static void calc_nav_steer()
*****************************************/ *****************************************/
static void set_servos(void) static void set_servos(void)
{ {
int16_t last_throttle = channel_throttle->radio_out; static int16_t last_throttle;
// support a separate steering channel // support a separate steering channel
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0)); RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0));
if ((control_mode == MANUAL || control_mode == LEARNING) && if (control_mode == MANUAL || control_mode == LEARNING) {
(g.skid_steer_out == g.skid_steer_in)) {
// do a direct pass through of radio values // do a direct pass through of radio values
channel_steer->radio_out = channel_steer->read(); channel_steer->radio_out = channel_steer->read();
channel_throttle->radio_out = channel_throttle->read(); channel_throttle->radio_out = channel_throttle->read();
...@@ -244,25 +243,28 @@ static void set_servos(void) ...@@ -244,25 +243,28 @@ static void set_servos(void)
// limit throttle movement speed // limit throttle movement speed
throttle_slew_limit(last_throttle); throttle_slew_limit(last_throttle);
}
if (g.skid_steer_out) { // record last throttle before we apply skid steering
// convert the two radio_out values to skid steering values last_throttle = channel_throttle->radio_out;
/*
mixing rule: if (g.skid_steer_out) {
steering = motor1 - motor2 // convert the two radio_out values to skid steering values
throttle = 0.5*(motor1 + motor2) /*
motor1 = throttle + 0.5*steering mixing rule:
motor2 = throttle - 0.5*steering steering = motor1 - motor2
*/ throttle = 0.5*(motor1 + motor2)
float steering_scaled = channel_steer->norm_output(); motor1 = throttle + 0.5*steering
float throttle_scaled = channel_throttle->norm_output(); motor2 = throttle - 0.5*steering
float motor1 = throttle_scaled + 0.5*steering_scaled; */
float motor2 = throttle_scaled - 0.5*steering_scaled; float steering_scaled = channel_steer->norm_output();
channel_steer->servo_out = 4500*motor1; float throttle_scaled = channel_throttle->norm_output();
channel_throttle->servo_out = 100*motor2; float motor1 = throttle_scaled + 0.5*steering_scaled;
channel_steer->calc_pwm(); float motor2 = throttle_scaled - 0.5*steering_scaled;
channel_throttle->calc_pwm(); channel_steer->servo_out = 4500*motor1;
} channel_throttle->servo_out = 100*motor2;
channel_steer->calc_pwm();
channel_throttle->calc_pwm();
} }
......
APMrover2/commands_logic.pde
View file @ 62e7778b
...@@ -96,12 +96,17 @@ start_command(const AP_Mission::Mission_Command& cmd) ...@@ -96,12 +96,17 @@ start_command(const AP_Mission::Mission_Command& cmd)
// system to control the vehicle attitude and the attitude of various // system to control the vehicle attitude and the attitude of various
// devices such as cameras. // devices such as cameras.
// |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| // |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
case MAV_CMD_DO_SET_ROI: case MAV_CMD_DO_SET_ROI:
#if 0 if (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
// not supported yet // switch off the camera tracking if enabled
camera_mount.set_roi_cmd(&cmd.content.location); if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
#endif camera_mount.set_mode_to_default();
break; }
} else {
// send the command to the camera mount
camera_mount.set_roi_cmd(&cmd.content.location);
}
break;
case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
camera_mount.configure_cmd(); camera_mount.configure_cmd();
...@@ -161,6 +166,10 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) ...@@ -161,6 +166,10 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)
break; break;
default: default:
if (cmd.id > MAV_CMD_CONDITION_LAST) {
// this is a command that doesn't require verify
return true;
}
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command")); gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command"));
return true; return true;
break; break;
...@@ -299,8 +308,9 @@ static void do_take_picture() ...@@ -299,8 +308,9 @@ static void do_take_picture()
{ {
#if CAMERA == ENABLED #if CAMERA == ENABLED
camera.trigger_pic(); camera.trigger_pic();
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) { if (should_log(MASK_LOG_CAMERA)) {
Log_Write_Camera(); DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
} }
#endif #endif
} }
APMrover2/compat.h
View file @ 62e7778b
...@@ -2,9 +2,6 @@ ...@@ -2,9 +2,6 @@
#ifndef __COMPAT_H__ #ifndef __COMPAT_H__
#define __COMPAT_H__ #define __COMPAT_H__
#define OUTPUT GPIO_OUTPUT
#define INPUT GPIO_INPUT
#define HIGH 1 #define HIGH 1
#define LOW 0 #define LOW 0
......
APMrover2/config.h
View file @ 62e7778b
...@@ -49,70 +49,52 @@ ...@@ -49,70 +49,52 @@
#define DISABLED 0 #define DISABLED 0
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////// // sensor types
// HARDWARE CONFIGURATION AND CONNECTIONS
////////////////////////////////////////////////////////////////////////////// #define CONFIG_BARO HAL_BARO_DEFAULT
////////////////////////////////////////////////////////////////////////////// #define CONFIG_COMPASS HAL_COMPASS_DEFAULT
#ifdef HAL_SERIAL0_BAUD_DEFAULT
# define SERIAL0_BAUD HAL_SERIAL0_BAUD_DEFAULT
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// LED and IO Pins // HIL_MODE OPTIONAL
//
#ifndef HIL_MODE
#define HIL_MODE HIL_MODE_DISABLED
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
#undef CONFIG_BARO
#define CONFIG_BARO HAL_BARO_HIL
#undef CONFIG_COMPASS
#define CONFIG_COMPASS HAL_COMPASS_HIL
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
# define CONFIG_INS_TYPE CONFIG_INS_OILPAN
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# define CONFIG_BARO AP_BARO_BMP085
# define BATTERY_PIN_1 0 # define BATTERY_PIN_1 0
# define CURRENT_PIN_1 1 # define CURRENT_PIN_1 1
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define CONFIG_INS_TYPE CONFIG_INS_MPU6000
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# ifdef APM2_BETA_HARDWARE
# define CONFIG_BARO AP_BARO_BMP085
# else // APM2 Production Hardware (default)
# define CONFIG_BARO AP_BARO_MS5611
# define CONFIG_MS5611_SERIAL AP_BARO_MS5611_SPI
# endif
# define BATTERY_PIN_1 1 # define BATTERY_PIN_1 1
# define CURRENT_PIN_1 2 # define CURRENT_PIN_1 2
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL #elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
# define CONFIG_INS_TYPE CONFIG_INS_HIL
# define CONFIG_COMPASS AP_COMPASS_HIL
# define CONFIG_BARO AP_BARO_HIL
# define BATTERY_PIN_1 1 # define BATTERY_PIN_1 1
# define CURRENT_PIN_1 2 # define CURRENT_PIN_1 2
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
# define CONFIG_INS_TYPE CONFIG_INS_PX4
# define CONFIG_COMPASS AP_COMPASS_PX4
# define CONFIG_BARO AP_BARO_PX4
# define BATTERY_PIN_1 -1 # define BATTERY_PIN_1 -1
# define CURRENT_PIN_1 -1 # define CURRENT_PIN_1 -1
#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE #elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
# define CONFIG_INS_TYPE CONFIG_INS_FLYMAPLE
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# define CONFIG_BARO AP_BARO_BMP085
# define BATTERY_PIN_1 20 # define BATTERY_PIN_1 20
# define CURRENT_PIN_1 19 # define CURRENT_PIN_1 19
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
# define CONFIG_INS_TYPE CONFIG_INS_L3G4200D
# define CONFIG_COMPASS AP_COMPASS_HMC5843
# define CONFIG_BARO AP_BARO_BMP085
# define BATTERY_PIN_1 -1 # define BATTERY_PIN_1 -1
# define CURRENT_PIN_1 -1 # define CURRENT_PIN_1 -1
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
# define CONFIG_INS_TYPE CONFIG_INS_VRBRAIN
# define CONFIG_COMPASS AP_COMPASS_VRBRAIN
# define CONFIG_BARO AP_BARO_VRBRAIN
# define BATTERY_PIN_1 -1 # define BATTERY_PIN_1 -1
# define CURRENT_PIN_1 -1 # define CURRENT_PIN_1 -1
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// IMU Selection
//
#ifndef CONFIG_INS_TYPE
# define CONFIG_INS_TYPE CONFIG_INS_OILPAN
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// HIL_MODE OPTIONAL // HIL_MODE OPTIONAL
...@@ -120,13 +102,6 @@ ...@@ -120,13 +102,6 @@
#define HIL_MODE HIL_MODE_DISABLED #define HIL_MODE HIL_MODE_DISABLED
#endif #endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
#undef CONFIG_INS_TYPE
#define CONFIG_INS_TYPE CONFIG_INS_HIL
#undef CONFIG_COMPASS
#define CONFIG_COMPASS AP_COMPASS_HIL
#endif
#ifndef MAV_SYSTEM_ID #ifndef MAV_SYSTEM_ID
# define MAV_SYSTEM_ID 1 # define MAV_SYSTEM_ID 1
#endif #endif
...@@ -144,6 +119,19 @@ ...@@ -144,6 +119,19 @@
# define SERIAL2_BAUD 57600 # define SERIAL2_BAUD 57600
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// FrSky telemetry support
//
#ifndef FRSKY_TELEM_ENABLED
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define FRSKY_TELEM_ENABLED DISABLED
#else
# define FRSKY_TELEM_ENABLED ENABLED
#endif
#endif
#ifndef CH7_OPTION #ifndef CH7_OPTION
# define CH7_OPTION CH7_SAVE_WP # define CH7_OPTION CH7_SAVE_WP
#endif #endif
......
APMrover2/defines.h
View file @ 62e7778b
...@@ -71,9 +71,9 @@ enum mode { ...@@ -71,9 +71,9 @@ enum mode {
#define LOG_ATTITUDE_MSG 0x08 #define LOG_ATTITUDE_MSG 0x08
#define LOG_MODE_MSG 0x09 #define LOG_MODE_MSG 0x09
#define LOG_COMPASS_MSG 0x0A #define LOG_COMPASS_MSG 0x0A
#define LOG_CAMERA_MSG 0x0B
#define LOG_COMPASS2_MSG 0x0C #define LOG_COMPASS2_MSG 0x0C
#define LOG_STEERING_MSG 0x0D #define LOG_STEERING_MSG 0x0D
#define LOG_COMPASS3_MSG 0x0E
#define TYPE_AIRSTART_MSG 0x00 #define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01 #define TYPE_GROUNDSTART_MSG 0x01
...@@ -126,38 +126,16 @@ enum mode { ...@@ -126,38 +126,16 @@ enum mode {
#define SPEEDFILT 400 // centimeters/second; the speed below which a groundstart will be triggered #define SPEEDFILT 400 // centimeters/second; the speed below which a groundstart will be triggered
// EEPROM addresses
// parameters get the first 1KiB of EEPROM, remainder is for mission commands
#define MISSION_START_BYTE 0x500
#define MISSION_END_BYTE HAL_STORAGE_SIZE_AVAILABLE
// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1) // convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1)
#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1) #define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1)
// mark a function as not to be inlined // mark a function as not to be inlined
#define NOINLINE __attribute__((noinline)) #define NOINLINE __attribute__((noinline))
// InertialSensor driver types enum Serial2Protocol {
#define CONFIG_INS_OILPAN 1 SERIAL2_MAVLINK = 1,
#define CONFIG_INS_MPU6000 2 SERIAL2_FRSKY_DPORT = 2,
#define CONFIG_INS_HIL 3 SERIAL2_FRSKY_SPORT = 3 // not supported yet
#define CONFIG_INS_PX4 4 };
#define CONFIG_INS_FLYMAPLE 5
#define CONFIG_INS_L3G4200D 6
#define CONFIG_INS_VRBRAIN 7
// barometer driver types
#define AP_BARO_BMP085 1
#define AP_BARO_MS5611 2
#define AP_BARO_PX4 3
#define AP_BARO_HIL 4
#define AP_BARO_VRBRAIN 5
// compass driver types
#define AP_COMPASS_HMC5843 1
#define AP_COMPASS_PX4 2
#define AP_COMPASS_HIL 3
#define AP_COMPASS_VRBRAIN 4
#endif // _DEFINES_H #endif // _DEFINES_H
APMrover2/radio.pde
View file @ 62e7778b
...@@ -12,6 +12,10 @@ static void set_control_channels(void) ...@@ -12,6 +12,10 @@ static void set_control_channels(void)
// set rc channel ranges // set rc channel ranges
channel_steer->set_angle(SERVO_MAX); channel_steer->set_angle(SERVO_MAX);
channel_throttle->set_angle(100); channel_throttle->set_angle(100);
// setup correct scaling for ESCs like the UAVCAN PX4ESC which
// take a proportion of speed.
hal.rcout->set_esc_scaling(channel_throttle->radio_min, channel_throttle->radio_max);
} }
static void init_rc_in() static void init_rc_in()
......
APMrover2/release-notes.txt 0 → 100644
View file @ 62e7778b
Release 2.47, November 15th 2014
--------------------------------
The ardupilot development team is proud to announce the release of
version 2.47 of APM:Rover. This is a minor bug fix release. The most
important change in this release is the fixing of the skid steering
support but there have been a number of fixes in other areas as well.
Full changes list for this release:
- add support for controlling safety switch on Pixhawk from ground station
- prevent reports of failed AHRS during initialisation
- fixed skid steering that was broken in the last release
- report gyro unhealthy if gyro calibration failed
- fixed dual sonar support in CLI sonar test
- fixed Nuttx crash on Pixhawk with bad I2C cables
- added GPS_SBAS_MODE parameter - turns on/off satellite based augemtation system for GPS
- added GPS_MIN_ELEV parameter - specifiy the elevation mask for GPS satellites
- added RELAY_DEFAULT parameter to control default of relay on startup
- fixed bug in FRAM storage on Pixhawk that could cause parameters changes not to be saved
- better handling of compass errors in the EKF (Extended Kalman Filter)
- improved support for linux based autopilots
- added support for PulsedLight LIDAR as a range finder
Many thanks to everyone who contributed to this release, especially
Tom Coyle and Linus Penzlien for their excellent testing and feedback.
Happy driving!
Release 2.46, August 26th 2014
------------------------------
The ardupilot development team is proud to announce the release of
version 2.46 of APM:Rover. This is a major release with a lot of new
features and bug fixes.
This release is based on a lot of development and testing that
happened prior to the AVC competition where APM based vehicles
performed very well.
Full changes list for this release:
- added support for higher baudrates on telemetry ports, to make it
easier to use high rate telemetry to companion boards. Rates of up
to 1.5MBit are now supported to companion boards.
- new Rangefinder code with support for a wider range of rangefinder
types including a range of Lidars (thanks to Allyson Kreft)
- added logging of power status on Pixhawk
- added PIVOT_TURN_ANGLE parameter for pivot based turns on skid
steering rovers
- lots of improvements to the EKF support for Rover, thanks to Paul
Riseborough and testing from Tom Coyle. Using the EKF can greatly
improve navigation accuracy for fast rovers. Enable with
AHRS_EKF_USE=1.
- improved support for dual GPS on Pixhawk. Using a 2nd GPS can
greatly improve performance when in an area with an obstructed
view of the sky
- support for up to 14 RC channels on Pihxawk
- added BRAKING_PERCENT and BRAKING_SPEEDERR parameters for better
breaking support when cornering
- added support for FrSky telemetry via SERIAL2_PROTOCOL parameter
(thanks to Matthias Badaire)
- added support for Linux based autopilots, initially with the PXF
BeagleBoneBlack cape and the Erle robotics board. Support for more
boards is expected in future releases. Thanks to Victor, Sid and
Anuj for their great work on the Linux port.
- added StorageManager library, which expands available FRAM storage
on Pixhawk to 16 kByte. This allows for 724 waypoints, 50 rally
points and 84 fence points on Pixhawk.
- improved reporting of magnetometer and barometer errors to the GCS
- fixed a bug in automatic flow control detection for serial ports
in Pixhawk
- fixed use of FMU servo pins as digital inputs on Pixhawk
- imported latest updates for VRBrain boards (thanks to Emile
Castelnuovo and Luca Micheletti)
- updates to the Piksi GPS support (thanks to Niels Joubert)
- improved gyro estimate in DCM (thanks to Jon Challinger)
- improved position projection in DCM in wind (thanks to Przemek
Lekston)
- several updates to AP_NavEKF for more robust handling of errors
(thanks to Paul Riseborough)
- lots of small code cleanups thanks to Daniel Frenzel
- initial support for NavIO board from Mikhail Avkhimenia
- fixed logging of RCOU for up to 12 channels (thanks to Emile
Castelnuovo)
- code cleanups from Silvia Nunezrivero
- improved parameter download speed on radio links with no flow
control
Many thanks to everyone who contributed to this release, especially
Tom Coyle and Linus Penzlien for their excellent testing and feedback.
Happy driving!
APMrover2/sensors.pde
View file @ 62e7778b
...@@ -9,13 +9,7 @@ static void init_barometer(void) ...@@ -9,13 +9,7 @@ static void init_barometer(void)
static void init_sonar(void) static void init_sonar(void)
{ {
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 sonar.init();
sonar.Init(&adc);
sonar2.Init(&adc);
#else
sonar.Init(NULL);
sonar2.Init(NULL);
#endif
} }
// read_battery - reads battery voltage and current and invokes failsafe // read_battery - reads battery voltage and current and invokes failsafe
...@@ -37,15 +31,17 @@ void read_receiver_rssi(void) ...@@ -37,15 +31,17 @@ void read_receiver_rssi(void)
// read the sonars // read the sonars
static void read_sonars(void) static void read_sonars(void)
{ {
if (!sonar.enabled()) { sonar.update();
if (!sonar.healthy()) {
// this makes it possible to disable sonar at runtime // this makes it possible to disable sonar at runtime
return; return;
} }
if (sonar2.enabled()) { if (sonar.healthy(1)) {
// we have two sonars // we have two sonars
obstacle.sonar1_distance_cm = sonar.distance_cm(); obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = sonar2.distance_cm(); obstacle.sonar2_distance_cm = sonar.distance_cm(1);
if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm && if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm &&
obstacle.sonar2_distance_cm <= (uint16_t)obstacle.sonar2_distance_cm) { obstacle.sonar2_distance_cm <= (uint16_t)obstacle.sonar2_distance_cm) {
// we have an object on the left // we have an object on the left
...@@ -72,7 +68,7 @@ static void read_sonars(void) ...@@ -72,7 +68,7 @@ static void read_sonars(void)
} }
} else { } else {
// we have a single sonar // we have a single sonar
obstacle.sonar1_distance_cm = sonar.distance_cm(); obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = 0; obstacle.sonar2_distance_cm = 0;
if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm) { if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm) {
// obstacle detected in front // obstacle detected in front
......
APMrover2/setup.pde
View file @ 62e7778b
...@@ -341,9 +341,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) ...@@ -341,9 +341,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
mode != AUTO && mode != AUTO &&
mode != RTL) mode != RTL)
{ {
if (mode < MANUAL) if (mode > RTL)
mode = RTL;
else if (mode > RTL)
mode = MANUAL; mode = MANUAL;
else else
mode += radioInputSwitch; mode += radioInputSwitch;
...@@ -445,7 +443,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv) ...@@ -445,7 +443,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
g.compass_enabled = false; g.compass_enabled = false;
} else if (!strcmp_P(argv[1].str, PSTR("reset"))) { } else if (!strcmp_P(argv[1].str, PSTR("reset"))) {
compass.set_offsets(0,0,0); compass.set_and_save_offsets(0,0,0,0);
} else { } else {
cliSerial->printf_P(PSTR("\nOptions:[on,off,reset]\n")); cliSerial->printf_P(PSTR("\nOptions:[on,off,reset]\n"));
...@@ -467,9 +465,13 @@ static void report_batt_monitor() ...@@ -467,9 +465,13 @@ static void report_batt_monitor()
//print_blanks(2); //print_blanks(2);
cliSerial->printf_P(PSTR("Batt Mointor\n")); cliSerial->printf_P(PSTR("Batt Mointor\n"));
print_divider(); print_divider();
if(battery.monitoring() == AP_BATT_MONITOR_DISABLED) cliSerial->printf_P(PSTR("Batt monitoring disabled")); if (battery.num_instances() == 0) {
if(battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("Monitoring batt volts")); cliSerial->printf_P(PSTR("Batt monitoring disabled"));
if(battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("Monitoring volts and current")); } else if (!battery.has_current()) {
cliSerial->printf_P(PSTR("Monitoring batt volts"));
} else {
cliSerial->printf_P(PSTR("Monitoring volts and current"));
}
print_blanks(2); print_blanks(2);
} }
static void report_radio() static void report_radio()
...@@ -652,11 +654,8 @@ radio_input_switch(void) ...@@ -652,11 +654,8 @@ radio_input_switch(void)
static void zero_eeprom(void) static void zero_eeprom(void)
{ {
uint8_t b = 0;
cliSerial->printf_P(PSTR("\nErasing EEPROM\n")); cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
for (uint16_t i = 0; i < HAL_STORAGE_SIZE_AVAILABLE; i++) { StorageManager::erase();
hal.storage->write_byte(i, b);
}
cliSerial->printf_P(PSTR("done\n")); cliSerial->printf_P(PSTR("done\n"));
} }
......
APMrover2/system.pde
View file @ 62e7778b
...@@ -117,7 +117,7 @@ static void init_ardupilot() ...@@ -117,7 +117,7 @@ static void init_ardupilot()
set_control_channels(); set_control_channels();
// after parameter load setup correct baud rate on uartA // after parameter load setup correct baud rate on uartA
hal.uartA->begin(map_baudrate(g.serial0_baud, SERIAL0_BAUD)); hal.uartA->begin(map_baudrate(g.serial0_baud));
// keep a record of how many resets have happened. This can be // keep a record of how many resets have happened. This can be
// used to detect in-flight resets // used to detect in-flight resets
...@@ -135,14 +135,14 @@ static void init_ardupilot() ...@@ -135,14 +135,14 @@ static void init_ardupilot()
check_usb_mux(); check_usb_mux();
// we have a 2nd serial port for telemetry // we have a 2nd serial port for telemetry
hal.uartC->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD), 128, 128); gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, 128);
gcs[1].init(hal.uartC);
#if MAVLINK_COMM_NUM_BUFFERS > 2 #if MAVLINK_COMM_NUM_BUFFERS > 2
// we may have a 3rd serial port for telemetry if (g.serial2_protocol == SERIAL2_FRSKY_DPORT ||
if (hal.uartD != NULL) { g.serial2_protocol == SERIAL2_FRSKY_SPORT) {
hal.uartD->begin(map_baudrate(g.serial2_baud, SERIAL2_BAUD), 128, 128); frsky_telemetry.init(hal.uartD, g.serial2_protocol);
gcs[2].init(hal.uartD); } else {
gcs[2].setup_uart(hal.uartD, map_baudrate(g.serial2_baud), 128, 128);
} }
#endif #endif
...@@ -167,7 +167,7 @@ static void init_ardupilot() ...@@ -167,7 +167,7 @@ static void init_ardupilot()
hal.scheduler->register_delay_callback(mavlink_delay_cb, 5); hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
adc.Init(); // APM ADC library initialization apm1_adc.Init(); // APM ADC library initialization
#endif #endif
if (g.compass_enabled==true) { if (g.compass_enabled==true) {
...@@ -189,10 +189,6 @@ static void init_ardupilot() ...@@ -189,10 +189,6 @@ static void init_ardupilot()
// Do GPS init // Do GPS init
gps.init(&DataFlash); gps.init(&DataFlash);
//mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav
mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id
mavlink_system.type = MAV_TYPE_GROUND_ROVER;
rc_override_active = hal.rcin->set_overrides(rc_override, 8); rc_override_active = hal.rcin->set_overrides(rc_override, 8);
init_rc_in(); // sets up rc channels from radio init_rc_in(); // sets up rc channels from radio
...@@ -262,6 +258,7 @@ static void startup_ground(void) ...@@ -262,6 +258,7 @@ static void startup_ground(void)
mission.init(); mission.init();
hal.uartA->set_blocking_writes(false); hal.uartA->set_blocking_writes(false);
hal.uartB->set_blocking_writes(false);
hal.uartC->set_blocking_writes(false); hal.uartC->set_blocking_writes(false);
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive.")); gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive."));
...@@ -324,6 +321,24 @@ static void set_mode(enum mode mode) ...@@ -324,6 +321,24 @@ static void set_mode(enum mode mode)
} }
} }
/*
set_mode() wrapper for MAVLink SET_MODE
*/
static bool mavlink_set_mode(uint8_t mode)
{
switch (mode) {
case MANUAL:
case HOLD:
case LEARNING:
case STEERING:
case AUTO:
case RTL:
set_mode((enum mode)mode);
return true;
}
return false;
}
/* /*
called to set/unset a failsafe event. called to set/unset a failsafe event.
*/ */
...@@ -413,27 +428,6 @@ static void resetPerfData(void) { ...@@ -413,27 +428,6 @@ static void resetPerfData(void) {
} }
/*
map from a 8 bit EEPROM baud rate to a real baud rate
*/
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
{
switch (rate) {
case 1: return 1200;
case 2: return 2400;
case 4: return 4800;
case 9: return 9600;
case 19: return 19200;
case 38: return 38400;
case 57: return 57600;
case 111: return 111100;
case 115: return 115200;
}
cliSerial->println_P(PSTR("Invalid baudrate"));
return default_baud;
}
static void check_usb_mux(void) static void check_usb_mux(void)
{ {
bool usb_check = hal.gpio->usb_connected(); bool usb_check = hal.gpio->usb_connected();
...@@ -452,7 +446,7 @@ static void check_usb_mux(void) ...@@ -452,7 +446,7 @@ static void check_usb_mux(void)
if (usb_connected) { if (usb_connected) {
hal.uartA->begin(SERIAL0_BAUD); hal.uartA->begin(SERIAL0_BAUD);
} else { } else {
hal.uartA->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD)); hal.uartA->begin(map_baudrate(g.serial1_baud));
} }
#endif #endif
} }
...@@ -496,7 +490,7 @@ static uint8_t check_digital_pin(uint8_t pin) ...@@ -496,7 +490,7 @@ static uint8_t check_digital_pin(uint8_t pin)
return 0; return 0;
} }
// ensure we are in input mode // ensure we are in input mode
hal.gpio->pinMode(dpin, GPIO_INPUT); hal.gpio->pinMode(dpin, HAL_GPIO_INPUT);
// enable pullup // enable pullup
hal.gpio->write(dpin, 1); hal.gpio->write(dpin, 1);
...@@ -537,3 +531,14 @@ static bool should_log(uint32_t mask) ...@@ -537,3 +531,14 @@ static bool should_log(uint32_t mask)
} }
return ret; return ret;
} }
/*
send FrSky telemetry. Should be called at 5Hz by scheduler
*/
static void telemetry_send(void)
{
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.send_frames((uint8_t)control_mode,
(AP_Frsky_Telem::FrSkyProtocol)g.serial2_protocol.get());
#endif
}
APMrover2/test.pde
View file @ 62e7778b
...@@ -349,7 +349,7 @@ test_ins(uint8_t argc, const Menu::arg *argv) ...@@ -349,7 +349,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
uint8_t medium_loopCounter = 0; uint8_t medium_loopCounter = 0;
while(1){ while(1){
ins.wait_for_sample(1000); ins.wait_for_sample();
ahrs.update(); ahrs.update();
...@@ -371,9 +371,9 @@ test_ins(uint8_t argc, const Menu::arg *argv) ...@@ -371,9 +371,9 @@ test_ins(uint8_t argc, const Menu::arg *argv)
(uint16_t)ahrs.yaw_sensor / 100, (uint16_t)ahrs.yaw_sensor / 100,
gyros.x, gyros.y, gyros.z, gyros.x, gyros.y, gyros.z,
accels.x, accels.y, accels.z); accels.x, accels.y, accels.z);
} if(cliSerial->available() > 0){
if(cliSerial->available() > 0){ return (0);
return (0); }
} }
} }
...@@ -409,7 +409,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) ...@@ -409,7 +409,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
uint8_t medium_loopCounter = 0; uint8_t medium_loopCounter = 0;
while(1) { while(1) {
ins.wait_for_sample(1000); ins.wait_for_sample();
ahrs.update(); ahrs.update();
medium_loopCounter++; medium_loopCounter++;
...@@ -455,12 +455,15 @@ test_mag(uint8_t argc, const Menu::arg *argv) ...@@ -455,12 +455,15 @@ test_mag(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_sonar(uint8_t argc, const Menu::arg *argv) test_sonar(uint8_t argc, const Menu::arg *argv)
{ {
if (!sonar.enabled()) { init_sonar();
delay(20);
sonar.update();
if (!sonar.healthy()) {
cliSerial->println_P(PSTR("WARNING: Sonar is not enabled")); cliSerial->println_P(PSTR("WARNING: Sonar is not enabled"));
} }
print_hit_enter(); print_hit_enter();
init_sonar();
float sonar_dist_cm_min = 0.0f; float sonar_dist_cm_min = 0.0f;
float sonar_dist_cm_max = 0.0f; float sonar_dist_cm_max = 0.0f;
...@@ -472,10 +475,11 @@ test_sonar(uint8_t argc, const Menu::arg *argv) ...@@ -472,10 +475,11 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
while (true) { while (true) {
delay(20); delay(20);
sonar.update();
uint32_t now = millis(); uint32_t now = millis();
float dist_cm = sonar.distance_cm(); float dist_cm = sonar.distance_cm(0);
float voltage = sonar.voltage(); float voltage = sonar.voltage_mv(0);
if (sonar_dist_cm_min == 0.0f) { if (sonar_dist_cm_min == 0.0f) {
sonar_dist_cm_min = dist_cm; sonar_dist_cm_min = dist_cm;
voltage_min = voltage; voltage_min = voltage;
...@@ -485,8 +489,8 @@ test_sonar(uint8_t argc, const Menu::arg *argv) ...@@ -485,8 +489,8 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
voltage_min = min(voltage_min, voltage); voltage_min = min(voltage_min, voltage);
voltage_max = max(voltage_max, voltage); voltage_max = max(voltage_max, voltage);
dist_cm = sonar2.distance_cm(); dist_cm = sonar.distance_cm(1);
voltage = sonar2.voltage(); voltage = sonar.voltage_mv(1);
if (sonar2_dist_cm_min == 0.0f) { if (sonar2_dist_cm_min == 0.0f) {
sonar2_dist_cm_min = dist_cm; sonar2_dist_cm_min = dist_cm;
voltage2_min = voltage; voltage2_min = voltage;
......