diff --git a/Tools/AntennaTracker/GCS_Mavlink.pde b/Tools/AntennaTracker/GCS_Mavlink.pde index 7869783a25..1e4a7e226c 100644 --- a/Tools/AntennaTracker/GCS_Mavlink.pde +++ b/Tools/AntennaTracker/GCS_Mavlink.pde @@ -28,9 +28,9 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) chan, MAV_TYPE_ANTENNA_TRACKER, MAV_AUTOPILOT_ARDUPILOTMEGA, + MAV_MODE_FLAG_AUTO_ENABLED, 0, - 0, - 0); + MAV_STATE_ACTIVE); } static NOINLINE void send_attitude(mavlink_channel_t chan) @@ -56,16 +56,12 @@ static void NOINLINE send_location(mavlink_channel_t chan) } else { fix_time = hal.scheduler->millis(); } - - Location loc; - ahrs.get_position(loc); - mavlink_msg_global_position_int_send( chan, fix_time, - loc.lat, // in 1E7 degrees - loc.lng, // in 1E7 degrees - g_gps->altitude_cm * 10, // millimeters above sea level + current_loc.lat, // in 1E7 degrees + current_loc.lng, // in 1E7 degrees + current_loc.alt * 10, // millimeters above sea level 0, g_gps->velocity_north() * 100, // X speed cm/s (+ve North) g_gps->velocity_east() * 100, // Y speed cm/s (+ve East) @@ -75,6 +71,11 @@ static void NOINLINE send_location(mavlink_channel_t chan) static void NOINLINE send_gps_raw(mavlink_channel_t chan) { + static uint32_t last_send_time; + if (last_send_time != 0 && last_send_time == g_gps->last_fix_time && g_gps->status() >= GPS::GPS_OK_FIX_3D) { + return; + } + last_send_time = g_gps->last_fix_time; mavlink_msg_gps_raw_int_send( chan, g_gps->last_fix_time*(uint64_t)1000, @@ -209,6 +210,14 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan) hal.i2c->lockup_count()); } +static void NOINLINE send_waypoint_request(mavlink_channel_t chan) +{ + if (chan == MAVLINK_COMM_0) + gcs0.queued_waypoint_send(); + else + gcs3.queued_waypoint_send(); +} + static void NOINLINE send_statustext(mavlink_channel_t chan) { mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status); @@ -292,6 +301,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, } break; + case MSG_NEXT_WAYPOINT: + CHECK_PAYLOAD_SIZE(MISSION_REQUEST); + send_waypoint_request(chan); + break; + case MSG_STATUSTEXT: CHECK_PAYLOAD_SIZE(STATUSTEXT); send_statustext(chan); @@ -315,7 +329,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, case MSG_VFR_HUD: case MSG_RADIO_IN: case MSG_SYSTEM_TIME: - case MSG_NEXT_WAYPOINT: case MSG_LIMITS_STATUS: case MSG_FENCE_STATUS: case MSG_SIMSTATE: @@ -514,6 +527,7 @@ GCS_MAVLINK::update(void) // Update packet drops counter packet_drops += status.packet_rx_drop_count; + } // see if we should send a stream now. Called at 50Hz @@ -624,6 +638,8 @@ GCS_MAVLINK::send_text_P(gcs_severity severity, const prog_char_t *str) void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { +// hal.uartA->printf("handleMessage %d\n", msg->msgid); + switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: @@ -811,7 +827,182 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { + case MAVLINK_MSG_ID_COMMAND_LONG: + { + // decode + mavlink_command_long_t packet; + mavlink_msg_command_long_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) break; + + uint8_t result = MAV_RESULT_UNSUPPORTED; + + // do command + send_text_P(SEVERITY_LOW,PSTR("command received: ")); + + switch(packet.command) { + + case MAV_CMD_PREFLIGHT_CALIBRATION: + { + if (packet.param1 == 1 || + packet.param2 == 1) { + calibrate_ins(); + } else if (packet.param3 == 1) { + init_barometer(); + } + if (packet.param4 == 1) { + // Cant trim radio + } +#if !defined( __AVR_ATmega1280__ ) + if (packet.param5 == 1) { + float trim_roll, trim_pitch; + AP_InertialSensor_UserInteract_MAVLink interact(chan); + if(ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { + // reset ahrs's trim to suggested values from calibration routine + ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); + } + } +#endif + result = MAV_RESULT_ACCEPTED; + break; + } + + case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + { + if (packet.param1 == 1 || packet.param1 == 3) { + // when packet.param1 == 3 we reboot to hold in bootloader + hal.scheduler->reboot(packet.param1 == 3); + result = MAV_RESULT_ACCEPTED; + } + break; + } + + default: + break; + } + mavlink_msg_command_ack_send( + chan, + packet.command, + result); + + break; + } + + // When mavproxy 'wp sethome' + case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: + { + // decode + mavlink_mission_write_partial_list_t packet; + mavlink_msg_mission_write_partial_list_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + if (packet.start_index == 0) + { + // New home at wp index 0. Ask for it + waypoint_receiving = true; + waypoint_request_i = 0; + waypoint_request_last = 0; + send_message(MSG_NEXT_WAYPOINT); + waypoint_receiving = true; + } + break; + } + + // XXX receive a WP from GCS and store in EEPROM if it is HOME + case MAVLINK_MSG_ID_MISSION_ITEM: + { + // decode + mavlink_mission_item_t packet; + uint8_t result = MAV_MISSION_ACCEPTED; + + mavlink_msg_mission_item_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + struct Location tell_command = {}; + + // defaults + tell_command.id = packet.command; + + switch (packet.frame) + { + case MAV_FRAME_MISSION: + case MAV_FRAME_GLOBAL: + { + tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z*1.0e2f; // in as m converted to cm + tell_command.options = 0; // absolute altitude + break; + } + +#ifdef MAV_FRAME_LOCAL_NED + case MAV_FRAME_LOCAL_NED: // local (relative to home position) + { + tell_command.lat = 1.0e7f*ToDeg(packet.x/ + (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; + tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; + tell_command.alt = -packet.z*1.0e2f; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + break; + } +#endif + +#ifdef MAV_FRAME_LOCAL + case MAV_FRAME_LOCAL: // local (relative to home position) + { + tell_command.lat = 1.0e7f*ToDeg(packet.x/ + (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; + tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; + tell_command.alt = packet.z*1.0e2f; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + break; + } +#endif + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude + { + tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z * 1.0e2f; + tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! + break; + } + + default: + result = MAV_MISSION_UNSUPPORTED_FRAME; + break; + } + + if (result != MAV_MISSION_ACCEPTED) goto mission_failed; + + // Check if receiving waypoints (mission upload expected) + if (!waypoint_receiving) { + result = MAV_MISSION_ERROR; + goto mission_failed; + } + + // check if this is the HOME wp + if (packet.seq == 0) { + set_home(tell_command); // New home in EEPROM + send_text_P(SEVERITY_LOW,PSTR("new HOME received")); + waypoint_receiving = false; + + } + + + +mission_failed: + // we are rejecting the mission/waypoint + mavlink_msg_mission_ack_send( + chan, + msg->sysid, + msg->compid, + result); + break; + } + + + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + { // decode mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(msg, &packet); diff --git a/Tools/AntennaTracker/Parameters.h b/Tools/AntennaTracker/Parameters.h index 89db93f5a6..2597bb8b9a 100644 --- a/Tools/AntennaTracker/Parameters.h +++ b/Tools/AntennaTracker/Parameters.h @@ -69,7 +69,12 @@ public: k_param_pidYaw2Srv, k_param_channel_yaw = 200, - k_param_channel_pitch + k_param_channel_pitch, + + // + // 220: Waypoint data + // + k_param_command_total = 220, // 254,255: reserved }; @@ -86,6 +91,10 @@ public: AP_Int8 compass_enabled; + // Waypoints + // + AP_Int8 command_total; // 1 if HOME is set + // PID controllers PID pidPitch2Srv; PID pidYaw2Srv; diff --git a/Tools/AntennaTracker/Parameters.pde b/Tools/AntennaTracker/Parameters.pde index 655f247226..6af1ce39fc 100644 --- a/Tools/AntennaTracker/Parameters.pde +++ b/Tools/AntennaTracker/Parameters.pde @@ -100,6 +100,14 @@ const AP_Param::Info var_info[] PROGMEM = { GGROUP(pidPitch2Srv, "PITCH2SRV_", PID), GGROUP(pidYaw2Srv, "YAW2SRV_", PID), + // @Param: CMD_TOTAL + // @DisplayName: Number of loaded mission items + // @Description: Set to 1 if HOME location has been loaded by the ground station. Do not change this manually. + // @Range: 1 255 + // @User: Advanced + GSCALAR(command_total, "CMD_TOTAL", 0), + + AP_VAREND }; diff --git a/Tools/AntennaTracker/config.h b/Tools/AntennaTracker/config.h index c34ccdaab7..48ed1b8e46 100644 --- a/Tools/AntennaTracker/config.h +++ b/Tools/AntennaTracker/config.h @@ -81,3 +81,13 @@ #ifndef SERIAL2_BUFSIZE # define SERIAL2_BUFSIZE 256 #endif + +////////////////////////////////////////////////////////////////////////////// +// Developer Items +// + +// use this to completely disable the CLI +#ifndef CLI_ENABLED + # define CLI_ENABLED ENABLED +#endif + diff --git a/Tools/AntennaTracker/defines.h b/Tools/AntennaTracker/defines.h index 9ac9bca658..56f5edb46a 100644 --- a/Tools/AntennaTracker/defines.h +++ b/Tools/AntennaTracker/defines.h @@ -30,4 +30,25 @@ #define AP_COMPASS_PX4 2 #define AP_COMPASS_HIL 3 +// In AntennaTracker, EEPROM is used to store waypoint 0 = HOME location, if available +// EEPROM addresses +#define EEPROM_MAX_ADDR 4096 +// parameters get the first 1280 bytes of EEPROM, remainder is for waypoints +#define WP_START_BYTE 0x500 // where in memory home WP is stored + all other + // WP +#define WP_SIZE 15 + +// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library +#define CMD_BLANK 0 // there is no command stored in the mem location requested +#define NO_COMMAND 0 +#define WAIT_COMMAND 255 + +// Command/Waypoint/Location Options Bitmask +//-------------------- +#define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative + // altitude +#define MASK_OPTIONS_LOITER_DIRECTION (1<<2) // 0 = CW + // 1 = CCW + #endif // _DEFINES_H + diff --git a/Tools/AntennaTracker/sensors.pde b/Tools/AntennaTracker/sensors.pde index b2773c97eb..3de3ab952e 100644 --- a/Tools/AntennaTracker/sensors.pde +++ b/Tools/AntennaTracker/sensors.pde @@ -61,5 +61,7 @@ static void barometer_accumulate(void) static void update_GPS(void) { g_gps->update(); + // REVISIT: add compass variation with compass.set_initial_location(g_gps->latitude, g_gps->longitude); + // when have a solid GPS fix. Also init_home(); } diff --git a/Tools/AntennaTracker/system.pde b/Tools/AntennaTracker/system.pde index 0a25ca1890..381fa70850 100644 --- a/Tools/AntennaTracker/system.pde +++ b/Tools/AntennaTracker/system.pde @@ -63,8 +63,8 @@ static void init_tracker() hal.uartC->set_blocking_writes(false); // setup antenna control PWM channels - channel_yaw.set_angle(4500); - channel_pitch.set_angle(4500); + channel_yaw.set_angle(18000); // Yaw is expected to drive antenna azimuth -180-0-180 + channel_pitch.set_angle(9000); // Pitch is expected to drive elevation -90-0-90 channel_yaw.output_trim(); channel_pitch.output_trim(); @@ -75,8 +75,24 @@ static void init_tracker() channel_yaw.enable_out(); channel_pitch.enable_out(); + home_loc = get_home_eeprom(); // GPS may update this later + gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track.")); - hal.scheduler->delay(1000); + hal.scheduler->delay(1000); // Why???? +} + +// Level the tracker by calibrating the INS +// Requires that the tracker be physically 'level' and horizontal +static void calibrate_ins() +{ + gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move tracker")); + ahrs.init(); + ahrs.set_fly_forward(true); + ins.init(AP_InertialSensor::COLD_START, ins_sample_rate); + ins.init_accel(); + ahrs.set_trim(Vector3f(0, 0, 0)); + ahrs.reset(); + init_barometer(); } // updates the status of the notify objects @@ -106,4 +122,72 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) return default_baud; } +/* + fetch HOME from EEPROM +*/ +static struct Location get_home_eeprom() +{ + struct Location temp; + uint16_t mem; + // Find out proper location in memory by using the start_byte position + the index + // -------------------------------------------------------------------------------- + if (g.command_total.get() == 0) { + memset(&temp, 0, sizeof(temp)); + temp.id = CMD_BLANK; + }else{ + // read WP position + mem = WP_START_BYTE; + temp.id = hal.storage->read_byte(mem); + + mem++; + temp.options = hal.storage->read_byte(mem); + + mem++; + temp.p1 = hal.storage->read_byte(mem); + + mem++; + temp.alt = hal.storage->read_dword(mem); + + mem += 4; + temp.lat = hal.storage->read_dword(mem); + + mem += 4; + temp.lng = hal.storage->read_dword(mem); + } + + return temp; +} + +static void set_home_eeprom(struct Location temp) +{ + uint16_t mem = WP_START_BYTE; + + hal.storage->write_byte(mem, temp.id); + + mem++; + hal.storage->write_byte(mem, temp.options); + + mem++; + hal.storage->write_byte(mem, temp.p1); + + mem++; + hal.storage->write_dword(mem, temp.alt); + + mem += 4; + hal.storage->write_dword(mem, temp.lat); + + mem += 4; + hal.storage->write_dword(mem, temp.lng); + + // Now have a home location in EEPROM + g.command_total.set_and_save(1); // At most 1 entry for HOME +} + +static void set_home(struct Location temp) +{ + if (g.compass_enabled) + compass.set_initial_location(temp.lat, temp.lng); + set_home_eeprom(temp); + home_loc = temp; +} diff --git a/Tools/AntennaTracker/tracking.pde b/Tools/AntennaTracker/tracking.pde index b8f0d7b6b1..6c03425f82 100644 --- a/Tools/AntennaTracker/tracking.pde +++ b/Tools/AntennaTracker/tracking.pde @@ -4,31 +4,149 @@ state of the vehicle we are tracking */ static struct { - Location location; + Location location; // lat, long in degrees * 10^7; alt in meters * 100 uint32_t last_update_us; float heading; // degrees float ground_speed; // m/s } vehicle; -static Location our_location; - /** - update the pitch servo. The aim is to drive the boards pitch to the - requested pitch + update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the + requested pitch, so the board (and therefore the antenna) will be pointing at the target */ static void update_pitch_servo(float pitch) { - channel_pitch.servo_out = g.pidPitch2Srv.get_pid_4500(degrees(ahrs.pitch) - pitch); + // degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal + // pitch argument is -90 to 90, where 0 is horizontal + // servo_out is in 100ths of a degree + float ahrs_pitch = degrees(ahrs.pitch); + int32_t err = (ahrs_pitch - pitch) * 100.0; + // Need to configure your servo so that increasing servo_out causes increase in pitch/elevation (ie pointing higher into the sky, + // above the horizon. On my antenna tracker this requires the pitch/elevation servo to be reversed + // param set RC2_REV -1 + // + // The pitch servo (RC channel 2) is configured for servo_out of -9000-0-9000 servo_out, + // which will drive the servo from RC2_MIN to RC2_MAX usec pulse width. + // Therefore, you must set RC2_MIN and RC2_MAX so that your servo drives the antenna altitude between -90 to 90 exactly + // To drive my HS-645MG servos through their full 180 degrees of rotational range, I have to set: + // param set RC2_MAX 2540 + // param set RC2_MIN 640 + // + // You will also need to tune the pitch PID to suit your antenna and servos. I use: + // PITCH2SRV_P 0.100000 + // PITCH2SRV_I 0.020000 + // PITCH2SRV_D 0.000000 + // PITCH2SRV_IMAX 4000.000000 + int32_t new_servo_out = channel_pitch.servo_out - g.pidPitch2Srv.get_pid(err); + channel_pitch.servo_out = constrain_float(new_servo_out, -9000, 9000); channel_pitch.calc_pwm(); channel_pitch.output(); } /** - update the yaw servo + update the yaw (azimuth) servo. The aim is to drive the boards ahrs yaw to the + requested yaw, so the board (and therefore the antenna) will be pinting at the target */ static void update_yaw_servo(float yaw) { - channel_yaw.servo_out = g.pidYaw2Srv.get_pid_4500(degrees(ahrs.yaw) - yaw); + // degrees(ahrs.yaw) is -180 to 180, where 0 is north + float ahrs_yaw = degrees(ahrs.yaw); + // yaw argument is 0 to 360 where 0 and 360 are north + // Make yaw -180-0-180 too + if (yaw > 180) + yaw = yaw - 360; + + // Antenna as Ballerina. Use with antenna that do not have continuously rotating servos, ie at some point in rotation + // the servo limits are reached and the servo has to slew 360 degrees to the 'other side' to keep tracking. + // + // This algorithm accounts for the fact that the antenna mount may not be aligned with North + // (in fact, any alignment is permissable), and that the alignment may change (possibly rapidly) over time + // (as when the antenna is mounted on a moving, turning vehicle) + // When the servo is being forced beyond its limits, it rapidly slews to the 'other side' then normal tracking takes over. + // + // Caution: this whole system is compromised by the fact that the ahrs_yaw reported by the compass system lags + // the actual yaw by about 5 seconds, and also periodically realigns itself with about a 30 second period, + // which makes it very hard to be completely sure exactly where the antenna is _really_ pointing + // especially during the high speed slew. This can cause odd pointing artifacts from time to time. The best strategy is to + // position and point the mount so the aircraft does not 'go behind' the antenna (if possible) + // + // With my antenna mount, large pwm output drives the antenna anticlockise, so need: + // param set RC1_REV -1 + // to reverse the servo. Yours may be different + // + // You MUST set RC1_MIN and RC1_MAX so that your servo drives the antenna azimuth from -180 to 180 relative + // to the mount. + // To drive my HS-645MG servos through their full 180 degrees of rotational range and therefore the + // antenna through a full 360 degrees, I have to set: + // param set RC1_MAX 2380 + // param set RC1_MIN 680 + // According to the specs at https://www.servocity.com/html/hs-645mg_ultra_torque.html, + // that should be 600 through 2400, but the azimuth gearing in my antenna pointer is not exactly 2:1 + int32_t err = (ahrs_yaw - yaw) * 100.0; + static int32_t last_err = 0.0; + + // Correct for wrapping yaw at +-180 + // so we get the error to the _closest_ version of the target azimuth + // +ve error requires anticlockwise motion (ie towards more negative yaw) + if (err > 18000) + err -= 36000; + else if (err < -18000) + err += 36000; + + static int32_t slew_to = 0; + + int32_t servo_err = channel_yaw.servo_out - err; // Servo position we need to get to + if ( !slew_to + && servo_err > 19000 // 10 degreee deadband + && err < 0 + && last_err > err) + { + // We need to be beyond the servo limits and the error magnitude is increasing + // Slew most of the way to the other side at high speed... + slew_to = servo_err - 27000; + } + else if ( !slew_to + && servo_err < -19000 // 10 degreee deadband + && err > 0 + && last_err < err) + { + // We need to be beyond the servo limits and the error magnitude is increasing + // Slew most of the way to the other side at high speed... + slew_to = servo_err + 27000; + } + else if ( slew_to < 0 + && err > 0 + && last_err < err) + { + // ...then let normal tracking take over + slew_to = 0; + } + else if ( slew_to > 0 + && err < 0 + && last_err > err) + { + // ...then let normal tracking take over + slew_to = 0; + } + + if (slew_to) + { + channel_yaw.servo_out = slew_to; + } + else + { + // Normal tracking + // You will need to tune the yaw PID to suit your antenna and servos + // For my servos, suitable PID settings are: + // param set YAW2SRV_P 0.1 + // param set YAW2SRV_I 0.05 + // param set YAW2SRV_D 0 + // param set YAW2SRV_IMAX 4000 + + int32_t new_servo_out = channel_yaw.servo_out - g.pidYaw2Srv.get_pid(err); + channel_yaw.servo_out = constrain_float(new_servo_out, -18000, 18000); + } + last_err = err; channel_yaw.calc_pwm(); channel_yaw.output(); } @@ -46,16 +164,17 @@ static void update_tracking(void) // update our position if we have at least a 2D fix if (g_gps->status() >= GPS::GPS_OK_FIX_2D) { - our_location.lat = g_gps->latitude; - our_location.lng = g_gps->longitude; - our_location.alt = 0; // assume ground level for now + current_loc.lat = g_gps->latitude; + current_loc.lng = g_gps->longitude; + current_loc.alt = 0; // assume ground level for now REVISIT: WHY? + } + else { + current_loc = home_loc; // dont know any better } - // calculate the bearing to the vehicle - float bearing = get_bearing_cd(our_location, vehicle.location) * 0.01f; - float distance = get_distance(our_location, vehicle.location); - float pitch = degrees(atan2(vehicle.location.alt - our_location.alt, distance)); - + float bearing = get_bearing_cd(current_loc, vehicle.location) * 0.01f; + float distance = get_distance(current_loc, vehicle.location); + float pitch = degrees(atan2((vehicle.location.alt - current_loc.alt)/100, distance)); // update the servos update_pitch_servo(pitch); update_yaw_servo(bearing); @@ -79,3 +198,4 @@ static void tracking_update_position(const mavlink_global_position_int_t &msg) vehicle.ground_speed = pythagorous2(msg.vx, msg.vy) * 0.01f; vehicle.last_update_us = hal.scheduler->micros(); } +