From b284d4c21e730d83e757ac7cfd9bcb96f72e659f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 18 Dec 2012 22:44:12 +1100 Subject: [PATCH] Rover: first cut at porting rover to AP_HAL --- APMrover2/APM_Config.h | 2 - APMrover2/APMrover2.pde | 181 ++++++++++++++-------------- APMrover2/Attitude.pde | 34 +++--- APMrover2/GCS.h | 24 ++-- APMrover2/GCS_Mavlink.pde | 209 +++++++++++++++++---------------- APMrover2/Log.pde | 30 ++--- APMrover2/Makefile | 6 - APMrover2/Parameters.h | 9 +- APMrover2/Parameters.pde | 24 +++- APMrover2/commands.pde | 30 ++--- APMrover2/commands_logic.pde | 3 +- APMrover2/commands_process.pde | 2 +- APMrover2/compat.h | 15 +++ APMrover2/compat.pde | 37 ++++++ APMrover2/config.h | 30 +++-- APMrover2/control_modes.pde | 8 +- APMrover2/defines.h | 3 - APMrover2/events.pde | 6 +- APMrover2/failsafe.pde | 11 +- APMrover2/planner.pde | 53 --------- APMrover2/radio.pde | 68 ++++++----- APMrover2/sensors.pde | 18 +-- APMrover2/setup.pde | 52 ++------ APMrover2/system.pde | 79 ++++--------- APMrover2/test.pde | 88 ++------------ 25 files changed, 457 insertions(+), 565 deletions(-) create mode 100644 APMrover2/compat.h create mode 100644 APMrover2/compat.pde delete mode 100644 APMrover2/planner.pde diff --git a/APMrover2/APM_Config.h b/APMrover2/APM_Config.h index 93d61b4b20..923a198912 100644 --- a/APMrover2/APM_Config.h +++ b/APMrover2/APM_Config.h @@ -3,8 +3,6 @@ // This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from // their default values, place the appropriate #define statements here. -//#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2 - #define LITE DISABLED // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329 // if LITE is DISABLED, this is for a full APM v1 (Oilpan + GPS MT3329 + Magnetometer HMC5883L) or APM v2 diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 9bd5e3c25f..7258374261 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -69,28 +69,19 @@ version 2.1 of the License, or (at your option) any later version. // Header includes //////////////////////////////////////////////////////////////////////////////// -// AVR runtime -#include -#include -#include #include +#include +#include // Libraries -#include #include #include +#include #include #include -#include -#include // ArduPilot Mega RC Library #include // ArduPilot GPS library -#include // Wayne Truchsess I2C lib -#include // Arduino SPI lib -#include // for removing conflict between optical flow and dataflash on SPI3 bus -#include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Analog to Digital Converter Library -#include // ArduPilot Mega polymorphic analog getter -#include // ArduPilot Mega TimerProcess +#include #include #include // ArduPilot Mega Magnetometer Library #include // ArduPilot Mega Vector/Matrix math Library @@ -108,6 +99,14 @@ version 2.1 of the License, or (at your option) any later version. #include // MAVLink GCS definitions #include // needed for AHRS build #include +#include +#include +#include + +#include +#include +#include +#include "compat.h" // Configuration #include "config.h" @@ -119,24 +118,9 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega Declination Helper Library -//////////////////////////////////////////////////////////////////////////////// -// Serial ports -//////////////////////////////////////////////////////////////////////////////// -// -// Note that FastSerial port buffers are allocated at ::begin time, -// so there is not much of a penalty to defining ports that we don't -// use. -// -FastSerialPort0(Serial); // FTDI/console -FastSerialPort1(Serial1); // GPS port -#if TELEMETRY_UART2 == ENABLED - // solder bridge set to enable UART2 instead of USB MUX - FastSerialPort2(Serial3); -#else - FastSerialPort3(Serial3); // Telemetry port for APM1 -#endif +AP_HAL::BetterStream* cliSerial; -static FastSerial *cliSerial = &Serial; +const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; // this sets up the parameter table, and sets the default values. This // must be the first AP_Param variable declared to ensure its @@ -144,31 +128,6 @@ static FastSerial *cliSerial = &Serial; // variables AP_Param param_loader(var_info, WP_START_BYTE); -//////////////////////////////////////////////////////////////////////////////// -// ISR Registry -//////////////////////////////////////////////////////////////////////////////// -Arduino_Mega_ISR_Registry isr_registry; - - -//////////////////////////////////////////////////////////////////////////////// -// APM_RC_Class Instance -//////////////////////////////////////////////////////////////////////////////// -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 - APM_RC_APM2 APM_RC; -#else - APM_RC_APM1 APM_RC; -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Dataflash -//////////////////////////////////////////////////////////////////////////////// -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 -AP_Semaphore spi3_semaphore; -DataFlash_APM2 DataFlash(&spi3_semaphore); -#else -DataFlash_APM1 DataFlash; -#endif - //////////////////////////////////////////////////////////////////////////////// // the rate we run the main loop at //////////////////////////////////////////////////////////////////////////////// @@ -187,6 +146,17 @@ static Parameters g; // prototypes static void update_events(void); +//////////////////////////////////////////////////////////////////////////////// +// DataFlash +//////////////////////////////////////////////////////////////////////////////// +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 +DataFlash_APM1 DataFlash; +#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 +DataFlash_APM2 DataFlash; +#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL +DataFlash_SITL DataFlash; +#endif + //////////////////////////////////////////////////////////////////////////////// // Sensors @@ -214,36 +184,34 @@ static AP_Int8 *flight_modes = &g.flight_mode1; static AP_ADC_ADS7844 adc; #endif -#ifdef DESKTOP_BUILD -AP_Baro_BMP085_HIL barometer; -AP_Compass_HIL compass; -#include -SITL sitl; +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL +static AP_Compass_HIL compass; +static SITL sitl; #else static AP_Compass_HMC5843 compass; #endif // real GPS selection #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO -AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); +AP_GPS_Auto g_gps_driver(&g_gps); #elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA -AP_GPS_NMEA g_gps_driver(&Serial1); +AP_GPS_NMEA g_gps_driver(); #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF -AP_GPS_SIRF g_gps_driver(&Serial1); +AP_GPS_SIRF g_gps_driver(); #elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX -AP_GPS_UBLOX g_gps_driver(&Serial1); +AP_GPS_UBLOX g_gps_driver(); #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK -AP_GPS_MTK g_gps_driver(&Serial1); +AP_GPS_MTK g_gps_driver(); #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 -AP_GPS_MTK16 g_gps_driver(&Serial1); +AP_GPS_MTK16 g_gps_driver(); #elif GPS_PROTOCOL == GPS_PROTOCOL_NONE -AP_GPS_None g_gps_driver(NULL); +AP_GPS_None g_gps_driver(); #else #error Unrecognised GPS_PROTOCOL setting. @@ -251,7 +219,9 @@ AP_GPS_None g_gps_driver(NULL); # if CONFIG_INS_TYPE == CONFIG_INS_MPU6000 AP_InertialSensor_MPU6000 ins; -# else +# elif CONFIG_INS_TYPE == CONFIG_INS_SITL + AP_InertialSensor_Stub ins; +#else AP_InertialSensor_Oilpan ins( &adc ); #endif // CONFIG_INS_TYPE @@ -274,9 +244,6 @@ AP_Compass_HIL compass; // never used #error Unrecognised HIL_MODE setting. #endif // HIL MODE -// we always have a timer scheduler -AP_TimerProcess timer_scheduler; - //////////////////////////////////////////////////////////////////////////////// // GCS selection //////////////////////////////////////////////////////////////////////////////// @@ -284,22 +251,23 @@ AP_TimerProcess timer_scheduler; GCS_MAVLINK gcs0; GCS_MAVLINK gcs3; +// a pin for reading the receiver RSSI voltage. The scaling by 0.25 +// is to take the 0 to 1024 range down to an 8 bit range for MAVLink +AP_HAL::AnalogSource *rssi_analog_source; + +AP_HAL::AnalogSource *vcc_pin; + +AP_HAL::AnalogSource * batt_volt_pin; +AP_HAL::AnalogSource * batt_curr_pin; + //////////////////////////////////////////////////////////////////////////////// // SONAR selection //////////////////////////////////////////////////////////////////////////////// // ModeFilterInt16_Size5 sonar_mode_filter(2); #if CONFIG_SONAR == ENABLED -/* - #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC - AP_AnalogSource_ADC sonar_analog_source( &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25); - #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN - AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN); - #endif - AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter); -*/ - AP_AnalogSource_Arduino sonar_analog_source(A0); // use AN0 analog pin for APM2 on left - AP_RangeFinder_SharpGP2Y sonar(&sonar_analog_source, &sonar_mode_filter); + AP_HAL::AnalogSource *sonar_analog_source; + AP_RangeFinder_MaxsonarXL *sonar; #endif // relay support @@ -341,10 +309,10 @@ static const char *comma = ","; //////////////////////////////////////////////////////////////////////////////// // This is the state of the flight control system // There are multiple states defined such as MANUAL, FBW-A, AUTO -byte control_mode = INITIALISING; +uint8_t control_mode = INITIALISING; // Used to maintain the state of the previous control switch position // This is set to -1 when we need to re-read the switch -byte oldSwitchPosition; +uint8_t oldSwitchPosition; // These are values received from the GCS if the user is using GCS joystick // control and are substituted for the values coming from the RC radio static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; @@ -383,7 +351,7 @@ static const float t7 = 10000000.0; // A counter used to count down valid gps fixes to allow the gps estimate to settle // before recording our home position (and executing a ground start if we booted with an air start) -static byte ground_start_count = 5; +static uint8_t ground_start_count = 5; // Used to compute a speed estimate from the first valid gps fixes to decide if we are // on the ground or in the air. Used to decide if a ground start is appropriate if we // booted with an air start. @@ -416,12 +384,12 @@ static bool rtl_complete = false; // There may be two active commands in Auto mode. // This indicates the active navigation command by index number -static byte nav_command_index; +static uint8_t nav_command_index; // This indicates the active non-navigation command by index number -static byte non_nav_command_index; +static uint8_t non_nav_command_index; // This is the command type (eg navigate to waypoint) of the active navigation command -static byte nav_command_ID = NO_COMMAND; -static byte non_nav_command_ID = NO_COMMAND; +static uint8_t nav_command_ID = NO_COMMAND; +static uint8_t non_nav_command_ID = NO_COMMAND; static float groundspeed_error; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel @@ -455,7 +423,7 @@ static float crosstrack_error; // Used to track the CH7 toggle state. // When CH7 goes LOW PWM from HIGH PWM, this value will have been set true // This allows advanced functionality to know when to execute -static boolean trim_flag; +static bool trim_flag; // This register tracks the current Mission Command index when writing // a mission using CH7 in flight static int8_t CH7_wp_index; @@ -500,7 +468,7 @@ static int32_t wp_totalDistance; // repeating event control //////////////////////////////////////////////////////////////////////////////// // Flag indicating current event type -static byte event_id; +static uint8_t event_id; // when the event was started in ms static int32_t event_timer; // how long to delay the next firing of event in millis @@ -582,16 +550,16 @@ static uint16_t mainLoop_count; // Time in miliseconds of start of medium control loop. Milliseconds static uint32_t medium_loopTimer; // Counters for branching from main control loop to slower loops -static byte medium_loopCounter; +static uint8_t medium_loopCounter; // Number of milliseconds used in last medium loop cycle static uint8_t delta_ms_medium_loop; // Counters for branching from medium control loop to slower loops -static byte slow_loopCounter; +static uint8_t slow_loopCounter; // Counter to trigger execution of very low rate processes -static byte superslow_loopCounter; +static uint8_t superslow_loopCounter; // Counter to trigger execution of 1 Hz processes -static byte counter_one_herz; +static uint8_t counter_one_herz; // % MCU cycles used static float load; @@ -602,6 +570,27 @@ static float load; void setup() { memcheck_init(); + cliSerial = hal.console; + + rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE, 0.25); + vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC); + batt_volt_pin = hal.analogin->channel(g.battery_volt_pin); + batt_curr_pin = hal.analogin->channel(g.battery_curr_pin); + +#if CONFIG_SONAR == ENABLED + #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC + sonar_analog_source = new AP_ADC_AnalogSource( + &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25); + #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN + sonar_analog_source = hal.analogin->channel( + CONFIG_SONAR_SOURCE_ANALOG_PIN); + #else + #warning "Invalid CONFIG_SONAR_SOURCE" + #endif + sonar = new AP_RangeFinder_MaxsonarXL(sonar_analog_source, + &sonar_mode_filter); +#endif + init_ardupilot(); } @@ -688,7 +677,7 @@ static void fast_loop() // ---------- #if CONFIG_SONAR == ENABLED if(g.sonar_enabled){ - sonar_dist = sonar.read(); + sonar_dist = sonar->read(); if(sonar_dist <= g.sonar_trigger) { // obstacle detected in front obstacle = true; @@ -972,3 +961,5 @@ static void update_navigation() break; } } + +AP_HAL_MAIN(); diff --git a/APMrover2/Attitude.pde b/APMrover2/Attitude.pde index 6185fc8e7a..34f905325d 100644 --- a/APMrover2/Attitude.pde +++ b/APMrover2/Attitude.pde @@ -120,10 +120,10 @@ static void set_servos(void) #if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS // send values to the PWM timers for output // ---------------------------------------- - APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos - APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos - APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos - APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos + hal.rcout->write(CH_1, g.channel_roll.radio_out); // send to Servos + hal.rcout->write(CH_2, g.channel_pitch.radio_out); // send to Servos + hal.rcout->write(CH_3, g.channel_throttle.radio_out); // send to Servos + hal.rcout->write(CH_4, g.channel_rudder.radio_out); // send to Servos // Route configurable aux. functions to their respective servos g.rc_5.output_ch(CH_5); @@ -134,18 +134,22 @@ static void set_servos(void) #endif } -static void demo_servos(byte i) { +static bool demoing_servos; - while(i > 0){ - gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); +static void demo_servos(uint8_t i) { + + while(i > 0) { + gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); + demoing_servos = true; #if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS - APM_RC.OutputCh(1, 1400); - mavlink_delay(400); - APM_RC.OutputCh(1, 1600); - mavlink_delay(200); - APM_RC.OutputCh(1, 1500); + hal.rcout->write(1, 1400); + mavlink_delay(400); + hal.rcout->write(1, 1600); + mavlink_delay(200); + hal.rcout->write(1, 1500); #endif - mavlink_delay(400); - i--; - } + demoing_servos = false; + mavlink_delay(400); + i--; + } } diff --git a/APMrover2/GCS.h b/APMrover2/GCS.h index 5db92c425f..1a741ac701 100644 --- a/APMrover2/GCS.h +++ b/APMrover2/GCS.h @@ -6,10 +6,8 @@ #ifndef __GCS_H #define __GCS_H -#include #include #include -#include #include /// @@ -39,7 +37,7 @@ public: /// /// @param port The stream over which messages are exchanged. /// - void init(FastSerial *port) { + void init(AP_HAL::UARTDriver *port) { _port = port; initialised = true; last_gps_satellites = 255; @@ -61,19 +59,12 @@ public: /// void send_message(enum ap_message id) {} - /// Send a text message. - /// - /// @param severity A value describing the importance of the message. - /// @param str The text to be sent. - /// - void send_text(gcs_severity severity, const char *str) {} - /// Send a text message with a PSTR() /// /// @param severity A value describing the importance of the message. /// @param str The text to be sent. /// - void send_text(gcs_severity severity, const prog_char_t *str) {} + void send_text_P(gcs_severity severity, const prog_char_t *str) {} // send streams which match frequency range void data_stream_send(void); @@ -86,7 +77,7 @@ public: protected: /// The stream we are communicating over - FastSerial *_port; + AP_HAL::UARTDriver * _port; }; // @@ -105,10 +96,9 @@ class GCS_MAVLINK : public GCS_Class public: GCS_MAVLINK(); void update(void); - void init(FastSerial *port); + void init(AP_HAL::UARTDriver *port); void send_message(enum ap_message id); - void send_text(gcs_severity severity, const char *str); - void send_text(gcs_severity severity, const prog_char_t *str); + void send_text_P(gcs_severity severity, const prog_char_t *str); void data_stream_send(void); void queued_param_send(); void queued_waypoint_send(); @@ -132,6 +122,10 @@ public: // see if we should send a stream now. Called at 50Hz bool stream_trigger(enum streams stream_num); + // this costs us 51 bytes per instance, but means that low priority + // messages don't block the CPU + mavlink_statustext_t pending_status; + private: void handleMessage(mavlink_message_t * msg); diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 9d0067cb67..cc022a0809 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -3,10 +3,6 @@ // use this to prevent recursion during sensor init static bool in_mavlink_delay; -// this costs us 51 bytes, but means that low priority -// messages don't block the CPU -static mavlink_statustext_t pending_status; - // true when we have received at least 1 MAVLink packet static bool mavlink_active; @@ -192,8 +188,10 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack static void NOINLINE send_meminfo(mavlink_channel_t chan) { +#if CONFIG_HAL_BOARD != HAL_BOARD_AVR_SITL extern unsigned __brkval; mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); +#endif } static void NOINLINE send_location(mavlink_channel_t chan) @@ -308,18 +306,34 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan) static void NOINLINE send_radio_out(mavlink_channel_t chan) { - mavlink_msg_servo_output_raw_send( - chan, - micros(), - 0, // port - APM_RC.OutputCh_current(0), - APM_RC.OutputCh_current(1), - APM_RC.OutputCh_current(2), - APM_RC.OutputCh_current(3), - APM_RC.OutputCh_current(4), - APM_RC.OutputCh_current(5), - APM_RC.OutputCh_current(6), - APM_RC.OutputCh_current(7)); +#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS + mavlink_msg_servo_output_raw_send( + chan, + micros(), + 0, // port + hal.rcout->read(0), + hal.rcout->read(1), + hal.rcout->read(2), + hal.rcout->read(3), + hal.rcout->read(4), + hal.rcout->read(5), + hal.rcout->read(6), + hal.rcout->read(7)); +#else + extern RC_Channel* rc_ch[8]; + mavlink_msg_servo_output_raw_send( + chan, + micros(), + 0, // port + rc_ch[0]->radio_out, + rc_ch[1]->radio_out, + rc_ch[2]->radio_out, + rc_ch[3]->radio_out, + rc_ch[4]->radio_out, + rc_ch[5]->radio_out, + rc_ch[6]->radio_out, + rc_ch[7]->radio_out); +#endif } static void NOINLINE send_vfr_hud(mavlink_channel_t chan) @@ -390,7 +404,7 @@ static void NOINLINE send_ahrs(mavlink_channel_t chan) #endif // HIL_MODE != HIL_MODE_ATTITUDE -#ifdef DESKTOP_BUILD +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL // report simulator state static void NOINLINE send_simstate(mavlink_channel_t chan) { @@ -398,15 +412,13 @@ static void NOINLINE send_simstate(mavlink_channel_t chan) } #endif -#ifndef DESKTOP_BUILD static void NOINLINE send_hwstatus(mavlink_channel_t chan) { mavlink_msg_hwstatus_send( chan, board_voltage(), - I2c.lockup_count()); + hal.i2c->lockup_count()); } -#endif static void NOINLINE send_gps_status(mavlink_channel_t chan) { @@ -429,10 +441,11 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan) static void NOINLINE send_statustext(mavlink_channel_t chan) { + mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status); mavlink_msg_statustext_send( chan, - pending_status.severity, - pending_status.text); + s->severity, + s->text); } // are we still delaying telemetry to try to avoid Xbee bricking? @@ -581,17 +594,15 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, break; case MSG_SIMSTATE: -#ifdef DESKTOP_BUILD +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL CHECK_PAYLOAD_SIZE(SIMSTATE); send_simstate(chan); #endif break; case MSG_HWSTATUS: -#ifndef DESKTOP_BUILD CHECK_PAYLOAD_SIZE(HWSTATUS); send_hwstatus(chan); -#endif break; case MSG_RETRY_DEFERRED: @@ -668,8 +679,9 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char if (severity == SEVERITY_LOW) { // send via the deferred queuing system - pending_status.severity = (uint8_t)severity; - strncpy((char *)pending_status.text, str, sizeof(pending_status.text)); + mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status); + s->severity = (uint8_t)severity; + strncpy((char *)s->text, str, sizeof(s->text)); mavlink_send_message(chan, MSG_STATUSTEXT, 0); } else { // send immediately @@ -699,17 +711,17 @@ GCS_MAVLINK::GCS_MAVLINK() : } void -GCS_MAVLINK::init(FastSerial * port) +GCS_MAVLINK::init(AP_HAL::UARTDriver *port) { GCS_Class::init(port); - if (port == &Serial) { + if (port == (AP_HAL::BetterStream*)hal.uartA) { mavlink_comm_0_port = port; chan = MAVLINK_COMM_0; }else{ mavlink_comm_1_port = port; chan = MAVLINK_COMM_1; } - _queued_parameter = NULL; + _queued_parameter = NULL; } void @@ -881,13 +893,7 @@ GCS_MAVLINK::send_message(enum ap_message id) } void -GCS_MAVLINK::send_text(gcs_severity severity, const char *str) -{ - mavlink_send_text(chan,severity,str); -} - -void -GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str) +GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str) { mavlink_statustext_t m; uint8_t i; @@ -987,7 +993,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) uint8_t result; // do command - send_text(SEVERITY_LOW,PSTR("command received: ")); + send_text_P(SEVERITY_LOW,PSTR("command received: ")); switch(packet.command) { @@ -1274,7 +1280,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.start_index > g.command_total || packet.end_index > g.command_total || packet.end_index < packet.start_index) { - send_text(SEVERITY_LOW,PSTR("flight plan update rejected")); + send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected")); break; } @@ -1457,7 +1463,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) msg->compid, result); - send_text(SEVERITY_LOW,PSTR("flight plan received")); + send_text_P(SEVERITY_LOW,PSTR("flight plan received")); waypoint_receiving = false; // XXX ignores waypoint radius for individual waypoints, can // only set WP_RADIUS parameter @@ -1545,30 +1551,32 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } // end case case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: - { - // allow override of RC channel values for HIL - // or for complete GCS control of switch position - // and RC PWM values. - if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs - mavlink_rc_channels_override_t packet; - int16_t v[8]; - mavlink_msg_rc_channels_override_decode(msg, &packet); + { + // allow override of RC channel values for HIL + // or for complete GCS control of switch position + // and RC PWM values. + if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs + mavlink_rc_channels_override_t packet; + int16_t v[8]; + mavlink_msg_rc_channels_override_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) - break; - - v[0] = packet.chan1_raw; - v[1] = packet.chan2_raw; - v[2] = packet.chan3_raw; - v[3] = packet.chan4_raw; - v[4] = packet.chan5_raw; - v[5] = packet.chan6_raw; - v[6] = packet.chan7_raw; - v[7] = packet.chan8_raw; - rc_override_active = APM_RC.setHIL(v); - rc_override_fs_timer = millis(); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; - } + + v[0] = packet.chan1_raw; + v[1] = packet.chan2_raw; + v[2] = packet.chan3_raw; + v[3] = packet.chan4_raw; + v[4] = packet.chan5_raw; + v[5] = packet.chan6_raw; + v[6] = packet.chan7_raw; + v[7] = packet.chan8_raw; + + hal.rcin->set_overrides(v, 8); + + rc_override_fs_timer = millis(); + break; + } case MAVLINK_MSG_ID_HEARTBEAT: { @@ -1811,42 +1819,36 @@ GCS_MAVLINK::queued_waypoint_send() } /* - a delay() callback that processes MAVLink packets. We set this as the - callback in long running library initialisation routines to allow - MAVLink to process packets while waiting for the initialisation to - complete -*/ -static void mavlink_delay(unsigned long t) + * a delay() callback that processes MAVLink packets. We set this as the + * callback in long running library initialisation routines to allow + * MAVLink to process packets while waiting for the initialisation to + * complete + */ +static void mavlink_delay_cb() { - uint32_t tstart; - static uint32_t last_1hz, last_50hz; - - if (in_mavlink_delay) { - // this should never happen, but let's not tempt fate by - // letting the stack grow too much - delay(t); - return; - } + static uint32_t last_1hz, last_50hz, last_5s; + if (!gcs0.initialised) return; in_mavlink_delay = true; - tstart = millis(); - do { - uint32_t tnow = millis(); - if (tnow - last_1hz > 1000) { - last_1hz = tnow; - gcs_send_message(MSG_HEARTBEAT); - gcs_send_message(MSG_EXTENDED_STATUS1); - } - if (tnow - last_50hz > 20) { - last_50hz = tnow; - gcs_update(); - } - delay(1); + uint32_t tnow = millis(); + if (tnow - last_1hz > 1000) { + last_1hz = tnow; + gcs_send_message(MSG_HEARTBEAT); + gcs_send_message(MSG_EXTENDED_STATUS1); + } + if (tnow - last_50hz > 20) { + last_50hz = tnow; + gcs_update(); + gcs_data_stream_send(); + } + if (tnow - last_5s > 5000) { + last_5s = tnow; + gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM...")); + } #if USB_MUX_PIN > 0 - check_usb_mux(); + check_usb_mux(); #endif - } while (millis() - tstart < t); in_mavlink_delay = false; } @@ -1886,31 +1888,32 @@ static void gcs_update(void) static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) { - gcs0.send_text(severity, str); + gcs0.send_text_P(severity, str); if (gcs3.initialised) { - gcs3.send_text(severity, str); + gcs3.send_text_P(severity, str); } } /* - send a low priority formatted message to the GCS - only one fits in the queue, so if you send more than one before the - last one gets into the serial buffer then the old one will be lost + * send a low priority formatted message to the GCS + * only one fits in the queue, so if you send more than one before the + * last one gets into the serial buffer then the old one will be lost */ -static void gcs_send_text_fmt(const prog_char_t *fmt, ...) +void gcs_send_text_fmt(const prog_char_t *fmt, ...) { char fmtstr[40]; - va_list ap; + va_list arg_list; uint8_t i; for (i=0; iprintf_P(PSTR("\nErasing log...\n")); - DataFlash.EraseAll(erase_callback); + DataFlash.EraseAll(); cliSerial->printf_P(PSTR("\nLog erased.\n")); } @@ -257,7 +257,7 @@ static void Log_Write_Performance() // Write a command processing packet. Total length : 19 bytes //void Log_Write_Cmd(byte num, byte id, byte p1, int32_t alt, int32_t lat, int32_t lng) -static void Log_Write_Cmd(byte num, struct Location *wp) +static void Log_Write_Cmd(uint8_t num, struct Location *wp) { DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); @@ -271,7 +271,7 @@ static void Log_Write_Cmd(byte num, struct Location *wp) DataFlash.WriteByte(END_BYTE); } -static void Log_Write_Startup(byte type) +static void Log_Write_Startup(uint8_t type) { DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); @@ -330,7 +330,7 @@ static void Log_Write_Nav_Tuning() } // Write a mode packet. Total length : 5 bytes -static void Log_Write_Mode(byte mode) +static void Log_Write_Mode(uint8_t mode) { DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); @@ -341,7 +341,7 @@ static void Log_Write_Mode(byte mode) // Write an GPS packet. Total length : 30 bytes static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt, - int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats) + int32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix, uint8_t log_NumSats) { DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); @@ -468,7 +468,7 @@ static void Log_Read_Performance() // Read a command processing packet static void Log_Read_Cmd() { - byte logvarb; + uint8_t logvarb; int32_t logvarl; cliSerial->printf_P(PSTR("CMD: ")); @@ -489,7 +489,7 @@ static void Log_Read_Cmd() static void Log_Read_Startup() { - byte logbyte = DataFlash.ReadByte(); + uint8_t logbyte = DataFlash.ReadByte(); if (logbyte == TYPE_AIRSTART_MSG) cliSerial->printf_P(PSTR("AIR START - ")); @@ -524,7 +524,7 @@ static void Log_Read_Mode() static void Log_Read_GPS() { int32_t l[7]; - byte b[2]; + uint8_t b[2]; int16_t j,k,m; l[0] = DataFlash.ReadLong(); b[0] = DataFlash.ReadByte(); @@ -595,8 +595,8 @@ static void Log_Read(int16_t start_page, int16_t end_page) // Read the DataFlash log memory : Packet Parser static int Log_Read_Process(int16_t start_page, int16_t end_page) { - byte data; - byte log_step = 0; + uint8_t data; + uint8_t log_step = 0; int page = start_page; int packet_count = 0; @@ -679,13 +679,13 @@ static int Log_Read_Process(int16_t start_page, int16_t end_page) #else // LOGGING_ENABLED // dummy functions -static void Log_Write_Mode(byte mode) {} -static void Log_Write_Startup(byte type) {} -static void Log_Write_Cmd(byte num, struct Location *wp) {} +static void Log_Write_Mode(uint8_t mode) {} +static void Log_Write_Startup(uint8_t type) {} +static void Log_Write_Cmd(uint8_t num, struct Location *wp) {} static void Log_Write_Current() {} static void Log_Write_Nav_Tuning() {} static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt, - int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats) {} + int32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix, uint8_t log_NumSats) {} static void Log_Write_Performance() {} static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw) {} diff --git a/APMrover2/Makefile b/APMrover2/Makefile index 94f7203414..6e02aa4c09 100644 --- a/APMrover2/Makefile +++ b/APMrover2/Makefile @@ -18,15 +18,9 @@ hilnocli: heli: make -f Makefile EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME" -apm2: - make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2" - apm2beta: make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DAPM2_BETA_HARDWARE" -sitl: - make -f ../libraries/Desktop/Makefile.desktop - etags: cd .. && etags -f ArduPlane/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduPlane libraries) diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 68b84748fa..f2bfe22f4e 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -50,6 +50,9 @@ public: k_param_reset_switch_chan, k_param_manual_level, k_param_ins, // libraries/AP_InertialSensor variables + k_param_rssi_pin, + k_param_battery_volt_pin, + k_param_battery_curr_pin, // 110: Telemetry control @@ -312,7 +315,11 @@ public: AP_Float volt_div_ratio; AP_Float curr_amp_per_volt; AP_Float input_voltage; - AP_Int16 pack_capacity; // Battery pack capacity less reserve + AP_Int16 pack_capacity; // Battery pack capacity less reserve + AP_Int8 rssi_pin; + AP_Int8 battery_volt_pin; + AP_Int8 battery_curr_pin; + #if HIL_MODE != HIL_MODE_ATTITUDE #if CONFIG_SONAR == ENABLED AP_Int8 sonar_enabled; diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 8b9fb82c7e..fef2903ee9 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -201,6 +201,28 @@ const AP_Param::Info var_info[] PROGMEM = { GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT", CURR_AMP_PER_VOLT), GSCALAR(input_voltage, "INPUT_VOLTS", INPUT_VOLTAGE), GSCALAR(pack_capacity, "BATT_CAPACITY", HIGH_DISCHARGE), + + // @Param: BATT_VOLT_PIN + // @DisplayName: Battery Voltage sensing pin + // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. + // @Values: -1:Disabled, 0:A0, 1:A1, 13:A13 + // @User: Standard + GSCALAR(battery_volt_pin, "BATT_VOLT_PIN", 1), + + // @Param: BATT_CURR_PIN + // @DisplayName: Battery Current sensing pin + // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. + // @Values: -1:Disabled, 1:A1, 2:A2, 12:A12 + // @User: Standard + GSCALAR(battery_curr_pin, "BATT_CURR_PIN", 2), + + // @Param: RSSI_PIN + // @DisplayName: Receiver RSSI sensing pin + // @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum + // @Values: -1:Disabled, 0:A0, 1:A1, 13:A13 + // @User: Standard + GSCALAR(rssi_pin, "RSSI_PIN", -1), + #if HIL_MODE != HIL_MODE_ATTITUDE #if CONFIG_SONAR == ENABLED // @Param: SONAR_ENABLE @@ -250,7 +272,7 @@ const AP_Param::Info var_info[] PROGMEM = { GOBJECT(ins, "INS_", AP_InertialSensor), #endif -#ifdef DESKTOP_BUILD +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL // @Group: SIM_ // @Path: ../libraries/SITL/SITL.cpp GOBJECT(sitl, "SIM_", SITL), diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde index 60ce3e0f98..9db4349968 100644 --- a/APMrover2/commands.pde +++ b/APMrover2/commands.pde @@ -27,7 +27,7 @@ static void init_commands() static struct Location get_cmd_with_index(int i) { struct Location temp; - long mem; + uint16_t mem; // Find out proper location in memory by using the start_byte position + the index // -------------------------------------------------------------------------------- @@ -37,22 +37,22 @@ static struct Location get_cmd_with_index(int i) }else{ // read WP position mem = (WP_START_BYTE) + (i * WP_SIZE); - temp.id = eeprom_read_byte((uint8_t*)mem); + temp.id = hal.storage->read_byte(mem); mem++; - temp.options = eeprom_read_byte((uint8_t*)mem); + temp.options = hal.storage->read_byte(mem); mem++; - temp.p1 = eeprom_read_byte((uint8_t*)mem); + temp.p1 = hal.storage->read_byte(mem); mem++; - temp.alt = (long)eeprom_read_dword((uint32_t*)mem); + temp.alt = (long)hal.storage->read_dword(mem); mem += 4; - temp.lat = (long)eeprom_read_dword((uint32_t*)mem); + temp.lat = (long)hal.storage->read_dword(mem); mem += 4; - temp.lng = (long)eeprom_read_dword((uint32_t*)mem); + temp.lng = (long)hal.storage->read_dword(mem); } // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative @@ -68,7 +68,7 @@ static struct Location get_cmd_with_index(int i) static void set_cmd_with_index(struct Location temp, int i) { i = constrain(i, 0, g.command_total.get()); - intptr_t mem = WP_START_BYTE + (i * WP_SIZE); + uint16_t mem = WP_START_BYTE + (i * WP_SIZE); // Set altitude options bitmask // XXX What is this trying to do? @@ -78,22 +78,22 @@ static void set_cmd_with_index(struct Location temp, int i) temp.options = 0; } - eeprom_write_byte((uint8_t *) mem, temp.id); + hal.storage->write_byte(mem, temp.id); - mem++; - eeprom_write_byte((uint8_t *) mem, temp.options); + mem++; + hal.storage->write_byte(mem, temp.options); mem++; - eeprom_write_byte((uint8_t *) mem, temp.p1); + hal.storage->write_byte(mem, temp.p1); mem++; - eeprom_write_dword((uint32_t *) mem, temp.alt); + hal.storage->write_dword(mem, temp.alt); mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lat); + hal.storage->write_dword(mem, temp.lat); mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lng); + hal.storage->write_dword(mem, temp.lng); } /* diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index 0f7c3f9fae..78878ae7ee 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -363,7 +363,8 @@ static void do_set_home() static void do_set_servo() { - APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt); + hal.rcout->enable_ch(next_nonnav_command.p1 - 1); + hal.rcout->write(next_nonnav_command.p1 - 1, next_nonnav_command.alt); } static void do_set_relay() diff --git a/APMrover2/commands_process.pde b/APMrover2/commands_process.pde index 738feb1c3e..e606da1ecf 100644 --- a/APMrover2/commands_process.pde +++ b/APMrover2/commands_process.pde @@ -50,7 +50,7 @@ static void process_next_command() // and loads conditional or immediate commands if applicable struct Location temp; - byte old_index = 0; + uint8_t old_index = 0; // these are Navigation/Must commands // --------------------------------- diff --git a/APMrover2/compat.h b/APMrover2/compat.h new file mode 100644 index 0000000000..451e28c2c9 --- /dev/null +++ b/APMrover2/compat.h @@ -0,0 +1,15 @@ + +#ifndef __COMPAT_H__ +#define __COMPAT_H__ + +#define OUTPUT GPIO_OUTPUT +#define INPUT GPIO_INPUT + +#define HIGH 1 +#define LOW 0 + +/* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */ +static void run_cli(AP_HAL::UARTDriver *port); + +#endif // __COMPAT_H__ + diff --git a/APMrover2/compat.pde b/APMrover2/compat.pde new file mode 100644 index 0000000000..171d2dcf79 --- /dev/null +++ b/APMrover2/compat.pde @@ -0,0 +1,37 @@ + + +void delay(uint32_t ms) +{ + hal.scheduler->delay(ms); +} + +void mavlink_delay(uint32_t ms) +{ + hal.scheduler->delay(ms); +} + +uint32_t millis() +{ + return hal.scheduler->millis(); +} + +uint32_t micros() +{ + return hal.scheduler->micros(); +} + +void pinMode(uint8_t pin, uint8_t output) +{ + hal.gpio->pinMode(pin, output); +} + +void digitalWrite(uint8_t pin, uint8_t out) +{ + hal.gpio->write(pin,out); +} + +uint8_t digitalRead(uint8_t pin) +{ + return hal.gpio->read(pin); +} + diff --git a/APMrover2/config.h b/APMrover2/config.h index 121e484833..67d6928e57 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -42,19 +42,11 @@ ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// -// APM HARDWARE -// - -#ifndef CONFIG_APM_HARDWARE -# define CONFIG_APM_HARDWARE APM_HARDWARE_APM1 -#endif - ////////////////////////////////////////////////////////////////////////////// // APM2 HARDWARE DEFAULTS // -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 +#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 # define CONFIG_INS_TYPE CONFIG_INS_MPU6000 # define CONFIG_PUSHBUTTON DISABLED # define CONFIG_RELAY DISABLED @@ -66,7 +58,7 @@ ////////////////////////////////////////////////////////////////////////////// // LED and IO Pins // -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 # define A_LED_PIN 37 # define B_LED_PIN 36 # define C_LED_PIN 35 @@ -78,7 +70,7 @@ # define CONFIG_RELAY ENABLED # define BATTERY_PIN_1 0 # define CURRENT_PIN_1 1 -#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 +#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 # define A_LED_PIN 27 # define B_LED_PIN 26 # define C_LED_PIN 25 @@ -90,9 +82,21 @@ # define USB_MUX_PIN 23 # define BATTERY_PIN_1 1 # define CURRENT_PIN_1 2 +#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL +# define A_LED_PIN 27 +# define B_LED_PIN 26 +# define C_LED_PIN 25 +# define LED_ON LOW +# define LED_OFF HIGH +# define SLIDE_SWITCH_PIN (-1) +# define PUSHBUTTON_PIN (-1) +# define CLI_SLIDER_ENABLED DISABLED +# define USB_MUX_PIN -1 +# define BATTERY_PIN_1 1 +# define CURRENT_PIN_1 2 #endif -#ifdef DESKTOP_BUILD +#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL #define CONFIG_SONAR DISABLED #endif @@ -139,7 +143,7 @@ # endif #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN # ifndef CONFIG_SONAR_SOURCE_ANALOG_PIN -# define CONFIG_SONAR_SOURCE_ANALOG_PIN A0 +# define CONFIG_SONAR_SOURCE_ANALOG_PIN 0 # endif #else # warning Invalid value for CONFIG_SONAR_SOURCE, disabling sonar diff --git a/APMrover2/control_modes.pde b/APMrover2/control_modes.pde index 7b4cd37414..a0ade4098a 100644 --- a/APMrover2/control_modes.pde +++ b/APMrover2/control_modes.pde @@ -3,7 +3,7 @@ static void read_control_switch() { - byte switchPosition = readSwitch(); + uint8_t switchPosition = readSwitch(); // If switchPosition = 255 this indicates that the mode control channel input was out of range // If we get this value we do not want to change modes. @@ -17,7 +17,7 @@ static void read_control_switch() // as a spring loaded trainer switch). if (oldSwitchPosition != switchPosition || (g.reset_switch_chan != 0 && - APM_RC.InputCh(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) { + hal.rcin->read(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) { set_mode(flight_modes[switchPosition]); @@ -32,8 +32,8 @@ static void read_control_switch() } -static byte readSwitch(void){ - uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1); +static uint8_t readSwitch(void){ + uint16_t pulsewidth = hal.rcin->read(g.flight_mode_channel - 1); if (pulsewidth <= 910 || pulsewidth >= 2090) return 255; // This is an error condition if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; diff --git a/APMrover2/defines.h b/APMrover2/defines.h index ec378b0e62..8b7ef1d366 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -215,9 +215,6 @@ enum gcs_severity { #define CONFIG_INS_OILPAN 1 #define CONFIG_INS_MPU6000 2 -#define APM_HARDWARE_APM1 1 -#define APM_HARDWARE_APM2 2 - #define AP_BARO_BMP085 1 #define AP_BARO_MS5611 2 diff --git a/APMrover2/events.pde b/APMrover2/events.pde index 707c6cc8f9..755f3d62bb 100644 --- a/APMrover2/events.pde +++ b/APMrover2/events.pde @@ -32,7 +32,7 @@ static void failsafe_long_on_event(int fstype) { // This is how to handle a long loss of control signal failsafe. gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, ")); - APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC + hal.rcin->clear_overrides(); failsafe = fstype; switch(control_mode) { @@ -94,9 +94,9 @@ static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_ if (event_id >= CH_5 && event_id <= CH_8) { if(event_repeat%2) { - APM_RC.OutputCh(event_id, event_value); // send to Servos + hal.rcout->write(event_id, event_value); // send to Servos } else { - APM_RC.OutputCh(event_id, event_undo_value); + hal.rcout->write(event_id, event_undo_value); } } diff --git a/APMrover2/failsafe.pde b/APMrover2/failsafe.pde index a1d2b80c7d..69092608a4 100644 --- a/APMrover2/failsafe.pde +++ b/APMrover2/failsafe.pde @@ -38,9 +38,14 @@ void failsafe_check(uint32_t tnow) if (in_failsafe && tnow - last_timestamp > 20000) { // pass RC inputs to outputs every 20ms last_timestamp = tnow; - APM_RC.clearOverride(); - for (uint8_t ch=0; ch<8; ch++) { - APM_RC.OutputCh(ch, APM_RC.InputCh(ch)); + hal.rcin->clear_overrides(); + uint8_t start_ch = 0; + if (demoing_servos) { + start_ch = 1; } + for (uint8_t ch=start_ch; ch<4; ch++) { + hal.rcout->write(ch, hal.rcin->read(ch)); + } + RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true); } } diff --git a/APMrover2/planner.pde b/APMrover2/planner.pde deleted file mode 100644 index 30b15fbbc3..0000000000 --- a/APMrover2/planner.pde +++ /dev/null @@ -1,53 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// These are function definitions so the Menu can be constructed before the functions -// are defined below. Order matters to the compiler. -static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv); - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Common for implementation details -static const struct Menu::command planner_menu_commands[] PROGMEM = { - {"gcs", planner_gcs}, -}; - -// A Macro to create the Menu -MENU(planner_menu, "planner", planner_menu_commands); - -static int8_t -planner_mode(uint8_t argc, const Menu::arg *argv) -{ - cliSerial->printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n")); - planner_menu.run(); - return 0; -} -static int8_t -planner_gcs(uint8_t argc, const Menu::arg *argv) -{ - gcs0.init(&Serial); - -#if USB_MUX_PIN < 0 - // we don't have gcs3 if we have the USB mux setup - gcs3.init(&Serial3); -#endif - - int loopcount = 0; - while (1) { - if (millis()-fast_loopTimer > 19) { - fast_loopTimer = millis(); - - gcs_update(); - - read_radio(); - - gcs_data_stream_send(); - if ((loopcount % 16) == 0) { // 3 hz - gcs_send_message(MSG_HEARTBEAT); - } - loopcount++; - } - } - return 0; -} - diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde index 65070aee59..aa384e1769 100644 --- a/APMrover2/radio.pde +++ b/APMrover2/radio.pde @@ -2,7 +2,7 @@ //Function that will read the radio data, limit servos and trigger a failsafe // ---------------------------------------------------------------------------- -static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling +static uint8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling static void init_rc_in() @@ -30,52 +30,50 @@ static void init_rc_in() static void init_rc_out() { - APM_RC.Init( &isr_registry ); // APM Radio initialization - - APM_RC.enable_out(CH_1); - APM_RC.enable_out(CH_2); - APM_RC.enable_out(CH_3); - APM_RC.enable_out(CH_4); - APM_RC.enable_out(CH_5); - APM_RC.enable_out(CH_6); - APM_RC.enable_out(CH_7); - APM_RC.enable_out(CH_8); + hal.rcout->enable_ch(CH_1); + hal.rcout->enable_ch(CH_2); + hal.rcout->enable_ch(CH_3); + hal.rcout->enable_ch(CH_4); + hal.rcout->enable_ch(CH_5); + hal.rcout->enable_ch(CH_6); + hal.rcout->enable_ch(CH_7); + hal.rcout->enable_ch(CH_8); #if HIL_MODE != HIL_MODE_ATTITUDE - APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs - APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); - APM_RC.OutputCh(CH_3, g.channel_throttle.radio_trim); - APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim); + hal.rcout->write(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs + hal.rcout->write(CH_2, g.channel_pitch.radio_trim); + hal.rcout->write(CH_3, g.channel_throttle.radio_trim); + hal.rcout->write(CH_4, g.channel_rudder.radio_trim); - APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); - APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); - APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); - APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); + hal.rcout->write(CH_5, g.rc_5.radio_trim); + hal.rcout->write(CH_6, g.rc_6.radio_trim); + hal.rcout->write(CH_7, g.rc_7.radio_trim); + hal.rcout->write(CH_8, g.rc_8.radio_trim); #else - APM_RC.OutputCh(CH_1, 1500); // Initialization of servo outputs - APM_RC.OutputCh(CH_2, 1500); - APM_RC.OutputCh(CH_3, 1000); - APM_RC.OutputCh(CH_4, 1500); + hal.rcout->write(CH_1, 1500); // Initialization of servo outputs + hal.rcout->write(CH_2, 1500); + hal.rcout->write(CH_3, 1000); + hal.rcout->write(CH_4, 1500); - APM_RC.OutputCh(CH_5, 1500); - APM_RC.OutputCh(CH_6, 1500); - APM_RC.OutputCh(CH_7, 1500); - APM_RC.OutputCh(CH_8, 2000); + hal.rcout->write(CH_5, 1500); + hal.rcout->write(CH_6, 1500); + hal.rcout->write(CH_7, 1500); + hal.rcout->write(CH_8, 2000); #endif } static void read_radio() { - g.channel_roll.set_pwm(APM_RC.InputCh(CH_ROLL)); - g.channel_pitch.set_pwm(APM_RC.InputCh(CH_PITCH)); + g.channel_roll.set_pwm(hal.rcin->read(CH_ROLL)); + g.channel_pitch.set_pwm(hal.rcin->read(CH_PITCH)); - g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3)); - g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4)); - g.rc_5.set_pwm(APM_RC.InputCh(CH_5)); - g.rc_6.set_pwm(APM_RC.InputCh(CH_6)); - g.rc_7.set_pwm(APM_RC.InputCh(CH_7)); - g.rc_8.set_pwm(APM_RC.InputCh(CH_8)); + g.channel_throttle.set_pwm(hal.rcout->read(CH_3)); + g.channel_rudder.set_pwm(hal.rcout->read(CH_4)); + g.rc_5.set_pwm(hal.rcout->read(CH_5)); + g.rc_6.set_pwm(hal.rcout->read(CH_6)); + g.rc_7.set_pwm(hal.rcout->read(CH_7)); + g.rc_8.set_pwm(hal.rcout->read(CH_8)); control_failsafe(g.channel_throttle.radio_in); diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index dc248b9c02..4ad1631b5d 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -28,15 +28,17 @@ static void read_battery(void) return; } - if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { - static AP_AnalogSource_Arduino bat_pin(BATTERY_PIN_1); - battery_voltage1 = BATTERY_VOLTAGE(bat_pin.read_average()); + if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { + // this copes with changing the pin at runtime + batt_volt_pin->set_pin(g.battery_volt_pin); + battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin->read_average()); + } + if(g.battery_monitoring == 4) { + // this copes with changing the pin at runtime + batt_curr_pin->set_pin(g.battery_curr_pin); + current_amps1 = CURRENT_AMPS(batt_curr_pin->read_average()); + current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours) } - if(g.battery_monitoring == 4) { - static AP_AnalogSource_Arduino current_pin(CURRENT_PIN_1); - current_amps1 = CURRENT_AMPS(current_pin.read_average()); - current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours) - } #if BATTERY_EVENT == ENABLED if(battery_voltage1 < LOW_VOLTAGE) low_battery_event(); diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index 8b799fbbcd..660969e8fd 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -169,7 +169,9 @@ setup_radio(uint8_t argc, const Menu::arg *argv) g.rc_8.update_min_max(); if(cliSerial->available() > 0){ - cliSerial->flush(); + while (cliSerial->available() > 0) { + cliSerial->read(); + } g.channel_roll.save_eeprom(); g.channel_pitch.save_eeprom(); g.channel_throttle.save_eeprom(); @@ -191,7 +193,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv) { - byte switchPosition, mode = 0; + uint8_t switchPosition, mode = 0; cliSerial->printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); print_hit_enter(); @@ -294,34 +296,14 @@ setup_erase(uint8_t argc, const Menu::arg *argv) handle full accelerometer calibration via user dialog */ #if !defined( __AVR_ATmega1280__ ) -static void setup_printf_P(const prog_char_t *fmt, ...) -{ - va_list arg_list; - va_start(arg_list, fmt); - cliSerial->vprintf_P(fmt, arg_list); - va_end(arg_list); -} - -static void setup_wait_key(void) -{ - // wait for user input - while (!cliSerial->available()) { - delay(20); - } - // clear input buffer - while( cliSerial->available() ) { - cliSerial->read(); - } -} - static int8_t setup_accel_scale(uint8_t argc, const Menu::arg *argv) { cliSerial->println_P(PSTR("Initialising gyros")); ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, - delay, flash_leds, &timer_scheduler); - if (ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key)) { + flash_leds); + if (ins.calibrate_accel(flash_leds, hal.console)) { if (g.manual_level == 0) { cliSerial->println_P(PSTR("Setting MANUAL_LEVEL to 1")); g.manual_level.set_and_save(1); @@ -538,7 +520,7 @@ print_radio_values() } static void -print_switch(byte p, byte m) +print_switch(uint8_t p, uint8_t m) { cliSerial->printf_P(PSTR("Pos %d: "),p); print_flight_mode(m); @@ -597,10 +579,10 @@ radio_input_switch(void) static void zero_eeprom(void) { - byte b = 0; + uint8_t b = 0; cliSerial->printf_P(PSTR("\nErasing EEPROM\n")); - for (intptr_t i = 0; i < EEPROM_MAX_ADDR; i++) { - eeprom_write_byte((uint8_t *) i, b); + for (uint16_t i = 0; i < EEPROM_MAX_ADDR; i++) { + hal.storage->write_byte(i, b); } cliSerial->printf_P(PSTR("done\n")); } @@ -614,18 +596,4 @@ static void print_enabled(bool b) cliSerial->printf_P(PSTR("abled\n")); } -static void -print_accel_offsets_and_scaling(void) -{ - Vector3f accel_offsets = ins.get_accel_offsets(); - Vector3f accel_scale = ins.get_accel_scale(); - cliSerial->printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"), - (float)accel_offsets.x, // Pitch - (float)accel_offsets.y, // Roll - (float)accel_offsets.z, // YAW - (float)accel_scale.x, // Pitch - (float)accel_scale.y, // Roll - (float)accel_scale.z); // YAW -} - #endif // CLI_ENABLED diff --git a/APMrover2/system.pde b/APMrover2/system.pde index 84cd10f83f..f98052b80d 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -14,7 +14,6 @@ static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde #endif static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp -static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde // This is the help function // PSTR is an AVR macro to read strings from flash memory @@ -40,18 +39,17 @@ static const struct Menu::command main_menu_commands[] PROGMEM = { #endif {"setup", setup_mode}, {"test", test_mode}, - {"help", main_menu_help}, - {"planner", planner_mode} + {"help", main_menu_help} }; // Create the top-level menu object. MENU(main_menu, THISFIRMWARE, main_menu_commands); // the user wants the CLI. It never exits -static void run_cli(FastSerial *port) +static void run_cli(AP_HAL::UARTDriver *port) { // disable the failsafe code in the CLI - timer_scheduler.set_failsafe(NULL); + hal.scheduler->register_timer_failsafe(NULL,1); cliSerial = port; Menu::set_port(port); @@ -93,7 +91,7 @@ static void init_ardupilot() // XXX This could be optimised to reduce the buffer sizes in the cases // where they are not otherwise required. // - cliSerial->begin(SERIAL0_BAUD, 128, 128); + hal.uartA->begin(SERIAL0_BAUD, 128, 128); // GPS serial port. // @@ -105,37 +103,12 @@ static void init_ardupilot() // on the message set configured. // // standard gps running - Serial1.begin(115200, 128, 16); + hal.uartB->begin(115200, 128, 16); cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE "\n\nFree RAM: %u\n"), memcheck_available_memory()); - // - // Initialize Wire and SPI libraries - // -#ifndef DESKTOP_BUILD - I2c.begin(); - I2c.timeOut(5); - // initially set a fast I2c speed, and drop it on first failures - I2c.setSpeed(true); -#endif - SPI.begin(); - SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate - // - // Initialize the ISR registry. - // - isr_registry.init(); - - // - // Initialize the timer scheduler to use the ISR registry. - // - - timer_scheduler.init( & isr_registry ); - - // initialise the analog port reader - AP_AnalogSource_Arduino::init_timer(&timer_scheduler); - // // Check the EEPROM format version before loading any parameters from EEPROM. // @@ -147,18 +120,22 @@ static void init_ardupilot() g.num_resets.set_and_save(g.num_resets+1); // init the GCS - gcs0.init(&Serial); + gcs0.init(hal.uartA); + + // Register mavlink_delay_cb, which will run anytime you have + // more than 5ms remaining in your call to hal.scheduler->delay + hal.scheduler->register_delay_callback(mavlink_delay_cb, 5); #if USB_MUX_PIN > 0 if (!usb_connected) { // we are not connected via USB, re-init UART0 with right // baud rate - cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); } #else // we have a 2nd serial port for telemetry - Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); - gcs3.init(&Serial3); + hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + gcs3.init(hal.uartC); #endif mavlink_system.sysid = g.sysid_this_mav; @@ -182,7 +159,7 @@ static void init_ardupilot() #if HIL_MODE != HIL_MODE_ATTITUDE #if CONFIG_ADC == ENABLED - adc.Init(&timer_scheduler); // APM ADC library initialization + adc.Init(); // APM ADC library initialization #endif #if LITE == DISABLED @@ -239,15 +216,14 @@ static void init_ardupilot() // Do GPS init g_gps = &g_gps_driver; // GPS initialisation - g_gps->init(GPS::GPS_ENGINE_AUTOMOTIVE); + g_gps->init(hal.uartB, GPS::GPS_ENGINE_AUTOMOTIVE); //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 = APM_RC.setHIL(rc_override); // Set initial values for no override + rc_override_active = hal.rcin->set_overrides(rc_override, 8); - RC_Channel::set_apm_rc( &APM_RC ); // Provide reference to RC outputs. init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up the timer libs @@ -261,15 +237,14 @@ static void init_ardupilot() pinMode(PUSHBUTTON_PIN, INPUT); // unused #endif #if CONFIG_RELAY == ENABLED - DDRL |= B00000100; // Set Port L, pin 2 to output for the relay + relay.init(); #endif /* setup the 'main loop is dead' check. Note that this relies on the RC library being initialised. */ - timer_scheduler.set_failsafe(failsafe_check); - + hal.scheduler->register_timer_failsafe(failsafe_check, 1000); // If the switch is in 'menu' mode, run the main menu. // @@ -293,7 +268,7 @@ static void init_ardupilot() const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n"); cliSerial->println_P(msg); #if USB_MUX_PIN == 0 - Serial3.println_P(msg); + hal.uartC->println_P(msg); #endif #endif // CLI_ENABLED @@ -353,7 +328,7 @@ static void startup_ground(void) gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive.")); } -static void set_mode(byte mode) +static void set_mode(uint8_t mode) { if(control_mode == mode){ @@ -449,12 +424,12 @@ static void startup_INS_ground(bool force_accel_level) ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, - mavlink_delay, flash_leds, &timer_scheduler); + flash_leds); if (force_accel_level || g.manual_level == 0) { // when MANUAL_LEVEL is set to 1 we don't do accelerometer // levelling on each boot, and instead rely on the user to do // it once via the ground station - ins.init_accel(mavlink_delay, flash_leds); + ins.init_accel(flash_leds); } ahrs.set_fly_forward(true); ahrs.reset(); @@ -538,9 +513,9 @@ static void check_usb_mux(void) // the user has switched to/from the telemetry port usb_connected = usb_check; if (usb_connected) { - cliSerial->begin(SERIAL0_BAUD, 128, 128); + hal.uartA->begin(SERIAL0_BAUD, 128, 128); } else { - cliSerial->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); } } #endif @@ -556,17 +531,13 @@ void flash_leds(bool on) digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF); } -#ifndef DESKTOP_BUILD /* * Read Vcc vs 1.1v internal reference */ uint16_t board_voltage(void) { - static AP_AnalogSource_Arduino vcc(ANALOG_PIN_VCC); - return vcc.read_vcc(); + return vcc_pin->read_latest(); } -#endif - static void print_flight_mode(uint8_t mode) diff --git a/APMrover2/test.pde b/APMrover2/test.pde index d29aa663f4..22df42266c 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -20,9 +20,6 @@ static int8_t test_wp(uint8_t argc, const Menu::arg *argv); static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); #endif static int8_t test_mag(uint8_t argc, const Menu::arg *argv); -static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); -static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); -static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv); static int8_t test_logging(uint8_t argc, const Menu::arg *argv); @@ -38,8 +35,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { {"battery", test_battery}, {"relay", test_relay}, {"waypoints", test_wp}, - {"xbee", test_xbee}, - {"eedump", test_eedump}, {"modeswitch", test_modeswitch}, // Tests below here are for hardware sensors only present @@ -49,7 +44,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { {"adc", test_adc}, #endif {"gps", test_gps}, - {"rawgps", test_rawgps}, {"ins", test_ins}, #if CONFIG_SONAR == ENABLED {"sonartest", test_sonar}, @@ -82,21 +76,6 @@ static void print_hit_enter() cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n")); } -static int8_t -test_eedump(uint8_t argc, const Menu::arg *argv) -{ - intptr_t i, j; - - // hexdump the EEPROM - for (i = 0; i < EEPROM_MAX_ADDR; i += 16) { - cliSerial->printf_P(PSTR("%04x:"), i); - for (j = 0; j < 16; j++) - cliSerial->printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j))); - cliSerial->println(); - } - return(0); -} - static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv) { @@ -137,12 +116,12 @@ test_passthru(uint8_t argc, const Menu::arg *argv) delay(20); // New radio frame? (we could use also if((millis()- timer) > 20) - if (APM_RC.GetState() == 1){ + if (hal.rcin->valid() > 0) { cliSerial->print("CH:"); for(int i = 0; i < 8; i++){ - cliSerial->print(APM_RC.InputCh(i)); // Print channel values + cliSerial->print(hal.rcout->read(i)); // Print channel values cliSerial->print(","); - APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos + hal.rcout->write(i, hal.rcout->read(i)); // Copy input to Servos } cliSerial->println(); } @@ -198,7 +177,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv) { - byte fail_test; + uint8_t fail_test; print_hit_enter(); for(int i = 0; i < 50; i++){ delay(20); @@ -317,7 +296,7 @@ test_wp(uint8_t argc, const Menu::arg *argv) cliSerial->printf_P(PSTR("%d waypoints\n"), (int)g.command_total); cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); - for(byte i = 0; i <= g.command_total; i++){ + for(uint8_t i = 0; i <= g.command_total; i++){ struct Location temp = get_cmd_with_index(i); test_wp_print(&temp, i); } @@ -326,7 +305,7 @@ test_wp(uint8_t argc, const Menu::arg *argv) } static void -test_wp_print(struct Location *cmd, byte wp_index) +test_wp_print(struct Location *cmd, uint8_t wp_index) { cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), (int)wp_index, @@ -338,25 +317,6 @@ test_wp_print(struct Location *cmd, byte wp_index) cmd->lng); } -static int8_t -test_xbee(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - cliSerial->printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); - - while(1){ - - if (Serial3.available()) - Serial3.write(Serial3.read()); - - if(cliSerial->available() > 0){ - return (0); - } - } -} - - static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv) { @@ -369,7 +329,7 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv) while(1){ delay(20); - byte switchPosition = readSwitch(); + uint8_t switchPosition = readSwitch(); if (oldSwitchPosition != switchPosition){ cliSerial->printf_P(PSTR("Position %d\n"), switchPosition); oldSwitchPosition = switchPosition; @@ -415,7 +375,7 @@ static int8_t test_adc(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); - adc.Init(&timer_scheduler); + adc.Init(); delay(1000); cliSerial->printf_P(PSTR("ADC\n")); delay(1000); @@ -467,7 +427,7 @@ test_ins(uint8_t argc, const Menu::arg *argv) //cliSerial->printf_P(PSTR("Calibrating.")); ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, - delay, flash_leds, &timer_scheduler); + flash_leds); ahrs.reset(); print_hit_enter(); @@ -530,7 +490,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) // we need the AHRS initialised for this test ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, - delay, flash_leds, &timer_scheduler); + flash_leds); ahrs.reset(); int counter = 0; @@ -597,31 +557,6 @@ test_mag(uint8_t argc, const Menu::arg *argv) //------------------------------------------------------------------------------------------- // real sensors that have not been simulated yet go here -#if HIL_MODE == HIL_MODE_DISABLED - -static int8_t -test_rawgps(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - if (Serial3.available()){ - digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS - Serial1.write(Serial3.read()); - digitalWrite(B_LED_PIN, LED_OFF); - } - if (Serial1.available()){ - digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS - Serial3.write(Serial1.read()); - digitalWrite(C_LED_PIN, LED_OFF); - } - if(cliSerial->available() > 0){ - return (0); - } - } -} - #if CONFIG_SONAR == ENABLED static int8_t test_sonar(uint8_t argc, const Menu::arg *argv) @@ -634,7 +569,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv) while(1){ delay(20); if(g.sonar_enabled){ - sonar_dist = sonar.read(); + sonar_dist = sonar->read(); } cliSerial->printf_P(PSTR("sonar_dist = %d\n"), (int)sonar_dist); @@ -645,6 +580,5 @@ test_sonar(uint8_t argc, const Menu::arg *argv) return (0); } #endif // SONAR == ENABLED -#endif // HIL_MODE == HIL_MODE_DISABLED #endif // CLI_ENABLED