ArduCopter: more work

ArduCopter/ArduCopter.pde

@@ -54,7 +54,9 @@
 // Header includes
 ////////////////////////////////////////////////////////////////////////////////
 
-// Libraries
+#include <math.h>
+#include <stdio.h>
+#include <stdarg.h>
 
 // Common dependencies
 #include <AP_Common.h>
@@ -65,6 +67,7 @@
 #include <AP_HAL.h>
 #include <AP_HAL_AVR.h>
 #include <AP_HAL_AVR_SITL.h>
+
 // Application dependencies
 #include <GCS_MAVLink.h>        // MAVLink GCS definitions
 #include <AP_GPS.h>             // ArduPilot GPS library
@@ -1175,7 +1178,7 @@ static void fifty_hz_loop()
     // ----------
 # if CONFIG_SONAR == ENABLED
     if(g.sonar_enabled) {
-        sonar_alt = sonar.read();
+        sonar_alt = sonar->read();
     }
 #endif
 

ArduCopter/Attitude.pde

@@ -1007,7 +1007,7 @@ get_throttle_rate_stabilized(int16_t target_rate)
     static float alt_rate = 0;      // the desired climb rate in cm/s.
     static uint32_t last_call_ms = 0;
 
-    float altitude_error;
+    float _altitude_error;
     int16_t desired_rate;
     int16_t alt_error_max;
     uint32_t now = millis();
@@ -1023,10 +1023,10 @@ get_throttle_rate_stabilized(int16_t target_rate)
     alt_desired += alt_rate * 0.02;
 
     alt_error_max = constrain(600-abs(target_rate),100,600);
-    altitude_error = constrain(alt_desired - current_loc.alt, -alt_error_max, alt_error_max);
+    _altitude_error = constrain(alt_desired - current_loc.alt, -alt_error_max, alt_error_max);
-    alt_desired = current_loc.alt + altitude_error;
+    alt_desired = current_loc.alt + _altitude_error;
 
-    desired_rate = g.pi_alt_hold.get_p(altitude_error);
+    desired_rate = g.pi_alt_hold.get_p(_altitude_error);
     desired_rate = constrain(desired_rate, -200, 200) + target_rate;
     desired_rate = constrain(desired_rate, -VELOCITY_MAX_Z, VELOCITY_MAX_Z);  // TO-DO: replace constrains with ALTHOLD_MIN_CLIMB_RATE and ALTHOLD_MAX_CLIMB_RATE?
 
@@ -1040,11 +1040,11 @@ get_throttle_rate_stabilized(int16_t target_rate)
 static void
 get_throttle_althold(int32_t target_alt, int16_t max_climb_rate)
 {
-    int32_t altitude_error;
+    int32_t _altitude_error;
     int16_t desired_rate;
 
-    altitude_error = target_alt - current_loc.alt;
+    _altitude_error = target_alt - current_loc.alt;
-    desired_rate = g.pi_alt_hold.get_p(altitude_error);
+    desired_rate = g.pi_alt_hold.get_p(_altitude_error);
     desired_rate = constrain(desired_rate, ALTHOLD_MIN_CLIMB_RATE, max_climb_rate);
 
     // call rate based throttle controller which will update accel based throttle controller targets

ArduCopter/GCS_Mavlink.pde

@@ -266,7 +266,7 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
     mavlink_msg_hwstatus_send(
         chan,
         board_voltage(),
-        I2c.lockup_count());
+        hal.i2c->lockup_count());
 }
 #endif
 
@@ -1090,14 +1090,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             if (packet.param1 == 1 ||
                 packet.param2 == 1 ||
                 packet.param3 == 1) {
-                ins.init_accel(mavlink_delay, flash_leds);
+                ins.init_accel(flash_leds);
             }
             if (packet.param4 == 1) {
                 trim_radio();
             }
             if (packet.param5 == 1) {
                 // this blocks
-                ins.calibrate_accel(mavlink_delay, flash_leds, gcs_send_text_fmt, setup_wait_key);
+                ins.calibrate_accel(flash_leds, hal.console);
             }
             result = MAV_RESULT_ACCEPTED;
             break;
@@ -1750,7 +1750,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         v[5] = packet.chan6_raw;
         v[6] = packet.chan7_raw;
         v[7] = packet.chan8_raw;
-        APM_RC.setHIL(v);
+        hal.rcin->set_overrides(v, 8);
         break;
     }
 
@@ -2048,42 +2048,32 @@ GCS_MAVLINK::queued_waypoint_send()
  *  MAVLink to process packets while waiting for the initialisation to
  *  complete
  */
-static void mavlink_delay(unsigned long t)
+static void mavlink_delay_cb()
 {
-    uint32_t tstart;
     static uint32_t last_1hz, last_50hz, last_5s;
 
-    if (in_mavlink_delay) {
+    if (!gcs0.initialised) return;
-        // this should never happen, but let's not tempt fate by
-        // letting the stack grow too much
-        delay(t);
-        return;
-    }
 
     in_mavlink_delay = true;
 
-    tstart = millis();
+    uint32_t tnow = millis();
-    do {
+    if (tnow - last_1hz > 1000) {
-        uint32_t tnow = millis();
+        last_1hz = tnow;
-        if (tnow - last_1hz > 1000) {
+        gcs_send_message(MSG_HEARTBEAT);
-            last_1hz = tnow;
+        gcs_send_message(MSG_EXTENDED_STATUS1);
-            gcs_send_message(MSG_HEARTBEAT);
+    }
-            gcs_send_message(MSG_EXTENDED_STATUS1);
+    if (tnow - last_50hz > 20) {
-        }
+        last_50hz = tnow;
-        if (tnow - last_50hz > 20) {
+        gcs_update();
-            last_50hz = tnow;
+        gcs_data_stream_send();
-            gcs_update();
+    }
-            gcs_data_stream_send();
+    if (tnow - last_5s > 5000) {
-        }
+        last_5s = tnow;
-        if (tnow - last_5s > 5000) {
+        gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
-            last_5s = tnow;
+    }
-            gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
-        }
-        delay(1);
 #if USB_MUX_PIN > 0
-        check_usb_mux();
+    check_usb_mux();
 #endif
-    } while (millis() - tstart < t);
 
     in_mavlink_delay = false;
 }

ArduCopter/Log.pde

@@ -156,7 +156,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
 static void do_erase_logs(void)
 {
 	gcs_send_text_P(SEVERITY_LOW, PSTR("Erasing logs\n"));
-    DataFlash.EraseAll(mavlink_delay);
+    DataFlash.EraseAll();
 	gcs_send_text_P(SEVERITY_LOW, PSTR("Log erase complete\n"));
 }
 
@@ -228,7 +228,7 @@ process_logs(uint8_t argc, const Menu::arg *argv)
 
 // print_latlon - prints an latitude or longitude value held in an int32_t
 // probably this should be moved to AP_Common
-void print_latlon(AL_HAL::BetterStream *s, int32_t lat_or_lon)
+void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon)
 {
     int32_t dec_portion, frac_portion;
     int32_t abs_lat_or_lon = labs(lat_or_lon);
@@ -283,9 +283,9 @@ static void Log_Read_GPS()
     cliSerial->printf_P(PSTR("GPS, %ld, %d, "),
                     (long)temp1,                          // 1 time
                     (int)temp2);                          // 2 sats
-    print_latlon(&Serial, temp3);
+    print_latlon(cliSerial, temp3);
     cliSerial->print_P(PSTR(", "));
-    print_latlon(&Serial, temp4);
+    print_latlon(cliSerial, temp4);
     cliSerial->printf_P(PSTR(", %4.4f, %4.4f, %d, %ld\n"),
                     temp5,                                // 5 gps alt
                     temp6,                                // 6 sensor alt

ArduCopter/commands.pde

@@ -32,24 +32,24 @@ static struct Location get_cmd_with_index(int i)
     }else{
         // we can load a command, we don't process it yet
         // read WP position
-        uintptr_t mem = (WP_START_BYTE) + (i * WP_SIZE);
+        uint16_t 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 = eeprom_read_dword((uint32_t*)mem);           // alt is stored in CM! Alt is stored relative!
+        temp.alt = hal.storage->read_dword(mem);           // alt is stored in CM! Alt is stored relative!
 
         mem += 4;
-        temp.lat = eeprom_read_dword((uint32_t*)mem);         // lat is stored in decimal * 10,000,000
+        temp.lat = hal.storage->read_dword(mem);         // lat is stored in decimal * 10,000,000
 
         mem += 4;
-        temp.lng = eeprom_read_dword((uint32_t*)mem);         // lon is stored in decimal * 10,000,000
+        temp.lng = hal.storage->read_dword(mem);         // lon is stored in decimal * 10,000,000
     }
 
     // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
@@ -81,24 +81,24 @@ static void set_cmd_with_index(struct Location temp, int i)
         temp.id = MAV_CMD_NAV_WAYPOINT;
     }
 
-    uintptr_t mem = WP_START_BYTE + (i * WP_SIZE);
+    uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
 
-    eeprom_write_byte((uint8_t *)   mem, temp.id);
+    hal.storage->write_byte(mem, temp.id);
 
     mem++;
-    eeprom_write_byte((uint8_t *)   mem, temp.options);
+    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);     // Alt is stored in CM!
+    hal.storage->write_dword(mem, temp.alt);     // Alt is stored in CM!
 
     mem += 4;
-    eeprom_write_dword((uint32_t *) mem, temp.lat);     // Lat is stored in decimal degrees * 10^7
+    hal.storage->write_dword(mem, temp.lat);     // Lat is stored in decimal degrees * 10^7
 
     mem += 4;
-    eeprom_write_dword((uint32_t *) mem, temp.lng);     // Long is stored in decimal degrees * 10^7
+    hal.storage->write_dword(mem, temp.lng);     // Long is stored in decimal degrees * 10^7
 
     // Make sure our WP_total
     if(g.command_total < (i+1))

ArduCopter/commands_logic.pde

@@ -782,8 +782,8 @@ static void do_set_servo()
 
     // send output to channel
     if (channel_num != 0xff) {
-        APM_RC.enable_out(channel_num);
+        hal.rcout->enable_ch(channel_num);
-        APM_RC.OutputCh(channel_num, command_cond_queue.alt);
+        hal.rcout->write(channel_num, command_cond_queue.alt);
     }
 }
 

ArduCopter/compat.h

@@ -11,5 +11,8 @@
 /* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */
 static void run_cli(AP_HAL::UARTDriver *port);
 
+// XXX AP_HAL tofix - what is bitRead? temporarily disable.
+#define bitRead(a,b) false
+
 #endif // __COMPAT_H__
 

ArduCopter/compat.pde

@@ -25,11 +25,21 @@ void pinMode(uint8_t pin, uint8_t output)
     hal.gpio->pinMode(pin, output);
 }
 
+void digitalWriteFast(uint8_t pin, uint8_t out)
+{
+    hal.gpio->write(pin,out);
+}
+
 void digitalWrite(uint8_t pin, uint8_t out)
 {
     hal.gpio->write(pin,out);
 }
 
+uint8_t digitalReadFast(uint8_t pin)
+{
+    return hal.gpio->read(pin);
+}
+
 uint8_t digitalRead(uint8_t pin)
 {
     return hal.gpio->read(pin);

ArduCopter/events.pde

@@ -111,9 +111,9 @@ static void update_events()     // 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);
             }
         }
 

ArduCopter/limits.pde

@@ -43,10 +43,12 @@ static void limits_loop() {
     lim_state = limits.state();
 
     // Use limits channel to determine LIMITS_ENABLED or LIMITS_DISABLED state
-    if (lim_state != LIMITS_DISABLED  && limits.channel() !=0 && APM_RC.InputCh(limits.channel()-1) < LIMITS_ENABLE_PWM) {
+    if (lim_state != LIMITS_DISABLED  && limits.channel() !=0 
+            && hal.rcin->read(limits.channel()-1) < LIMITS_ENABLE_PWM) {
         limits.set_state(LIMITS_DISABLED);
     }
-    else if (lim_state == LIMITS_DISABLED && limits.channel() !=0 && APM_RC.InputCh(limits.channel()-1) >= LIMITS_ENABLE_PWM) {
+    else if (lim_state == LIMITS_DISABLED && limits.channel() !=0
+            && hal.rcin->read(limits.channel()-1) >= LIMITS_ENABLE_PWM) {
         limits.set_state(LIMITS_ENABLED);
     }
 

ArduCopter/motors.pde

@@ -111,9 +111,9 @@ static void init_arm_motors()
     // we don't want writes to the serial port to cause us to pause
     // mid-flight, so set the serial ports non-blocking once we arm
     // the motors
-    cliSerial->set_blocking_writes(false);
+    hal.uartA->set_blocking_writes(false);
     if (gcs3.initialised) {
-        Serial3.set_blocking_writes(false);
+        hal.uartC->set_blocking_writes(false);
     }
 
 #if COPTER_LEDS == ENABLED