diff --git a/flight/Libraries/paths.c b/flight/Libraries/paths.c index d9b7fc389..8e6b63c9e 100644 --- a/flight/Libraries/paths.c +++ b/flight/Libraries/paths.c @@ -32,8 +32,8 @@ // no direct UAVObject usage allowed in this file // private functions -static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status); -static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status); +static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status); +static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status); static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise); /** @@ -46,26 +46,26 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin */ void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode) { - switch(mode) { - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - return path_vector(start_point, end_point, cur_point, status); - break; - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - return path_circle(start_point, end_point, cur_point, status, 1); - break; - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - return path_circle(start_point, end_point, cur_point, status, 0); - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - default: - // use the endpoint as default failsafe if called in unknown modes - return path_endpoint(start_point, end_point, cur_point, status); - break; - } + switch (mode) { + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + return path_vector(start_point, end_point, cur_point, status); + break; + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + return path_circle(start_point, end_point, cur_point, status, 1); + break; + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + return path_circle(start_point, end_point, cur_point, status, 0); + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + default: + // use the endpoint as default failsafe if called in unknown modes + return path_endpoint(start_point, end_point, cur_point, status); + break; + } } /** @@ -75,38 +75,38 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status) +static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status) { - float path_north, path_east, diff_north, diff_east; - float dist_path, dist_diff; + float path_north, path_east, diff_north, diff_east; + float dist_path, dist_diff; - // we do not correct in this mode - status->correction_direction[0] = status->correction_direction[1] = 0; + // we do not correct in this mode + status->correction_direction[0] = status->correction_direction[1] = 0; - // Distance to go - path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + // Distance to go + path_north = end_point[0] - start_point[0]; + path_east = end_point[1] - start_point[1]; - // Current progress location relative to end - diff_north = end_point[0] - cur_point[0]; - diff_east = end_point[1] - cur_point[1]; + // Current progress location relative to end + diff_north = end_point[0] - cur_point[0]; + diff_east = end_point[1] - cur_point[1]; - dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east ); - dist_path = sqrtf( path_north * path_north + path_east * path_east ); + dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east); + dist_path = sqrtf(path_north * path_north + path_east * path_east); - if(dist_diff < 1e-6 ) { - status->fractional_progress = 1; - status->error = 0; - status->path_direction[0] = status->path_direction[1] = 0; - return; - } + if (dist_diff < 1e-6) { + status->fractional_progress = 1; + status->error = 0; + status->path_direction[0] = status->path_direction[1] = 0; + return; + } - status->fractional_progress = 1 - dist_diff / (1 + dist_path); - status->error = dist_diff; + status->fractional_progress = 1 - dist_diff / (1 + dist_path); + status->error = dist_diff; - // Compute direction to travel - status->path_direction[0] = diff_north / dist_diff; - status->path_direction[1] = diff_east / dist_diff; + // Compute direction to travel + status->path_direction[0] = diff_north / dist_diff; + status->path_direction[1] = diff_east / dist_diff; } @@ -117,50 +117,50 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status) +static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status) { - float path_north, path_east, diff_north, diff_east; - float dist_path; - float dot; - float normal[2]; + float path_north, path_east, diff_north, diff_east; + float dist_path; + float dot; + float normal[2]; - // Distance to go - path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + // Distance to go + path_north = end_point[0] - start_point[0]; + path_east = end_point[1] - start_point[1]; - // Current progress location relative to start - diff_north = cur_point[0] - start_point[0]; - diff_east = cur_point[1] - start_point[1]; + // Current progress location relative to start + diff_north = cur_point[0] - start_point[0]; + diff_east = cur_point[1] - start_point[1]; - dot = path_north * diff_north + path_east * diff_east; - dist_path = sqrtf( path_north * path_north + path_east * path_east ); + dot = path_north * diff_north + path_east * diff_east; + dist_path = sqrtf(path_north * path_north + path_east * path_east); - if(dist_path < 1e-6) { - // if the path is too short, we cannot determine vector direction. - // Fly towards the endpoint to prevent flying away, - // but assume progress=1 either way. - path_endpoint( start_point, end_point, cur_point, status ); - status->fractional_progress = 1; - return; - } + if (dist_path < 1e-6) { + // if the path is too short, we cannot determine vector direction. + // Fly towards the endpoint to prevent flying away, + // but assume progress=1 either way. + path_endpoint(start_point, end_point, cur_point, status); + status->fractional_progress = 1; + return; + } - // Compute the normal to the path - normal[0] = -path_east / dist_path; - normal[1] = path_north / dist_path; + // Compute the normal to the path + normal[0] = -path_east / dist_path; + normal[1] = path_north / dist_path; - status->fractional_progress = dot / (dist_path * dist_path); - status->error = normal[0] * diff_north + normal[1] * diff_east; + status->fractional_progress = dot / (dist_path * dist_path); + status->error = normal[0] * diff_north + normal[1] * diff_east; - // Compute direction to correct error - status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0]; - status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1]; - - // Now just want magnitude of error - status->error = fabs(status->error); + // Compute direction to correct error + status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0]; + status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1]; - // Compute direction to travel - status->path_direction[0] = path_north / dist_path; - status->path_direction[1] = path_east / dist_path; + // Now just want magnitude of error + status->error = fabs(status->error); + + // Compute direction to travel + status->path_direction[0] = path_north / dist_path; + status->path_direction[1] = path_east / dist_path; } @@ -173,77 +173,79 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi */ static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise) { - float radius_north, radius_east, diff_north, diff_east; - float radius,cradius; - float normal[2]; - float progress; - float a_diff, a_radius; + float radius_north, radius_east, diff_north, diff_east; + float radius, cradius; + float normal[2]; + float progress; + float a_diff, a_radius; - // Radius - radius_north = end_point[0] - start_point[0]; - radius_east = end_point[1] - start_point[1]; + // Radius + radius_north = end_point[0] - start_point[0]; + radius_east = end_point[1] - start_point[1]; - // Current location relative to center - diff_north = cur_point[0] - end_point[0]; - diff_east = cur_point[1] - end_point[1]; + // Current location relative to center + diff_north = cur_point[0] - end_point[0]; + diff_east = cur_point[1] - end_point[1]; - radius = sqrtf( powf(radius_north,2) + powf(radius_east,2) ); - cradius = sqrtf( powf(diff_north,2) + powf(diff_east,2) ); + radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2)); + cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2)); - if (cradius < 1e-6) { - // cradius is zero, just fly somewhere and make sure correction is still a normal - status->fractional_progress = 1; - status->error = radius; - status->correction_direction[0] = 0; - status->correction_direction[1] = 1; - status->path_direction[0] = 1; - status->path_direction[1] = 0; - return; - } + if (cradius < 1e-6) { + // cradius is zero, just fly somewhere and make sure correction is still a normal + status->fractional_progress = 1; + status->error = radius; + status->correction_direction[0] = 0; + status->correction_direction[1] = 1; + status->path_direction[0] = 1; + status->path_direction[1] = 0; + return; + } - if (clockwise) { - // Compute the normal to the radius clockwise - normal[0] = -diff_east / cradius; - normal[1] = diff_north / cradius; - } else { - // Compute the normal to the radius counter clockwise - normal[0] = diff_east / cradius; - normal[1] = -diff_north / cradius; - } - + if (clockwise) { + // Compute the normal to the radius clockwise + normal[0] = -diff_east / cradius; + normal[1] = diff_north / cradius; + } else { + // Compute the normal to the radius counter clockwise + normal[0] = diff_east / cradius; + normal[1] = -diff_north / cradius; + } - // normalize progress to 0..1 - a_diff = atan2f( diff_north, diff_east); - a_radius = atan2f( radius_north, radius_east); - - if(a_diff<0) - a_diff+=2*M_PI; - if(a_radius<0) - a_radius+=2*M_PI; - - progress = (a_diff - a_radius + M_PI) / (2 * M_PI); + // normalize progress to 0..1 + a_diff = atan2f(diff_north, diff_east); + a_radius = atan2f(radius_north, radius_east); - if(progress<0) - progress+=1.0; - else if(progress>=1) - progress-=1.0; + if (a_diff < 0) { + a_diff += 2 * M_PI; + } + if (a_radius < 0) { + a_radius += 2 * M_PI; + } - if(clockwise) - { - progress=1-progress; - } - status->fractional_progress = progress; + progress = (a_diff - a_radius + M_PI) / (2 * M_PI); - // error is current radius minus wanted radius - positive if too close - status->error = radius - cradius; + if (progress < 0) { + progress += 1.0; + } else if (progress >= 1) { + progress -= 1.0; + } - // Compute direction to correct error - status->correction_direction[0] = (status->error>0?1:-1) * diff_north / cradius; - status->correction_direction[1] = (status->error>0?1:-1) * diff_east / cradius; + if (clockwise) { + progress = 1 - progress; + } - // Compute direction to travel - status->path_direction[0] = normal[0]; - status->path_direction[1] = normal[1]; + status->fractional_progress = progress; - status->error = fabs(status->error); + // error is current radius minus wanted radius - positive if too close + status->error = radius - cradius; + + // Compute direction to correct error + status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius; + status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius; + + // Compute direction to travel + status->path_direction[0] = normal[0]; + status->path_direction[1] = normal[1]; + + status->error = fabs(status->error); } diff --git a/flight/Modules/Altitude/altitude.c b/flight/Modules/Altitude/altitude.c index 21295c717..04c2ac48c 100644 --- a/flight/Modules/Altitude/altitude.c +++ b/flight/Modules/Altitude/altitude.c @@ -131,8 +131,8 @@ static void altitudeTask(void *parameters) #if defined(PIOS_INCLUDE_HCSR04) SonarAltitudeData sonardata; - int32_t value=0,timeout=5; - float coeff=0.25,height_out=0,height_in=0; + int32_t value = 0, timeout = 5; + float coeff = 0.25, height_out = 0, height_in = 0; if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) { PIOS_HCSR04_Trigger(); } @@ -149,25 +149,23 @@ static void altitudeTask(void *parameters) #if defined(PIOS_INCLUDE_HCSR04) // Compute the current altitude if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) { - if(PIOS_HCSR04_Completed()) - { + if(PIOS_HCSR04_Completed()) { value = PIOS_HCSR04_Get(); - if((value>100) && (value < 15000)) //from 3.4cm to 5.1m - { - height_in = value*0.00034f/2.0f; + //from 3.4cm to 5.1m + if((value > 100) && (value < 15000)) { + height_in = value * 0.00034f / 2.0f; height_out = (height_out * (1 - coeff)) + (height_in * coeff); sonardata.Altitude = height_out; // m/us } // Update the AltitudeActual UAVObject SonarAltitudeSet(&sonardata); - timeout=5; + timeout = 5; PIOS_HCSR04_Trigger(); } - if(!(timeout--)) - { + if(!(timeout--)) { //retrigger - timeout=5; + timeout = 5; PIOS_HCSR04_Trigger(); } } diff --git a/flight/Modules/Altitude/revolution/altitude.c b/flight/Modules/Altitude/revolution/altitude.c index bd59e5baa..1cee36418 100644 --- a/flight/Modules/Altitude/revolution/altitude.c +++ b/flight/Modules/Altitude/revolution/altitude.c @@ -90,8 +90,8 @@ static void altitudeTask(void *parameters) #if defined(PIOS_INCLUDE_HCSR04) SonarAltitudeData sonardata; - int32_t value=0,timeout=10,sample_rate=0; - float coeff=0.25,height_out=0,height_in=0; + int32_t value = 0, timeout = 10, sample_rate = 0; + float coeff = 0.25, height_out = 0, height_in = 0; PIOS_HCSR04_Trigger(); #endif @@ -110,30 +110,27 @@ static void altitudeTask(void *parameters) #if defined(PIOS_INCLUDE_HCSR04) // Compute the current altitude // depends on baro samplerate - if(!(sample_rate--)) - { - if(PIOS_HCSR04_Completed()) - { + if(!(sample_rate--)) { + if(PIOS_HCSR04_Completed()) { value = PIOS_HCSR04_Get(); - if((value>100) && (value < 15000)) //from 3.4cm to 5.1m - { - height_in = value*0.00034f/2.0f; + //from 3.4cm to 5.1m + if((value > 100) && (value < 15000)) { + height_in = value * 0.00034f / 2.0f; height_out = (height_out * (1 - coeff)) + (height_in * coeff); sonardata.Altitude = height_out; // m/us } // Update the SonarAltitude UAVObject SonarAltitudeSet(&sonardata); - timeout=10; + timeout = 10; PIOS_HCSR04_Trigger(); } - if(!(timeout--)) - { + if(!(timeout--)) { //retrigger - timeout=10; + timeout = 10; PIOS_HCSR04_Trigger(); } - sample_rate=25; + sample_rate = 25; } #endif float temp, press; diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index ef2043430..479ea221c 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -73,11 +73,7 @@ // Private types typedef enum { - ARM_STATE_DISARMED, - ARM_STATE_ARMING_MANUAL, - ARM_STATE_ARMED, - ARM_STATE_DISARMING_MANUAL, - ARM_STATE_DISARMING_TIMEOUT + ARM_STATE_DISARMED, ARM_STATE_ARMING_MANUAL, ARM_STATE_ARMED, ARM_STATE_DISARMING_MANUAL, ARM_STATE_DISARMING_TIMEOUT } ArmState_t; // Private variables @@ -114,10 +110,11 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 #define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 -struct rcvr_activity_fsm { - ManualControlSettingsChannelGroupsOptions group; - uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; - uint8_t sample_count; +struct rcvr_activity_fsm +{ + ManualControlSettingsChannelGroupsOptions group; + uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; + uint8_t sample_count; }; static struct rcvr_activity_fsm activity_fsm; @@ -131,12 +128,12 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm); */ int32_t ManualControlStart() { - // Start main task - xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); + // Start main task + xTaskCreate(manualControlTask, (signed char *) "ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); - return 0; + return 0; } /** @@ -145,555 +142,552 @@ int32_t ManualControlStart() int32_t ManualControlInitialize() { - /* Check the assumptions about uavobject enum's are correct */ - if(!assumptions) - return -1; + /* Check the assumptions about uavobject enum's are correct */ + if (!assumptions) + return -1; - AccessoryDesiredInitialize(); - ManualControlCommandInitialize(); - FlightStatusInitialize(); - StabilizationDesiredInitialize(); - ReceiverActivityInitialize(); - ManualControlSettingsInitialize(); + AccessoryDesiredInitialize(); + ManualControlCommandInitialize(); + FlightStatusInitialize(); + StabilizationDesiredInitialize(); + ReceiverActivityInitialize(); + ManualControlSettingsInitialize(); - return 0; + return 0; } -MODULE_INITCALL(ManualControlInitialize, ManualControlStart) +MODULE_INITCALL( ManualControlInitialize, ManualControlStart) /** * Module task */ static void manualControlTask(void *parameters) { - ManualControlSettingsData settings; - ManualControlCommandData cmd; - FlightStatusData flightStatus; - float flightMode = 0; + ManualControlSettingsData settings; + ManualControlCommandData cmd; + FlightStatusData flightStatus; + float flightMode = 0; - uint8_t disconnected_count = 0; - uint8_t connected_count = 0; + uint8_t disconnected_count = 0; + uint8_t connected_count = 0; - // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically - // this includes not even registering it if not used - AccessoryDesiredCreateInstance(); - AccessoryDesiredCreateInstance(); + // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically + // this includes not even registering it if not used + AccessoryDesiredCreateInstance(); + AccessoryDesiredCreateInstance(); - // Run this initially to make sure the configuration is checked - configuration_check(); - - // Whenever the configuration changes, make sure it is safe to fly - SystemSettingsConnectCallback(configurationUpdatedCb); - ManualControlSettingsConnectCallback(configurationUpdatedCb); - - // Whenever the configuration changes, make sure it is safe to fly + // Run this initially to make sure the configuration is checked + configuration_check(); - // Make sure unarmed on power up - ManualControlCommandGet(&cmd); - FlightStatusGet(&flightStatus); - flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; - armState = ARM_STATE_DISARMED; + // Whenever the configuration changes, make sure it is safe to fly + SystemSettingsConnectCallback(configurationUpdatedCb); + ManualControlSettingsConnectCallback(configurationUpdatedCb); - /* Initialize the RcvrActivty FSM */ - portTickType lastActivityTime = xTaskGetTickCount(); - resetRcvrActivity(&activity_fsm); + // Whenever the configuration changes, make sure it is safe to fly - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) { - float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM]; + // Make sure unarmed on power up + ManualControlCommandGet(&cmd); + FlightStatusGet(&flightStatus); + flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; + armState = ARM_STATE_DISARMED; - // Wait until next update - vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); - PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); + /* Initialize the RcvrActivty FSM */ + portTickType lastActivityTime = xTaskGetTickCount(); + resetRcvrActivity(&activity_fsm); - // Read settings - ManualControlSettingsGet(&settings); + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { + float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM]; - /* Update channel activity monitor */ - if (flightStatus.Armed == ARM_STATE_DISARMED) { - if (updateRcvrActivity(&activity_fsm)) { - /* Reset the aging timer because activity was detected */ - lastActivityTime = lastSysTime; - } - } - if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) { - resetRcvrActivity(&activity_fsm); - lastActivityTime = lastSysTime; - } + // Wait until next update + vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); + PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); - if (ManualControlCommandReadOnly()) { - FlightTelemetryStatsData flightTelemStats; - FlightTelemetryStatsGet(&flightTelemStats); - if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - /* trying to fly via GCS and lost connection. fall back to transmitter */ - UAVObjMetadata metadata; - ManualControlCommandGetMetadata(&metadata); - UAVObjSetAccess(&metadata, ACCESS_READWRITE); - ManualControlCommandSetMetadata(&metadata); - } - } + // Read settings + ManualControlSettingsGet(&settings); - if (!ManualControlCommandReadOnly()) { + /* Update channel activity monitor */ + if (flightStatus.Armed == ARM_STATE_DISARMED) { + if (updateRcvrActivity(&activity_fsm)) { + /* Reset the aging timer because activity was detected */ + lastActivityTime = lastSysTime; + } + } + if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) { + resetRcvrActivity(&activity_fsm); + lastActivityTime = lastSysTime; + } - bool valid_input_detected = true; - - // Read channel values in us - for (uint8_t n = 0; - n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; - ++n) { - extern uint32_t pios_rcvr_group_map[]; + if (ManualControlCommandReadOnly()) { + FlightTelemetryStatsData flightTelemStats; + FlightTelemetryStatsGet(&flightTelemStats); + if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { + /* trying to fly via GCS and lost connection. fall back to transmitter */ + UAVObjMetadata metadata; + ManualControlCommandGetMetadata(&metadata); + UAVObjSetAccess(&metadata, ACCESS_READWRITE); + ManualControlCommandSetMetadata(&metadata); + } + } - if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - cmd.Channel[n] = PIOS_RCVR_INVALID; - } else { - cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], - settings.ChannelNumber[n]); - } - - // If a channel has timed out this is not valid data and we shouldn't update anything - // until we decide to go to failsafe - if(cmd.Channel[n] == PIOS_RCVR_TIMEOUT) - valid_input_detected = false; - else - scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); - } + if (!ManualControlCommandReadOnly()) { - // Check settings, if error raise alarm - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || - settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || - settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || - settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || - // Check all channel mappings are valid - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID || - // Check the driver exists - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER || - // Check the FlightModeNumber is valid - settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || - // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care - ((settings.FlightModeNumber > 1) && ( - settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) { + bool valid_input_detected = true; - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; - ManualControlCommandSet(&cmd); + // Read channel values in us + for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { + extern uint32_t pios_rcvr_group_map[]; - // Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config) - // immediately disarm - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + cmd.Channel[n] = PIOS_RCVR_INVALID; + } else { + cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], settings.ChannelNumber[n]); + } - continue; - } + // If a channel has timed out this is not valid data and we shouldn't update anything + // until we decide to go to failsafe + if (cmd.Channel[n] == PIOS_RCVR_TIMEOUT) + valid_input_detected = false; + else + scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); + } - // decide if we have valid manual input or not - valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) && - validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) && - validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) && - validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); + // Check settings, if error raise alarm + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || + // Check all channel mappings are valid + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID + || + // Check the driver exists + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER || + // Check the FlightModeNumber is valid + settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || + // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care + ((settings.FlightModeNumber > 1) + && (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) { - // Implement hysteresis loop on connection status - if (valid_input_detected && (++connected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; - connected_count = 0; - disconnected_count = 0; - } else if (!valid_input_detected && (++disconnected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; - connected_count = 0; - disconnected_count = 0; - } + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + ManualControlCommandSet(&cmd); - if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - cmd.Throttle = -1; // Shut down engine with no control - cmd.Roll = 0; - cmd.Yaw = 0; - cmd.Pitch = 0; - cmd.Collective = 0; - //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning - // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, - // or leave throttle at IDLE speed or above when going into AUTO-failsafe. - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - - AccessoryDesiredData accessory; - // Set Accessory 0 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != - MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = 0; - if(AccessoryDesiredInstSet(0, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - // Set Accessory 1 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != - MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = 0; - if(AccessoryDesiredInstSet(1, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - // Set Accessory 2 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != - MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = 0; - if(AccessoryDesiredInstSet(2, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } + // Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config) + // immediately disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - } else if (valid_input_detected) { - AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); + continue; + } - // Scale channels to -1 -> +1 range - cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; - cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; - cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; - cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; - flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; + // decide if we have valid manual input or not + valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); - // Apply deadband for Roll/Pitch/Yaw stick inputs - if (settings.Deadband) { - applyDeadband(&cmd.Roll, settings.Deadband); - applyDeadband(&cmd.Pitch, settings.Deadband); - applyDeadband(&cmd.Yaw, settings.Deadband); - } + // Implement hysteresis loop on connection status + if (valid_input_detected && (++connected_count > 10)) { + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; + connected_count = 0; + disconnected_count = 0; + } else if (!valid_input_detected && (++disconnected_count > 10)) { + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + connected_count = 0; + disconnected_count = 0; + } + + if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { + cmd.Throttle = -1; // Shut down engine with no control + cmd.Roll = 0; + cmd.Yaw = 0; + cmd.Pitch = 0; + cmd.Collective = 0; + //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning + // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, + // or leave throttle at IDLE speed or above when going into AUTO-failsafe. + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + + AccessoryDesiredData accessory; + // Set Accessory 0 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = 0; + if (AccessoryDesiredInstSet(0, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accessory 1 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = 0; + if (AccessoryDesiredInstSet(1, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accessory 2 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = 0; + if (AccessoryDesiredInstSet(2, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + + } else if (valid_input_detected) { + AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); + + // Scale channels to -1 -> +1 range + cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; + cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; + cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; + cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; + flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; + + // Apply deadband for Roll/Pitch/Yaw stick inputs + if (settings.Deadband) { + applyDeadband(&cmd.Roll, settings.Deadband); + applyDeadband(&cmd.Pitch, settings.Deadband); + applyDeadband(&cmd.Yaw, settings.Deadband); + } #ifdef USE_INPUT_LPF - // Apply Low Pass Filter to input channels, time delta between calls in ms - portTickType thisSysTime = xTaskGetTickCount(); - float dT = (thisSysTime > lastSysTimeLPF) ? - (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : - (float)UPDATE_PERIOD_MS; - lastSysTimeLPF = thisSysTime; + // Apply Low Pass Filter to input channels, time delta between calls in ms + portTickType thisSysTime = xTaskGetTickCount(); + float dT = (thisSysTime > lastSysTimeLPF) ? + (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : + (float)UPDATE_PERIOD_MS; + lastSysTimeLPF = thisSysTime; - applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); - applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); - applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); + applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); + applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); + applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); #endif // USE_INPUT_LPF - if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID && - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER && - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT) - cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; - - AccessoryDesiredData accessory; - // Set Accessory 0 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != - MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; -#ifdef USE_INPUT_LPF - applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); -#endif - if(AccessoryDesiredInstSet(0, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - // Set Accessory 1 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != - MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; -#ifdef USE_INPUT_LPF - applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); -#endif - if(AccessoryDesiredInstSet(1, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - // Set Accessory 2 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != - MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; -#ifdef USE_INPUT_LPF - applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); -#endif - if(AccessoryDesiredInstSet(2, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } + if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID + && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER + && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT) + cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; - processFlightMode(&settings, flightMode); + AccessoryDesiredData accessory; + // Set Accessory 0 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); +#endif + if (AccessoryDesiredInstSet(0, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accessory 1 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); +#endif + if (AccessoryDesiredInstSet(1, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accessory 2 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); +#endif + if (AccessoryDesiredInstSet(2, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } - } + processFlightMode(&settings, flightMode); - // Process arming outside conditional so system will disarm when disconnected - processArm(&cmd, &settings); - - // Update cmd object - ManualControlCommandSet(&cmd); + } + + // Process arming outside conditional so system will disarm when disconnected + processArm(&cmd, &settings); + + // Update cmd object + ManualControlCommandSet(&cmd); #if defined(PIOS_INCLUDE_USB_RCTX) - if (pios_usb_rctx_id) { - PIOS_USB_RCTX_Update(pios_usb_rctx_id, - cmd.Channel, - settings.ChannelMin, - settings.ChannelMax, - NELEMENTS(cmd.Channel)); - } + if (pios_usb_rctx_id) { + PIOS_USB_RCTX_Update(pios_usb_rctx_id, + cmd.Channel, + settings.ChannelMin, + settings.ChannelMax, + NELEMENTS(cmd.Channel)); + } #endif /* PIOS_INCLUDE_USB_RCTX */ - } else { - ManualControlCommandGet(&cmd); /* Under GCS control */ - } + } else { + ManualControlCommandGet(&cmd); /* Under GCS control */ + } - FlightStatusGet(&flightStatus); + FlightStatusGet(&flightStatus); - // Depending on the mode update the Stabilization or Actuator objects - static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; - switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { - case FLIGHTMODE_UNDEFINED: - // This reflects a bug in the code architecture! - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - break; - case FLIGHTMODE_MANUAL: - updateActuatorDesired(&cmd); - break; - case FLIGHTMODE_STABILIZED: - updateStabilizationDesired(&cmd, &settings); - break; - case FLIGHTMODE_TUNING: - // Tuning takes settings directly from manualcontrolcommand. No need to - // call anything else. This just avoids errors. - break; - case FLIGHTMODE_GUIDANCE: - switch(flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: - altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode); - break; - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_POI: - updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false); - break; - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true); - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - // No need to call anything. This just avoids errors. - break; - case FLIGHTSTATUS_FLIGHTMODE_LAND: - updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode); - break; - default: - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - } - break; - } - lastFlightMode = flightStatus.FlightMode; - } + // Depending on the mode update the Stabilization or Actuator objects + static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; + switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { + case FLIGHTMODE_UNDEFINED: + // This reflects a bug in the code architecture! + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + break; + case FLIGHTMODE_MANUAL: + updateActuatorDesired(&cmd); + break; + case FLIGHTMODE_STABILIZED: + updateStabilizationDesired(&cmd, &settings); + break; + case FLIGHTMODE_TUNING: + // Tuning takes settings directly from manualcontrolcommand. No need to + // call anything else. This just avoids errors. + break; + case FLIGHTMODE_GUIDANCE: + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: + altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode); + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_POI: + updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false); + break; + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true); + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + // No need to call anything. This just avoids errors. + break; + case FLIGHTSTATUS_FLIGHTMODE_LAND: + updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode); + break; + default: + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + } + break; + } + lastFlightMode = flightStatus.FlightMode; + } } static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) { - ReceiverActivityData data; - bool updated = false; + ReceiverActivityData data; + bool updated = false; - /* Clear all channel activity flags */ - ReceiverActivityGet(&data); - if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && - data.ActiveChannel != 255) { - data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; - data.ActiveChannel = 255; - updated = true; - } - if (updated) { - ReceiverActivitySet(&data); - } + /* Clear all channel activity flags */ + ReceiverActivityGet(&data); + if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) { + data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; + data.ActiveChannel = 255; + updated = true; + } + if (updated) { + ReceiverActivitySet(&data); + } - /* Reset the FSM state */ - fsm->group = 0; - fsm->sample_count = 0; + /* Reset the FSM state */ + fsm->group = 0; + fsm->sample_count = 0; } -static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) { - for (uint8_t channel = 1; channel <= max_channels; channel++) { - // Subtract 1 because channels are 1 indexed - samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel); - } +static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) +{ + for (uint8_t channel = 1; channel <= max_channels; channel++) { + // Subtract 1 because channels are 1 indexed + samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel); + } } static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm * fsm) { - bool activity_updated = false; + bool activity_updated = false; - /* Compare the current value to the previous sampled value */ - for (uint8_t channel = 1; - channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; - channel++) { - uint16_t delta; - uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed - uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); - if (curr > prev) { - delta = curr - prev; - } else { - delta = prev - curr; - } + /* Compare the current value to the previous sampled value */ + for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) { + uint16_t delta; + uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed + uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); + if (curr > prev) { + delta = curr - prev; + } else { + delta = prev - curr; + } - if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { - /* Mark this channel as active */ - ReceiverActivityActiveGroupOptions group; + if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { + /* Mark this channel as active */ + ReceiverActivityActiveGroupOptions group; - /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ - switch (fsm->group) { - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: - group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: - group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: - group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; - break; - default: - PIOS_Assert(0); - break; - } + /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ + switch (fsm->group) { + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: + group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: + group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: + group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; + break; + default: + PIOS_Assert(0); + break; + } - ReceiverActivityActiveGroupSet((uint8_t*)&group); - ReceiverActivityActiveChannelSet(&channel); - activity_updated = true; - } - } - return (activity_updated); + ReceiverActivityActiveGroupSet((uint8_t*) &group); + ReceiverActivityActiveChannelSet(&channel); + activity_updated = true; + } + } + return (activity_updated); } static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) { - bool activity_updated = false; + bool activity_updated = false; - if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - /* We're out of range, reset things */ - resetRcvrActivity(fsm); - } + if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + /* We're out of range, reset things */ + resetRcvrActivity(fsm); + } - extern uint32_t pios_rcvr_group_map[]; - if (!pios_rcvr_group_map[fsm->group]) { - /* Unbound group, skip it */ - goto group_completed; - } + extern uint32_t pios_rcvr_group_map[]; + if (!pios_rcvr_group_map[fsm->group]) { + /* Unbound group, skip it */ + goto group_completed; + } - if (fsm->sample_count == 0) { - /* Take a sample of each channel in this group */ - updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], - fsm->prev, - NELEMENTS(fsm->prev)); - fsm->sample_count++; - return (false); - } + if (fsm->sample_count == 0) { + /* Take a sample of each channel in this group */ + updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev)); + fsm->sample_count++; + return (false); + } - /* Compare with previous sample */ - activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm); + /* Compare with previous sample */ + activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm); -group_completed: - /* Reset the sample counter */ - fsm->sample_count = 0; + group_completed: + /* Reset the sample counter */ + fsm->sample_count = 0; - /* Find the next active group, but limit search so we can't loop forever here */ - for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) { - /* Move to the next group */ - fsm->group++; - if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - /* Wrap back to the first group */ - fsm->group = 0; - } - if (pios_rcvr_group_map[fsm->group]) { - /* - * Found an active group, take a sample here to avoid an - * extra 20ms delay in the main thread so we can speed up - * this algorithm. - */ - updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], - fsm->prev, - NELEMENTS(fsm->prev)); - fsm->sample_count++; - break; - } - } + /* Find the next active group, but limit search so we can't loop forever here */ + for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) { + /* Move to the next group */ + fsm->group++; + if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + /* Wrap back to the first group */ + fsm->group = 0; + } + if (pios_rcvr_group_map[fsm->group]) { + /* + * Found an active group, take a sample here to avoid an + * extra 20ms delay in the main thread so we can speed up + * this algorithm. + */ + updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev)); + fsm->sample_count++; + break; + } + } - return (activity_updated); + return (activity_updated); } static void updateActuatorDesired(ManualControlCommandData * cmd) { - ActuatorDesiredData actuator; - ActuatorDesiredGet(&actuator); - actuator.Roll = cmd->Roll; - actuator.Pitch = cmd->Pitch; - actuator.Yaw = cmd->Yaw; - actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; - ActuatorDesiredSet(&actuator); + ActuatorDesiredData actuator; + ActuatorDesiredGet(&actuator); + actuator.Roll = cmd->Roll; + actuator.Pitch = cmd->Pitch; + actuator.Yaw = cmd->Yaw; + actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; + ActuatorDesiredSet(&actuator); } static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) { - StabilizationDesiredData stabilization; - StabilizationDesiredGet(&stabilization); + StabilizationDesiredData stabilization; + StabilizationDesiredGet(&stabilization); - StabilizationSettingsData stabSettings; - StabilizationSettingsGet(&stabSettings); + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); - uint8_t * stab_settings; - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - switch(flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - stab_settings = settings->Stabilization1Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - stab_settings = settings->Stabilization2Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - stab_settings = settings->Stabilization3Settings; - break; - default: - // Major error, this should not occur because only enter this block when one of these is true - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - return; - } + uint8_t * stab_settings; + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + stab_settings = settings->Stabilization1Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + stab_settings = settings->Stabilization2Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + stab_settings = settings->Stabilization3Settings; + break; + default: + // Major error, this should not occur because only enter this block when one of these is true + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + return; + } - // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; + // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; - stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : - 0; // this is an invalid mode - ; - stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - 0; // this is an invalid mode + stabilization.Roll = + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? + cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode + ; + stabilization.Pitch = + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode - stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - 0; // this is an invalid mode + stabilization.Yaw = + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode - stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; - StabilizationDesiredSet(&stabilization); + stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; + StabilizationDesiredSet(&stabilization); } #if defined(REVOLUTION) @@ -704,87 +698,86 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon */ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home) { - static portTickType lastSysTime; - portTickType thisSysTime = xTaskGetTickCount(); - /* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */ - lastSysTime = thisSysTime; + static portTickType lastSysTime; + portTickType thisSysTime = xTaskGetTickCount(); + /* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */ + lastSysTime = thisSysTime; - if (home && changed) { - // Simple Return To Base mode - keep altitude the same, fly to home position - PositionActualData positionActual; - PositionActualGet(&positionActual); - - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.Start[PATHDESIRED_START_NORTH] = 0; - pathDesired.Start[PATHDESIRED_START_EAST] = 0; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10; - pathDesired.End[PATHDESIRED_END_NORTH] = 0; - pathDesired.End[PATHDESIRED_END_EAST] = 0; - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - } else if(changed) { - // After not being in this mode for a while init at current height - PositionActualData positionActual; - PositionActualGet(&positionActual); - - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; - pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. - } else { - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; - pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - */ - } + if (home && changed) { + // Simple Return To Base mode - keep altitude the same, fly to home position + PositionActualData positionActual; + PositionActualGet(&positionActual); + + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.Start[PATHDESIRED_START_NORTH] = 0; + pathDesired.Start[PATHDESIRED_START_EAST] = 0; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10; + pathDesired.End[PATHDESIRED_END_NORTH] = 0; + pathDesired.End[PATHDESIRED_END_EAST] = 0; + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; + pathDesired.StartingVelocity=1; + pathDesired.EndingVelocity=0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + } else if(changed) { + // After not being in this mode for a while init at current height + PositionActualData positionActual; + PositionActualGet(&positionActual); + + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; + pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; + pathDesired.StartingVelocity=1; + pathDesired.EndingVelocity=0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. + } else { + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; + pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + */ + } } - static void updateLandDesired(ManualControlCommandData * cmd, bool changed) { - static portTickType lastSysTime; - portTickType thisSysTime; - float dT; + static portTickType lastSysTime; + portTickType thisSysTime; + float dT; - thisSysTime = xTaskGetTickCount(); - dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; - lastSysTime = thisSysTime; + thisSysTime = xTaskGetTickCount(); + dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; + lastSysTime = thisSysTime; - PositionActualData positionActual; - PositionActualGet(&positionActual); + PositionActualData positionActual; + PositionActualGet(&positionActual); - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - if(changed) { - // After not being in this mode for a while init at current height - pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; - pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - } - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5; - PathDesiredSet(&pathDesired); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + if(changed) { + // After not being in this mode for a while init at current height + pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; + pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; + pathDesired.StartingVelocity=1; + pathDesired.EndingVelocity=0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + } + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5; + PathDesiredSet(&pathDesired); } /** @@ -794,41 +787,43 @@ static void updateLandDesired(ManualControlCommandData * cmd, bool changed) */ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) { - const float DEADBAND_HIGH = 0.55; - const float DEADBAND_LOW = 0.45; - - static portTickType lastSysTime; - static bool zeroed = false; - portTickType thisSysTime; - float dT; - AltitudeHoldDesiredData altitudeHoldDesired; - AltitudeHoldDesiredGet(&altitudeHoldDesired); + const float DEADBAND_HIGH = 0.55; + const float DEADBAND_LOW = 0.45; - StabilizationSettingsData stabSettings; - StabilizationSettingsGet(&stabSettings); + static portTickType lastSysTime; + static bool zeroed = false; + portTickType thisSysTime; + float dT; + AltitudeHoldDesiredData altitudeHoldDesired; + AltitudeHoldDesiredGet(&altitudeHoldDesired); - thisSysTime = xTaskGetTickCount(); - dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; - lastSysTime = thisSysTime; + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); - altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax; - altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax; - altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; - - float currentDown; - PositionActualDownGet(¤tDown); - if(changed) { - // After not being in this mode for a while init at current height - altitudeHoldDesired.Altitude = 0; - zeroed = false; - } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) - altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT; - else if (cmd->Throttle < DEADBAND_LOW && zeroed) - altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT; - else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) // Require the stick to enter the dead band before they can move height - zeroed = true; - - AltitudeHoldDesiredSet(&altitudeHoldDesired); + thisSysTime = xTaskGetTickCount(); + dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; + lastSysTime = thisSysTime; + + altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax; + altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax; + altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + + float currentDown; + PositionActualDownGet(¤tDown); + if(changed) { + // After not being in this mode for a while init at current height + altitudeHoldDesired.Altitude = 0; + zeroed = false; + } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { + altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT; + } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { + altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT; + } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) { + // Require the stick to enter the dead band before they can move height + zeroed = true; + } + + AltitudeHoldDesiredSet(&altitudeHoldDesired); } #else @@ -837,17 +832,17 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) // in flight static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); } static void updateLandDesired(ManualControlCommandData * cmd, bool changed) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); } static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); } #endif /** @@ -855,36 +850,39 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) */ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral) { - float valueScaled; + float valueScaled; - // Scale - if ((max > min && value >= neutral) || (min > max && value <= neutral)) - { - if (max != neutral) - valueScaled = (float)(value - neutral) / (float)(max - neutral); - else - valueScaled = 0; - } - else - { - if (min != neutral) - valueScaled = (float)(value - neutral) / (float)(neutral - min); - else - valueScaled = 0; - } + // Scale + if ((max > min && value >= neutral) || (min > max && value <= neutral)) { + if (max != neutral) { + valueScaled = (float) (value - neutral) / (float) (max - neutral); + } else { + valueScaled = 0; + } + } else { + if (min != neutral) { + valueScaled = (float) (value - neutral) / (float) (neutral - min); + } else { + valueScaled = 0; + } + } - // Bound - if (valueScaled > 1.0) valueScaled = 1.0; - else - if (valueScaled < -1.0) valueScaled = -1.0; + // Bound + if (valueScaled > 1.0) { + valueScaled = 1.0; + } else if (valueScaled < -1.0) { + valueScaled = -1.0; + } - return valueScaled; + return valueScaled; } -static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { - if(end_time > start_time) - return (end_time - start_time) * portTICK_RATE_MS; - return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS; +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) +{ + if (end_time > start_time) { + return (end_time - start_time) * portTICK_RATE_MS; + } + return ((((portTICK_RATE_MS) - 1) - start_time) + end_time) * portTICK_RATE_MS; } /** @@ -893,24 +891,21 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) */ static bool okToArm(void) { - // read alarms - SystemAlarmsData alarms; - SystemAlarmsGet(&alarms); + // read alarms + SystemAlarmsData alarms; + SystemAlarmsGet(&alarms); + // Check each alarm + for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { + if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set + if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) + continue; - // Check each alarm - for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) - { - if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) - { // found an alarm thats set - if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) - continue; + return false; + } + } - return false; - } - } - - return true; + return true; } /** * @brief Determine if the aircraft is forced to disarm by an explicit alarm @@ -918,28 +913,29 @@ static bool okToArm(void) */ static bool forcedDisArm(void) { - // read alarms - SystemAlarmsData alarms; - SystemAlarmsGet(&alarms); + // read alarms + SystemAlarmsData alarms; + SystemAlarmsGet(&alarms); - if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) { - return true; - } - return false; + if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) { + return true; + } + return false; } /** * @brief Update the flightStatus object only if value changed. Reduces callbacks * @param[in] val The new value */ -static void setArmedIfChanged(uint8_t val) { - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); +static void setArmedIfChanged(uint8_t val) +{ + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); - if(flightStatus.Armed != val) { - flightStatus.Armed = val; - FlightStatusSet(&flightStatus); - } + if (flightStatus.Armed != val) { + flightStatus.Armed = val; + FlightStatusSet(&flightStatus); + } } /** @@ -950,116 +946,121 @@ static void setArmedIfChanged(uint8_t val) { static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) { - bool lowThrottle = cmd->Throttle <= 0; + bool lowThrottle = cmd->Throttle <= 0; - if (forcedDisArm()) { - // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...) - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - return; - } + if (forcedDisArm()) { + // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...) + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + return; + } - if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { - // In this configuration we always disarm - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - } else { - // Not really needed since this function not called when disconnected - if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) - lowThrottle = true; + if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { + // In this configuration we always disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + } else { + // Not really needed since this function not called when disconnected + if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) + lowThrottle = true; - // The throttle is not low, in case we where arming or disarming, abort - if (!lowThrottle) { - switch(armState) { - case ARM_STATE_DISARMING_MANUAL: - case ARM_STATE_DISARMING_TIMEOUT: - armState = ARM_STATE_ARMED; - break; - case ARM_STATE_ARMING_MANUAL: - armState = ARM_STATE_DISARMED; - break; - default: - // Nothing needs to be done in the other states - break; - } - return; - } + // The throttle is not low, in case we where arming or disarming, abort + if (!lowThrottle) { + switch (armState) { + case ARM_STATE_DISARMING_MANUAL: + case ARM_STATE_DISARMING_TIMEOUT: + armState = ARM_STATE_ARMED; + break; + case ARM_STATE_ARMING_MANUAL: + armState = ARM_STATE_DISARMED; + break; + default: + // Nothing needs to be done in the other states + break; + } + return; + } - // The rest of these cases throttle is low - if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { - // In this configuration, we go into armed state as soon as the throttle is low, never disarm - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); - return; - } + // The rest of these cases throttle is low + if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { + // In this configuration, we go into armed state as soon as the throttle is low, never disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + return; + } + // When the configuration is not "Always armed" and no "Always disarmed", + // the state will not be changed when the throttle is not low + static portTickType armedDisarmStart; + float armingInputLevel = 0; - // When the configuration is not "Always armed" and no "Always disarmed", - // the state will not be changed when the throttle is not low - static portTickType armedDisarmStart; - float armingInputLevel = 0; + // Calc channel see assumptions7 + int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1; + switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) { + case ARMING_CHANNEL_ROLL: + armingInputLevel = sign * cmd->Roll; + break; + case ARMING_CHANNEL_PITCH: + armingInputLevel = sign * cmd->Pitch; + break; + case ARMING_CHANNEL_YAW: + armingInputLevel = sign * cmd->Yaw; + break; + } - // Calc channel see assumptions7 - int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1; - switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { - case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break; - case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break; - case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break; - } + bool manualArm = false; + bool manualDisarm = false; - bool manualArm = false; - bool manualDisarm = false; + if (armingInputLevel <= -ARMED_THRESHOLD) + manualArm = true; + else if (armingInputLevel >= +ARMED_THRESHOLD) + manualDisarm = true; - if (armingInputLevel <= -ARMED_THRESHOLD) - manualArm = true; - else if (armingInputLevel >= +ARMED_THRESHOLD) - manualDisarm = true; + switch (armState) { + case ARM_STATE_DISARMED: + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - switch(armState) { - case ARM_STATE_DISARMED: - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + // only allow arming if it's OK too + if (manualArm && okToArm()) { + armedDisarmStart = lastSysTime; + armState = ARM_STATE_ARMING_MANUAL; + } + break; - // only allow arming if it's OK too - if (manualArm && okToArm()) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_ARMING_MANUAL; - } - break; + case ARM_STATE_ARMING_MANUAL: + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); - case ARM_STATE_ARMING_MANUAL: - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); + if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) + armState = ARM_STATE_ARMED; + else if (!manualArm) + armState = ARM_STATE_DISARMED; + break; - if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) - armState = ARM_STATE_ARMED; - else if (!manualArm) - armState = ARM_STATE_DISARMED; - break; + case ARM_STATE_ARMED: + // When we get here, the throttle is low, + // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_TIMEOUT; + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + break; - case ARM_STATE_ARMED: - // When we get here, the throttle is low, - // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled - armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_TIMEOUT; - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); - break; + case ARM_STATE_DISARMING_TIMEOUT: + // We get here when armed while throttle low, even when the arming timeout is not enabled + if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) + armState = ARM_STATE_DISARMED; - case ARM_STATE_DISARMING_TIMEOUT: - // We get here when armed while throttle low, even when the arming timeout is not enabled - if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) - armState = ARM_STATE_DISARMED; + // Switch to disarming due to manual control when needed + if (manualDisarm) { + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_MANUAL; + } + break; - // Switch to disarming due to manual control when needed - if (manualDisarm) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_MANUAL; - } - break; - - case ARM_STATE_DISARMING_MANUAL: - if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) - armState = ARM_STATE_DISARMED; - else if (!manualDisarm) - armState = ARM_STATE_ARMED; - break; - } // End Switch - } + case ARM_STATE_DISARMING_MANUAL: + if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) + armState = ARM_STATE_DISARMED; + else if (!manualDisarm) + armState = ARM_STATE_ARMED; + break; + } // End Switch + } } /** @@ -1070,20 +1071,20 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData */ static void processFlightMode(ManualControlSettingsData *settings, float flightMode) { - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); - // Convert flightMode value into the switch position in the range [0..N-1] - uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9; - if (pos >= settings->FlightModeNumber) - pos = settings->FlightModeNumber - 1; + // Convert flightMode value into the switch position in the range [0..N-1] + uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9; + if (pos >= settings->FlightModeNumber) + pos = settings->FlightModeNumber - 1; - uint8_t newMode = settings->FlightModePosition[pos]; + uint8_t newMode = settings->FlightModePosition[pos]; - if (flightStatus.FlightMode != newMode) { - flightStatus.FlightMode = newMode; - FlightStatusSet(&flightStatus); - } + if (flightStatus.FlightMode != newMode) { + flightStatus.FlightMode = newMode; + FlightStatusSet(&flightStatus); + } } /** @@ -1092,13 +1093,12 @@ static void processFlightMode(ManualControlSettingsData *settings, float flightM */ bool validInputRange(int16_t min, int16_t max, uint16_t value) { - if (min > max) - { - int16_t tmp = min; - min = max; - max = tmp; - } - return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET); + if (min > max) { + int16_t tmp = min; + min = max; + max = tmp; + } + return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET); } /** @@ -1106,13 +1106,12 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value) */ static void applyDeadband(float *value, float deadband) { - if (fabs(*value) < deadband) - *value = 0.0f; - else - if (*value > 0.0f) - *value -= deadband; - else - *value += deadband; + if (fabs(*value) < deadband) + *value = 0.0f; + else if (*value > 0.0f) + *value -= deadband; + else + *value += deadband; } #ifdef USE_INPUT_LPF @@ -1121,25 +1120,22 @@ static void applyDeadband(float *value, float deadband) */ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) { - if (settings->ResponseTime[channel]) { - float rt = (float)settings->ResponseTime[channel]; - inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); - *value = inputFiltered[channel]; - } + if (settings->ResponseTime[channel]) { + float rt = (float)settings->ResponseTime[channel]; + inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); + *value = inputFiltered[channel]; + } } #endif // USE_INPUT_LPF - /** * Called whenever a critical configuration component changes */ static void configurationUpdatedCb(UAVObjEvent * ev) { - configuration_check(); + configuration_check(); } - - /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/Modules/Osd/osdgen/osdgen.c b/flight/Modules/Osd/osdgen/osdgen.c index 33f43d69a..4bc427bf7 100644 --- a/flight/Modules/Osd/osdgen/osdgen.c +++ b/flight/Modules/Osd/osdgen/osdgen.c @@ -29,7 +29,6 @@ */ // **************** - #include "openpilot.h" #include "osdgen.h" #include "attitudeactual.h" @@ -47,29 +46,28 @@ #include "splash.h" /* -static uint16_t angleA=0; -static int16_t angleB=90; -static int16_t angleC=0; -static int16_t sum=2; + static uint16_t angleA=0; + static int16_t angleB=90; + static int16_t angleC=0; + static int16_t sum=2; -static int16_t m_pitch=0; -static int16_t m_roll=0; -static int16_t m_yaw=0; -static int16_t m_batt=0; -static int16_t m_alt=0; + static int16_t m_pitch=0; + static int16_t m_roll=0; + static int16_t m_yaw=0; + static int16_t m_batt=0; + static int16_t m_alt=0; -static uint8_t m_gpsStatus=0; -static int32_t m_gpsLat=0; -static int32_t m_gpsLon=0; -static float m_gpsAlt=0; -static float m_gpsSpd=0;*/ + static uint8_t m_gpsStatus=0; + static int32_t m_gpsLat=0; + static int32_t m_gpsLon=0; + static float m_gpsAlt=0; + static float m_gpsSpd=0;*/ extern uint8_t *draw_buffer_level; extern uint8_t *draw_buffer_mask; extern uint8_t *disp_buffer_level; extern uint8_t *disp_buffer_mask; - TTime timex; // **************** @@ -94,143 +92,136 @@ static xTaskHandle osdgenTaskHandle; struct splashEntry { - unsigned int width, height; - const uint16_t *level; - const uint16_t *mask; + unsigned int width, height; + const uint16_t *level; + const uint16_t *mask; }; -struct splashEntry splash[3] = { - { oplogo_width, - oplogo_height, - oplogo_bits, - oplogo_mask_bits }, - { level_width, - level_height, - level_bits, - level_mask_bits }, - { llama_width, - llama_height, - llama_bits, - llama_mask_bits }, +struct splashEntry splash[3] = +{ +{ oplogo_width, oplogo_height, oplogo_bits, oplogo_mask_bits }, +{ level_width, level_height, level_bits, level_mask_bits }, +{ llama_width, llama_height, llama_bits, llama_mask_bits }, }; uint16_t mirror(uint16_t source) { - int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) | - ((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) | - ((source & 0x0800) << 1) | ((source & 0x0400) << 3) | - ((source & 0x0200) << 5) | ((source & 0x0100) << 7) | - ((source & 0x0080) >> 7) | ((source & 0x0040) >> 5) | - ((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) | - ((source & 0x0008) << 1) | ((source & 0x0004) << 3) | - ((source & 0x0002) << 5) | ((source & 0x0001) << 7); + int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) | ((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) | ((source & 0x0800) << 1) + | ((source & 0x0400) << 3) | ((source & 0x0200) << 5) | ((source & 0x0100) << 7) | ((source & 0x0080) >> 7) | ((source & 0x0040) >> 5) + | ((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) | ((source & 0x0008) << 1) | ((source & 0x0004) << 3) | ((source & 0x0002) << 5) + | ((source & 0x0001) << 7); - return result; + return result; } -void clearGraphics() { - memset((uint8_t *) draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); - memset((uint8_t *) draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); +void clearGraphics() +{ + memset((uint8_t *) draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); + memset((uint8_t *) draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT); } -void copyimage(uint16_t offsetx, uint16_t offsety, int image) { - //check top/left position - if (!validPos(offsetx, offsety)) { - return; - } - struct splashEntry splash_info; - splash_info = splash[image]; - offsetx=offsetx/8; - for (uint16_t y = offsety; y < ((splash_info.height)+offsety); y++) { - uint16_t x1=offsetx; - for (uint16_t x = offsetx; x < (((splash_info.width)/16)+offsetx); x++) { - draw_buffer_level[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8); - draw_buffer_level[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF); - draw_buffer_mask[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.mask[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8); - draw_buffer_mask[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.mask[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF); - x1+=2; - } - } +void copyimage(uint16_t offsetx, uint16_t offsety, int image) +{ + //check top/left position + if (!validPos(offsetx, offsety)) { + return; + } + struct splashEntry splash_info; + splash_info = splash[image]; + offsetx = offsetx / 8; + for (uint16_t y = offsety; y < ((splash_info.height) + offsety); y++) { + uint16_t x1 = offsetx; + for (uint16_t x = offsetx; x < (((splash_info.width) / 16) + offsetx); x++) { + draw_buffer_level[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)( + mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8); + draw_buffer_level[y * GRAPHICS_WIDTH + x1] = (uint8_t)( + mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF); + draw_buffer_mask[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)( + mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8); + draw_buffer_mask[y * GRAPHICS_WIDTH + x1] = (uint8_t)(mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF); + x1 += 2; + } + } } -uint8_t validPos(uint16_t x, uint16_t y) { - if ( x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) { - return 0; - } - return 1; +uint8_t validPos(uint16_t x, uint16_t y) +{ + if (x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) { + return 0; + } + return 1; } // Credit for this one goes to wikipedia! :-) -void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) { - int f = 1 - radius; - int ddF_x = 1; - int ddF_y = -2 * radius; - int x = 0; - int y = radius; +void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) +{ + int f = 1 - radius; + int ddF_x = 1; + int ddF_y = -2 * radius; + int x = 0; + int y = radius; - write_pixel_lm(x0, y0 + radius,1,1); - write_pixel_lm(x0, y0 - radius,1,1); - write_pixel_lm(x0 + radius, y0,1,1); - write_pixel_lm(x0 - radius, y0,1,1); + write_pixel_lm(x0, y0 + radius, 1, 1); + write_pixel_lm(x0, y0 - radius, 1, 1); + write_pixel_lm(x0 + radius, y0, 1, 1); + write_pixel_lm(x0 - radius, y0, 1, 1); - while(x < y) - { - // ddF_x == 2 * x + 1; - // ddF_y == -2 * y; - // f == x*x + y*y - radius*radius + 2*x - y + 1; - if(f >= 0) - { - y--; - ddF_y += 2; - f += ddF_y; - } - x++; - ddF_x += 2; - f += ddF_x; - write_pixel_lm(x0 + x, y0 + y,1,1); - write_pixel_lm(x0 - x, y0 + y,1,1); - write_pixel_lm(x0 + x, y0 - y,1,1); - write_pixel_lm(x0 - x, y0 - y,1,1); - write_pixel_lm(x0 + y, y0 + x,1,1); - write_pixel_lm(x0 - y, y0 + x,1,1); - write_pixel_lm(x0 + y, y0 - x,1,1); - write_pixel_lm(x0 - y, y0 - x,1,1); - } + while (x < y) { + // ddF_x == 2 * x + 1; + // ddF_y == -2 * y; + // f == x*x + y*y - radius*radius + 2*x - y + 1; + if (f >= 0) { + y--; + ddF_y += 2; + f += ddF_y; + } + x++; + ddF_x += 2; + f += ddF_x; + write_pixel_lm(x0 + x, y0 + y, 1, 1); + write_pixel_lm(x0 - x, y0 + y, 1, 1); + write_pixel_lm(x0 + x, y0 - y, 1, 1); + write_pixel_lm(x0 - x, y0 - y, 1, 1); + write_pixel_lm(x0 + y, y0 + x, 1, 1); + write_pixel_lm(x0 - y, y0 + x, 1, 1); + write_pixel_lm(x0 + y, y0 - x, 1, 1); + write_pixel_lm(x0 - y, y0 - x, 1, 1); + } } -void swap(uint16_t* a, uint16_t* b) { - uint16_t temp = *a; - *a = *b; - *b = temp; +void swap(uint16_t* a, uint16_t* b) +{ + uint16_t temp = *a; + *a = *b; + *b = temp; } +const static int8_t sinData[91] = +{ 0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, 63, 64, + 66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, 97, 98, + 98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100 }; -const static int8_t sinData[91] = { - 0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, - 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, - 63, 64, 66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, - 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, - 97, 98, 98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100}; - -static int8_t mySin(uint16_t angle) { - uint16_t pos = 0; - pos = angle % 360; - int8_t mult = 1; - // 180-359 is same as 0-179 but negative. - if (pos >= 180) { - pos = pos - 180; - mult = -1; - } - // 0-89 is equal to 90-179 except backwards. - if (pos >= 90) { - pos = 180 - pos; - } - return mult * (int8_t)(sinData[pos]); +static int8_t mySin(uint16_t angle) +{ + uint16_t pos = 0; + pos = angle % 360; + int8_t mult = 1; + // 180-359 is same as 0-179 but negative. + if (pos >= 180) { + pos = pos - 180; + mult = -1; + } + // 0-89 is equal to 90-179 except backwards. + if (pos >= 90) { + pos = 180 - pos; + } + return mult * (int8_t)(sinData[pos]); } -static int8_t myCos(uint16_t angle) { - return mySin(angle + 90); +static int8_t myCos(uint16_t angle) +{ + return mySin(angle + 90); } /// Draws four points relative to the given center point. @@ -247,10 +238,10 @@ static int8_t myCos(uint16_t angle) { /// \param color the color to draw the pixels with. void plotFourQuadrants(int32_t centerX, int32_t centerY, int32_t deltaX, int32_t deltaY) { - write_pixel_lm(centerX + deltaX, centerY + deltaY,1,1); // Ist Quadrant - write_pixel_lm(centerX - deltaX, centerY + deltaY,1,1); // IInd Quadrant - write_pixel_lm(centerX - deltaX, centerY - deltaY,1,1); // IIIrd Quadrant - write_pixel_lm(centerX + deltaX, centerY - deltaY,1,1); // IVth Quadrant + write_pixel_lm(centerX + deltaX, centerY + deltaY, 1, 1); // Ist Quadrant + write_pixel_lm(centerX - deltaX, centerY + deltaY, 1, 1); // IInd Quadrant + write_pixel_lm(centerX - deltaX, centerY - deltaY, 1, 1); // IIIrd Quadrant + write_pixel_lm(centerX + deltaX, centerY - deltaY, 1, 1); // IVth Quadrant } /// Implements the midpoint ellipse drawing algorithm which is a bresenham @@ -275,62 +266,58 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius) plotFourQuadrants(centerX, centerY, x, y); - while(deltaY >= deltaX) - { - x++; - deltaX += (doubleVerticalRadius << 1); + while (deltaY >= deltaX) { + x++; + deltaX += (doubleVerticalRadius << 1); - error += deltaX + doubleVerticalRadius; + error += deltaX + doubleVerticalRadius; - if(error >= 0) - { - y--; - deltaY -= (doubleHorizontalRadius << 1); + if (error >= 0) { + y--; + deltaY -= (doubleHorizontalRadius << 1); - error -= deltaY; - } - plotFourQuadrants(centerX, centerY, x, y); + error -= deltaY; + } + plotFourQuadrants(centerX, centerY, x, y); } - error = (int64_t)(doubleVerticalRadius * (x + 1 / 2.0) * (x + 1 / 2.0) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius); + error = (int64_t)( + doubleVerticalRadius * (x + 1 / 2.0) * (x + 1 / 2.0) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius); - while (y>=0) - { - error += doubleHorizontalRadius; - y--; - deltaY -= (doubleHorizontalRadius<<1); - error -= deltaY; + while (y >= 0) { + error += doubleHorizontalRadius; + y--; + deltaY -= (doubleHorizontalRadius << 1); + error -= deltaY; - if(error <= 0) - { - x++; - deltaX += (doubleVerticalRadius << 1); - error += deltaX; - } + if (error <= 0) { + x++; + deltaX += (doubleVerticalRadius << 1); + error += deltaX; + } - plotFourQuadrants(centerX, centerY, x, y); + plotFourQuadrants(centerX, centerY, x, y); } } - void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size) { - int16_t a = myCos(angle); - int16_t b = mySin(angle); - a = (a * (size/2)) / 100; - b = (b * (size/2)) / 100; - write_line_lm((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a, 1, 1); //Direction line - //write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line - write_line_lm((x)-1 + b, (y)-1 - a, (x)-1 - a/2, (y)-1 - b/2, 1, 1); // Arrow "wings" - write_line_lm((x)-1 + b, (y)-1 - a, (x)-1 + a/2, (y)-1 + b/2, 1, 1); + int16_t a = myCos(angle); + int16_t b = mySin(angle); + a = (a * (size / 2)) / 100; + b = (b * (size / 2)) / 100; + write_line_lm((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 1, 1); //Direction line + //write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line + write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 - a / 2, (y) - 1 - b / 2, 1, 1); // Arrow "wings" + write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 + a / 2, (y) - 1 + b / 2, 1, 1); } void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2) { - write_line_lm(x1, y1, x2, y1, 1, 1); //top - write_line_lm(x1, y1, x1, y2, 1, 1); //left - write_line_lm(x2, y1, x2, y2, 1, 1); //right - write_line_lm(x1, y2, x2, y2, 1, 1); //bottom + write_line_lm(x1, y1, x2, y1, 1, 1); //top + write_line_lm(x1, y1, x1, y2, 1, 1); //left + write_line_lm(x2, y1, x2, y2, 1, 1); //right + write_line_lm(x1, y2, x2, y2, 1, 1); //bottom } // simple routines @@ -347,14 +334,14 @@ void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2) */ void write_pixel(uint8_t *buff, unsigned int x, unsigned int y, int mode) { - CHECK_COORDS(x, y); - // Determine the bit in the word to be set and the word - // index to set it in. - int bitnum = CALC_BIT_IN_WORD(x); - int wordnum = CALC_BUFF_ADDR(x, y); - // Apply a mask. - uint16_t mask = 1 << (7 - bitnum); - WRITE_WORD_MODE(buff, wordnum, mask, mode); + CHECK_COORDS(x, y); + // Determine the bit in the word to be set and the word + // index to set it in. + int bitnum = CALC_BIT_IN_WORD(x); + int wordnum = CALC_BUFF_ADDR(x, y); + // Apply a mask. + uint16_t mask = 1 << (7 - bitnum); + WRITE_WORD_MODE(buff, wordnum, mask, mode); } /** @@ -368,18 +355,17 @@ void write_pixel(uint8_t *buff, unsigned int x, unsigned int y, int mode) */ void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode) { - CHECK_COORDS(x, y); - // Determine the bit in the word to be set and the word - // index to set it in. - int bitnum = CALC_BIT_IN_WORD(x); - int wordnum = CALC_BUFF_ADDR(x, y); - // Apply the masks. - uint16_t mask = 1 << (7 - bitnum); - WRITE_WORD_MODE(draw_buffer_mask, wordnum, mask, mmode); - WRITE_WORD_MODE(draw_buffer_level, wordnum, mask, lmode); + CHECK_COORDS(x, y); + // Determine the bit in the word to be set and the word + // index to set it in. + int bitnum = CALC_BIT_IN_WORD(x); + int wordnum = CALC_BUFF_ADDR(x, y); + // Apply the masks. + uint16_t mask = 1 << (7 - bitnum); + WRITE_WORD_MODE(draw_buffer_mask, wordnum, mask, mmode); + WRITE_WORD_MODE(draw_buffer_level, wordnum, mask, lmode); } - /** * write_hline: optimised horizontal line writing algorithm * @@ -391,41 +377,38 @@ void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode) */ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y, int mode) { - CLIP_COORDS(x0, y); - CLIP_COORDS(x1, y); - if(x0 > x1) - { - SWAP(x0, x1); - } - if(x0 == x1) return; - /* This is an optimised algorithm for writing horizontal lines. - * We begin by finding the addresses of the x0 and x1 points. */ - int addr0 = CALC_BUFF_ADDR(x0, y); - int addr1 = CALC_BUFF_ADDR(x1, y); - int addr0_bit = CALC_BIT_IN_WORD(x0); - int addr1_bit = CALC_BIT_IN_WORD(x1); - int mask, mask_l, mask_r, i; - /* If the addresses are equal, we only need to write one word - * which is an island. */ - if(addr0 == addr1) - { - mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit); - WRITE_WORD_MODE(buff, addr0, mask, mode); - } - /* Otherwise we need to write the edges and then the middle. */ - else - { - mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit); - mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); - WRITE_WORD_MODE(buff, addr0, mask_l, mode); - WRITE_WORD_MODE(buff, addr1, mask_r, mode); - // Now write 0xffff words from start+1 to end-1. - for(i = addr0 + 1; i <= addr1 - 1; i++) - { - uint8_t m=0xff; - WRITE_WORD_MODE(buff, i, m, mode); - } - } + CLIP_COORDS(x0, y); + CLIP_COORDS(x1, y); + if (x0 > x1) { + SWAP(x0, x1); + } + if (x0 == x1) { + return; + } + /* This is an optimised algorithm for writing horizontal lines. + * We begin by finding the addresses of the x0 and x1 points. */ + int addr0 = CALC_BUFF_ADDR(x0, y); + int addr1 = CALC_BUFF_ADDR(x1, y); + int addr0_bit = CALC_BIT_IN_WORD(x0); + int addr1_bit = CALC_BIT_IN_WORD(x1); + int mask, mask_l, mask_r, i; + /* If the addresses are equal, we only need to write one word + * which is an island. */ + if (addr0 == addr1) { + mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit); + WRITE_WORD_MODE(buff, addr0, mask, mode); + } else { + /* Otherwise we need to write the edges and then the middle. */ + mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit); + mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); + WRITE_WORD_MODE(buff, addr0, mask_l, mode); + WRITE_WORD_MODE(buff, addr1, mask_r, mode); + // Now write 0xffff words from start+1 to end-1. + for (i = addr0 + 1; i <= addr1 - 1; i++) { + uint8_t m = 0xff; + WRITE_WORD_MODE(buff, i, m, mode); + } + } } /** @@ -439,10 +422,10 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y */ void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode, int mmode) { - // TODO: an optimisation would compute the masks and apply to - // both buffers simultaneously. - write_hline(draw_buffer_level, x0, x1, y, lmode); - write_hline(draw_buffer_mask, x0, x1, y, mmode); + // TODO: an optimisation would compute the masks and apply to + // both buffers simultaneously. + write_hline(draw_buffer_level, x0, x1, y, lmode); + write_hline(draw_buffer_mask, x0, x1, y, mmode); } /** @@ -459,19 +442,18 @@ void write_hline_lm(unsigned int x0, unsigned int x1, unsigned int y, int lmode, */ void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int endcap0, int endcap1, int mode, int mmode) { - int stroke, fill; - SETUP_STROKE_FILL(stroke, fill, mode) - if(x0 > x1) - { - SWAP(x0, x1); - } - // Draw the main body of the line. - write_hline_lm(x0 + 1, x1 - 1, y - 1, stroke, mmode); - write_hline_lm(x0 + 1, x1 - 1, y + 1, stroke, mmode); - write_hline_lm(x0 + 1, x1 - 1, y, fill, mmode); - // Draw the endcaps, if any. - DRAW_ENDCAP_HLINE(endcap0, x0, y, stroke, fill, mmode); - DRAW_ENDCAP_HLINE(endcap1, x1, y, stroke, fill, mmode); + int stroke, fill; + SETUP_STROKE_FILL(stroke, fill, mode) + if (x0 > x1) { + SWAP(x0, x1); + } + // Draw the main body of the line. + write_hline_lm(x0 + 1, x1 - 1, y - 1, stroke, mmode); + write_hline_lm(x0 + 1, x1 - 1, y + 1, stroke, mmode); + write_hline_lm(x0 + 1, x1 - 1, y, fill, mmode); + // Draw the endcaps, if any. + DRAW_ENDCAP_HLINE(endcap0, x0, y, stroke, fill, mmode); + DRAW_ENDCAP_HLINE(endcap1, x1, y, stroke, fill, mmode); } /** @@ -485,27 +467,27 @@ void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int */ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1, int mode) { - unsigned int a; - CLIP_COORDS(x, y0); - CLIP_COORDS(x, y1); - if(y0 > y1) - { - SWAP(y0, y1); - } - if(y0 == y1) return; - /* This is an optimised algorithm for writing vertical lines. - * We begin by finding the addresses of the x,y0 and x,y1 points. */ - int addr0 = CALC_BUFF_ADDR(x, y0); - int addr1 = CALC_BUFF_ADDR(x, y1); - /* Then we calculate the pixel data to be written. */ - int bitnum = CALC_BIT_IN_WORD(x); - uint16_t mask = 1 << (7 - bitnum); - /* Run from addr0 to addr1 placing pixels. Increment by the number - * of words n each graphics line. */ - for(a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8) - { - WRITE_WORD_MODE(buff, a, mask, mode); - } + unsigned int a; + CLIP_COORDS(x, y0); + CLIP_COORDS(x, y1); + if (y0 > y1) { + SWAP(y0, y1); + } + if (y0 == y1) { + return; + } + /* This is an optimised algorithm for writing vertical lines. + * We begin by finding the addresses of the x,y0 and x,y1 points. */ + int addr0 = CALC_BUFF_ADDR(x, y0); + int addr1 = CALC_BUFF_ADDR(x, y1); + /* Then we calculate the pixel data to be written. */ + int bitnum = CALC_BIT_IN_WORD(x); + uint16_t mask = 1 << (7 - bitnum); + /* Run from addr0 to addr1 placing pixels. Increment by the number + * of words n each graphics line. */ + for (a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8) { + WRITE_WORD_MODE(buff, a, mask, mode); + } } /** @@ -519,10 +501,10 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1 */ void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode, int mmode) { - // TODO: an optimisation would compute the masks and apply to - // both buffers simultaneously. - write_vline(draw_buffer_level, x, y0, y1, lmode); - write_vline(draw_buffer_mask, x, y0, y1, mmode); + // TODO: an optimisation would compute the masks and apply to + // both buffers simultaneously. + write_vline(draw_buffer_level, x, y0, y1, lmode); + write_vline(draw_buffer_mask, x, y0, y1, mmode); } /** @@ -539,19 +521,18 @@ void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode, */ void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int endcap0, int endcap1, int mode, int mmode) { - int stroke, fill; - if(y0 > y1) - { - SWAP(y0, y1); - } - SETUP_STROKE_FILL(stroke, fill, mode); - // Draw the main body of the line. - write_vline_lm(x - 1, y0 + 1, y1 - 1, stroke, mmode); - write_vline_lm(x + 1, y0 + 1, y1 - 1, stroke, mmode); - write_vline_lm(x, y0 + 1, y1 - 1, fill, mmode); - // Draw the endcaps, if any. - DRAW_ENDCAP_VLINE(endcap0, x, y0, stroke, fill, mmode); - DRAW_ENDCAP_VLINE(endcap1, x, y1, stroke, fill, mmode); + int stroke, fill; + if (y0 > y1) { + SWAP(y0, y1); + } + SETUP_STROKE_FILL(stroke, fill, mode); + // Draw the main body of the line. + write_vline_lm(x - 1, y0 + 1, y1 - 1, stroke, mmode); + write_vline_lm(x + 1, y0 + 1, y1 - 1, stroke, mmode); + write_vline_lm(x, y0 + 1, y1 - 1, fill, mmode); + // Draw the endcaps, if any. + DRAW_ENDCAP_VLINE(endcap0, x, y0, stroke, fill, mmode); + DRAW_ENDCAP_VLINE(endcap1, x, y1, stroke, fill, mmode); } /** @@ -570,61 +551,56 @@ void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int */ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsigned int width, unsigned int height, int mode) { - int yy, addr0_old, addr1_old; - CHECK_COORDS(x, y); - CHECK_COORD_X(x + width); - CHECK_COORD_Y(y + height); - if(width <= 0 || height <= 0) return; - // Calculate as if the rectangle was only a horizontal line. We then - // step these addresses through each row until we iterate `height` times. - int addr0 = CALC_BUFF_ADDR(x, y); - int addr1 = CALC_BUFF_ADDR(x + width, y); - int addr0_bit = CALC_BIT_IN_WORD(x); - int addr1_bit = CALC_BIT_IN_WORD(x + width); - int mask, mask_l, mask_r, i; - // If the addresses are equal, we need to write one word vertically. - if(addr0 == addr1) - { - mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit); - while(height--) - { - WRITE_WORD_MODE(buff, addr0, mask, mode); - addr0 += GRAPHICS_WIDTH_REAL / 8; - } + int yy, addr0_old, addr1_old; + CHECK_COORDS(x, y); + CHECK_COORD_X(x + width); + CHECK_COORD_Y(y + height); + if (width <= 0 || height <= 0) { + return; + } + // Calculate as if the rectangle was only a horizontal line. We then + // step these addresses through each row until we iterate `height` times. + int addr0 = CALC_BUFF_ADDR(x, y); + int addr1 = CALC_BUFF_ADDR(x + width, y); + int addr0_bit = CALC_BIT_IN_WORD(x); + int addr1_bit = CALC_BIT_IN_WORD(x + width); + int mask, mask_l, mask_r, i; + // If the addresses are equal, we need to write one word vertically. + if (addr0 == addr1) { + mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit); + while (height--) { + WRITE_WORD_MODE(buff, addr0, mask, mode); + addr0 += GRAPHICS_WIDTH_REAL / 8; } + } else { // Otherwise we need to write the edges and then the middle repeatedly. - else - { - mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit); - mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); - // Write edges first. - yy = 0; - addr0_old = addr0; - addr1_old = addr1; - while(yy < height) - { - WRITE_WORD_MODE(buff, addr0, mask_l, mode); - WRITE_WORD_MODE(buff, addr1, mask_r, mode); - addr0 += GRAPHICS_WIDTH_REAL / 8; - addr1 += GRAPHICS_WIDTH_REAL / 8; - yy++; - } - // Now write 0xffff words from start+1 to end-1 for each row. - yy = 0; - addr0 = addr0_old; - addr1 = addr1_old; - while(yy < height) - { - for(i = addr0 + 1; i <= addr1 - 1; i++) - { - uint8_t m=0xff; - WRITE_WORD_MODE(buff, i, m, mode); - } - addr0 += GRAPHICS_WIDTH_REAL / 8; - addr1 += GRAPHICS_WIDTH_REAL / 8; - yy++; - } + mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit); + mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit); + // Write edges first. + yy = 0; + addr0_old = addr0; + addr1_old = addr1; + while (yy < height) { + WRITE_WORD_MODE(buff, addr0, mask_l, mode); + WRITE_WORD_MODE(buff, addr1, mask_r, mode); + addr0 += GRAPHICS_WIDTH_REAL / 8; + addr1 += GRAPHICS_WIDTH_REAL / 8; + yy++; } + // Now write 0xffff words from start+1 to end-1 for each row. + yy = 0; + addr0 = addr0_old; + addr1 = addr1_old; + while (yy < height) { + for (i = addr0 + 1; i <= addr1 - 1; i++) { + uint8_t m = 0xff; + WRITE_WORD_MODE(buff, i, m, mode); + } + addr0 += GRAPHICS_WIDTH_REAL / 8; + addr1 += GRAPHICS_WIDTH_REAL / 8; + yy++; + } + } } /** @@ -639,8 +615,8 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig */ void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int width, unsigned int height, int lmode, int mmode) { - write_filled_rectangle(draw_buffer_mask, x, y, width, height, mmode); - write_filled_rectangle(draw_buffer_level, x, y, width, height, lmode); + write_filled_rectangle(draw_buffer_mask, x, y, width, height, mmode); + write_filled_rectangle(draw_buffer_level, x, y, width, height, lmode); } /** @@ -656,14 +632,14 @@ void write_filled_rectangle_lm(unsigned int x, unsigned int y, unsigned int widt */ void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int height, int mode, int mmode) { - //CHECK_COORDS(x, y); - //CHECK_COORDS(x + width, y + height); - //if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x; - //if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y; - write_hline_outlined(x, x + width, y, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); - write_hline_outlined(x, x + width, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); - write_vline_outlined(x, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); - write_vline_outlined(x + width, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); + //CHECK_COORDS(x, y); + //CHECK_COORDS(x + width, y + height); + //if((x + width) > DISP_WIDTH) width = DISP_WIDTH - x; + //if((y + height) > DISP_HEIGHT) height = DISP_HEIGHT - y; + write_hline_outlined(x, x + width, y, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); + write_hline_outlined(x, x + width, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); + write_vline_outlined(x, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); + write_vline_outlined(x + width, y, y + height, ENDCAP_ROUND, ENDCAP_ROUND, mode, mmode); } /** @@ -679,22 +655,19 @@ void write_rectangle_outlined(unsigned int x, unsigned int y, int width, int hei */ void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int mode) { - CHECK_COORDS(cx, cy); - int error = -r, x = r, y = 0; - while(x >= y) - { - if(dashp == 0 || (y % dashp) < (dashp / 2)) - { - CIRCLE_PLOT_8(buff, cx, cy, x, y, mode); - } - error += (y * 2) + 1; - y++; - if(error >= 0) - { - --x; - error -= x * 2; - } - } + CHECK_COORDS(cx, cy); + int error = -r, x = r, y = 0; + while (x >= y) { + if (dashp == 0 || (y % dashp) < (dashp / 2)) { + CIRCLE_PLOT_8(buff, cx, cy, x, y, mode); + } + error += (y * 2) + 1; + y++; + if (error >= 0) { + --x; + error -= x * 2; + } + } } /** @@ -710,58 +683,51 @@ void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int */ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, unsigned int dashp, int bmode, int mode, int mmode) { - int stroke, fill; - CHECK_COORDS(cx, cy); - SETUP_STROKE_FILL(stroke, fill, mode); - // This is a two step procedure. First, we draw the outline of the - // circle, then we draw the inner part. - int error = -r, x = r, y = 0; - while(x >= y) - { - if(dashp == 0 || (y % dashp) < (dashp / 2)) - { - CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y, mmode); - CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke); - CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode); - CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y + 1, stroke); - CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y, mmode); - CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke); - CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode); - CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke); - if(bmode == 1) - { - CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y + 1, mmode); - CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y + 1, stroke); - CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y - 1, mmode); - CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y - 1, stroke); - } - } - error += (y * 2) + 1; - y++; - if(error >= 0) - { - --x; - error -= x * 2; - } - } - error = -r; - x = r; - y = 0; - while(x >= y) - { - if(dashp == 0 || (y % dashp) < (dashp / 2)) - { - CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode); - CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill); - } - error += (y * 2) + 1; - y++; - if(error >= 0) - { - --x; - error -= x * 2; - } - } + int stroke, fill; + CHECK_COORDS(cx, cy); + SETUP_STROKE_FILL(stroke, fill, mode); + // This is a two step procedure. First, we draw the outline of the + // circle, then we draw the inner part. + int error = -r, x = r, y = 0; + while (x >= y) { + if (dashp == 0 || (y % dashp) < (dashp / 2)) { + CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y, mmode); + CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke); + CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode); + CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y + 1, stroke); + CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y, mmode); + CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke); + CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode); + CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke); + if (bmode == 1) { + CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y + 1, mmode); + CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y + 1, stroke); + CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y - 1, mmode); + CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y - 1, stroke); + } + } + error += (y * 2) + 1; + y++; + if (error >= 0) { + --x; + error -= x * 2; + } + } + error = -r; + x = r; + y = 0; + while (x >= y) { + if (dashp == 0 || (y % dashp) < (dashp / 2)) { + CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode); + CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill); + } + error += (y * 2) + 1; + y++; + if (error >= 0) { + --x; + error -= x * 2; + } + } } /** @@ -775,42 +741,37 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns */ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int r, int mode) { - CHECK_COORDS(cx, cy); - int error = -r, x = r, y = 0, xch = 0; - // It turns out that filled circles can take advantage of the midpoint - // circle algorithm. We simply draw very fast horizontal lines across each - // pair of X,Y coordinates. In some cases, this can even be faster than - // drawing an outlined circle! - // - // Due to multiple writes to each set of pixels, we have a special exception - // for when using the toggling draw mode. - while(x >= y) - { - if(y != 0) - { - write_hline(buff, cx - x, cx + x, cy + y, mode); - write_hline(buff, cx - x, cx + x, cy - y, mode); - if(mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y))) - { - write_hline(buff, cx - y, cx + y, cy + x, mode); - write_hline(buff, cx - y, cx + y, cy - x, mode); - xch = 0; - } - } - error += (y * 2) + 1; - y++; - if(error >= 0) - { - --x; - xch = 1; - error -= x * 2; - } - } - // Handle toggle mode. - if(mode == 2) - { - write_hline(buff, cx - r, cx + r, cy, mode); - } + CHECK_COORDS(cx, cy); + int error = -r, x = r, y = 0, xch = 0; + // It turns out that filled circles can take advantage of the midpoint + // circle algorithm. We simply draw very fast horizontal lines across each + // pair of X,Y coordinates. In some cases, this can even be faster than + // drawing an outlined circle! + // + // Due to multiple writes to each set of pixels, we have a special exception + // for when using the toggling draw mode. + while (x >= y) { + if (y != 0) { + write_hline(buff, cx - x, cx + x, cy + y, mode); + write_hline(buff, cx - x, cx + x, cy - y, mode); + if (mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y))) { + write_hline(buff, cx - y, cx + y, cy + x, mode); + write_hline(buff, cx - y, cx + y, cy - x, mode); + xch = 0; + } + } + error += (y * 2) + 1; + y++; + if (error >= 0) { + --x; + xch = 1; + error -= x * 2; + } + } + // Handle toggle mode. + if (mode == 2) { + write_hline(buff, cx - r, cx + r, cy, mode); + } } /** @@ -825,45 +786,39 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign */ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mode) { - // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm - int steep = abs(y1 - y0) > abs(x1 - x0); - if(steep) - { - SWAP(x0, y0); - SWAP(x1, y1); - } - if(x0 > x1) - { - SWAP(x0, x1); - SWAP(y0, y1); - } - int deltax = x1 - x0; - int deltay = abs(y1 - y0); - int error = deltax / 2; - int ystep; - int y = y0; - int x; //, lasty = y, stox = 0; - if(y0 < y1) - ystep = 1; - else - ystep = -1; - for(x = x0; x < x1; x++) - { - if(steep) - { - write_pixel(buff, y, x, mode); - } - else - { - write_pixel(buff, x, y, mode); - } - error -= deltay; - if(error < 0) - { - y += ystep; - error += deltax; - } - } + // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm + int steep = abs(y1 - y0) > abs(x1 - x0); + if (steep) { + SWAP(x0, y0); + SWAP(x1, y1); + } + if (x0 > x1) { + SWAP(x0, x1); + SWAP(y0, y1); + } + int deltax = x1 - x0; + int deltay = abs(y1 - y0); + int error = deltax / 2; + int ystep; + int y = y0; + int x; //, lasty = y, stox = 0; + if (y0 < y1) { + ystep = 1; + } else { + ystep = -1; + } + for (x = x0; x < x1; x++) { + if (steep) { + write_pixel(buff, y, x, mode); + } else { + write_pixel(buff, x, y, mode); + } + error -= deltay; + if (error < 0) { + y += ystep; + error += deltax; + } + } } /** @@ -878,8 +833,8 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1 */ void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int mmode, int lmode) { - write_line(draw_buffer_mask, x0, y0, x1, y1, mmode); - write_line(draw_buffer_level, x0, y0, x1, y1, lmode); + write_line(draw_buffer_mask, x0, y0, x1, y1, mmode); + write_line(draw_buffer_level, x0, y0, x1, y1, lmode); } /** @@ -897,84 +852,70 @@ void write_line_lm(unsigned int x0, unsigned int y0, unsigned int x1, unsigned i */ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, int endcap0, int endcap1, int mode, int mmode) { - // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm - // This could be improved for speed. - int omode, imode; - if(mode == 0) - { - omode = 0; - imode = 1; - } - else - { - omode = 1; - imode = 0; - } - int steep = abs(y1 - y0) > abs(x1 - x0); - if(steep) - { - SWAP(x0, y0); - SWAP(x1, y1); - } - if(x0 > x1) - { - SWAP(x0, x1); - SWAP(y0, y1); - } - int deltax = x1 - x0; - int deltay = abs(y1 - y0); - int error = deltax / 2; - int ystep; - int y = y0; - int x; - if(y0 < y1) - ystep = 1; - else - ystep = -1; - // Draw the outline. - for(x = x0; x < x1; x++) - { - if(steep) - { - write_pixel_lm(y - 1, x, mmode, omode); - write_pixel_lm(y + 1, x, mmode, omode); - write_pixel_lm(y, x - 1, mmode, omode); - write_pixel_lm(y, x + 1, mmode, omode); - } - else - { - write_pixel_lm(x - 1, y, mmode, omode); - write_pixel_lm(x + 1, y, mmode, omode); - write_pixel_lm(x, y - 1, mmode, omode); - write_pixel_lm(x, y + 1, mmode, omode); - } - error -= deltay; - if(error < 0) - { - y += ystep; - error += deltax; - } - } - // Now draw the innards. - error = deltax / 2; - y = y0; - for(x = x0; x < x1; x++) - { - if(steep) - { - write_pixel_lm(y, x, mmode, imode); - } - else - { - write_pixel_lm(x, y, mmode, imode); - } - error -= deltay; - if(error < 0) - { - y += ystep; - error += deltax; - } - } + // Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm + // This could be improved for speed. + int omode, imode; + if (mode == 0) { + omode = 0; + imode = 1; + } else { + omode = 1; + imode = 0; + } + int steep = abs(y1 - y0) > abs(x1 - x0); + if (steep) { + SWAP(x0, y0); + SWAP(x1, y1); + } + if (x0 > x1) { + SWAP(x0, x1); + SWAP(y0, y1); + } + int deltax = x1 - x0; + int deltay = abs(y1 - y0); + int error = deltax / 2; + int ystep; + int y = y0; + int x; + if (y0 < y1) { + ystep = 1; + } else { + ystep = -1; + } + // Draw the outline. + for (x = x0; x < x1; x++) { + if (steep) { + write_pixel_lm(y - 1, x, mmode, omode); + write_pixel_lm(y + 1, x, mmode, omode); + write_pixel_lm(y, x - 1, mmode, omode); + write_pixel_lm(y, x + 1, mmode, omode); + } else { + write_pixel_lm(x - 1, y, mmode, omode); + write_pixel_lm(x + 1, y, mmode, omode); + write_pixel_lm(x, y - 1, mmode, omode); + write_pixel_lm(x, y + 1, mmode, omode); + } + error -= deltay; + if (error < 0) { + y += ystep; + error += deltax; + } + } + // Now draw the innards. + error = deltax / 2; + y = y0; + for (x = x0; x < x1; x++) { + if (steep) { + write_pixel_lm(y, x, mmode, imode); + } else { + write_pixel_lm(x, y, mmode, imode); + } + error -= deltay; + if (error < 0) { + y += ystep; + error += deltax; + } + } } /** @@ -991,12 +932,13 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi */ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff, int mode) { - uint16_t firstmask = word >> xoff; - uint16_t lastmask = word << (16 - xoff); - WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode); - WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode); - if(xoff > 0) - WRITE_WORD_MODE(buff, addr+2, (lastmask & 0xff00) >> 8, mode); + uint16_t firstmask = word >> xoff; + uint16_t lastmask = word << (16 - xoff); + WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode); + WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode); + if (xoff > 0) { + WRITE_WORD_MODE(buff, addr+2, (lastmask & 0xff00) >> 8, mode); + } } /** @@ -1016,12 +958,13 @@ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsi */ void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff) { - uint16_t firstmask = word >> xoff; - uint16_t lastmask = word << (16 - xoff); - WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff); - WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8); - if(xoff > 0) - WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8); + uint16_t firstmask = word >> xoff; + uint16_t lastmask = word << (16 - xoff); + WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff); + WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8); + if (xoff > 0) { + WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8); + } } /** @@ -1041,12 +984,13 @@ void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr, */ void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, unsigned int xoff) { - uint16_t firstmask = word >> xoff; - uint16_t lastmask = word << (16 - xoff); - WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff); - WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8); - if(xoff > 0) - WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8); + uint16_t firstmask = word >> xoff; + uint16_t lastmask = word << (16 - xoff); + WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff); + WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8); + if (xoff > 0) { + WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8); + } } @@ -1064,8 +1008,8 @@ void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, u */ void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr, unsigned int xoff, int lmode, int mmode) { - write_word_misaligned(draw_buffer_level, wordl, addr, xoff, lmode); - write_word_misaligned(draw_buffer_mask, wordm, addr, xoff, mmode); + write_word_misaligned(draw_buffer_level, wordl, addr, xoff, lmode); + write_word_misaligned(draw_buffer_mask, wordm, addr, xoff, mmode); } /** @@ -1076,22 +1020,22 @@ void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr, */ int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *lookup) { - // First locate the font struct. - if(font > SIZEOF_ARRAY(fonts)) - return 0; // font does not exist, exit.*/ - // Load the font info; IDs are always sequential. - *font_info = fonts[font]; - // Locate character in font lookup table. (If required.) - if(lookup != NULL) - { - *lookup = font_info->lookup[ch]; - if(*lookup == 0xff) - return 0; // character doesn't exist, don't bother writing it. - } - return 1; + // First locate the font struct. + if (font > SIZEOF_ARRAY(fonts)) { + return 0; // font does not exist, exit. + } + // Load the font info; IDs are always sequential. + *font_info = fonts[font]; + // Locate character in font lookup table. (If required.) + if (lookup != NULL) { + *lookup = font_info->lookup[ch]; + if (*lookup == 0xff) { + return 0; // character doesn't exist, don't bother writing it. + } + } + return 1; } - /** * write_char16: Draw a character on the current draw buffer. * Currently supports outlined characters and characters with @@ -1110,7 +1054,7 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font) struct FontEntry font_info; //char lookup = 0; fetch_font_info(0, font, &font_info, NULL); - + // Compute starting address (for x,y) of character. int addr = CALC_BUFF_ADDR(x, y); int wbit = CALC_BIT_IN_WORD(x); @@ -1119,58 +1063,55 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font) // How big is the character? We handle characters up to 8 pixels // wide for now. Support for large characters may be added in future. { - // Ensure we don't overflow. - if(x + wbit > GRAPHICS_WIDTH_REAL) - return; - // Load data pointer. - row = ch * font_info.height; - row_temp = row; - addr_temp = addr; - xshift = 16 - font_info.width; - // We can write mask words easily. - for(yy = y; yy < y + font_info.height; yy++) - { - if(font==3) - write_word_misaligned_OR(draw_buffer_mask, font_mask12x18[row] << xshift, addr, wbit); - else - write_word_misaligned_OR(draw_buffer_mask, font_mask8x10[row] << xshift, addr, wbit); - addr += GRAPHICS_WIDTH_REAL / 8; - row++; - } - // Level bits are more complicated. We need to set or clear - // level bits, but only where the mask bit is set; otherwise, - // we need to leave them alone. To do this, for each word, we - // construct an AND mask and an OR mask, and apply each individually. - row = row_temp; - addr = addr_temp; - for(yy = y; yy < y + font_info.height; yy++) - { - if(font==3) - { - level_bits = font_frame12x18[row]; - //if(!(flags & FONT_INVERT)) // data is normally inverted - level_bits = ~level_bits; - or_mask = font_mask12x18[row] << xshift; - and_mask = (font_mask12x18[row] & level_bits) << xshift; - } else { - level_bits = font_frame8x10[row]; - //if(!(flags & FONT_INVERT)) // data is normally inverted - level_bits = ~level_bits; - or_mask = font_mask8x10[row] << xshift; - and_mask = (font_mask8x10[row] & level_bits) << xshift; - } - write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); - // If we're not bold write the AND mask. - //if(!(flags & FONT_BOLD)) - write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit); - addr += GRAPHICS_WIDTH_REAL / 8; - row++; - } + // Ensure we don't overflow. + if (x + wbit > GRAPHICS_WIDTH_REAL) { + return; + } + // Load data pointer. + row = ch * font_info.height; + row_temp = row; + addr_temp = addr; + xshift = 16 - font_info.width; + // We can write mask words easily. + for (yy = y; yy < y + font_info.height; yy++) { + if (font == 3) { + write_word_misaligned_OR(draw_buffer_mask, font_mask12x18[row] << xshift, addr, wbit); + } else { + write_word_misaligned_OR(draw_buffer_mask, font_mask8x10[row] << xshift, addr, wbit); + } + addr += GRAPHICS_WIDTH_REAL / 8; + row++; + } + // Level bits are more complicated. We need to set or clear + // level bits, but only where the mask bit is set; otherwise, + // we need to leave them alone. To do this, for each word, we + // construct an AND mask and an OR mask, and apply each individually. + row = row_temp; + addr = addr_temp; + for (yy = y; yy < y + font_info.height; yy++) { + if (font == 3) { + level_bits = font_frame12x18[row]; + //if(!(flags & FONT_INVERT)) // data is normally inverted + level_bits = ~level_bits; + or_mask = font_mask12x18[row] << xshift; + and_mask = (font_mask12x18[row] & level_bits) << xshift; + } else { + level_bits = font_frame8x10[row]; + //if(!(flags & FONT_INVERT)) // data is normally inverted + level_bits = ~level_bits; + or_mask = font_mask8x10[row] << xshift; + and_mask = (font_mask8x10[row] & level_bits) << xshift; + } + write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); + // If we're not bold write the AND mask. + //if(!(flags & FONT_BOLD)) + write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit); + addr += GRAPHICS_WIDTH_REAL / 8; + row++; + } } } - - /** * write_char: Draw a character on the current draw buffer. * Currently supports outlined characters and characters with @@ -1195,98 +1136,98 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font) // If font only supports lowercase or uppercase, make the letter // lowercase or uppercase. /*if(font_info.flags & FONT_LOWERCASE_ONLY) - ch = tolower(ch); - if(font_info.flags & FONT_UPPERCASE_ONLY) - ch = toupper(ch);*/ + ch = tolower(ch); + if(font_info.flags & FONT_UPPERCASE_ONLY) + ch = toupper(ch);*/ fetch_font_info(ch, font, &font_info, &lookup); // How big is the character? We handle characters up to 8 pixels // wide for now. Support for large characters may be added in future. - if(font_info.width <= 8) - { - // Ensure we don't overflow. - if(x + wbit > GRAPHICS_WIDTH_REAL) - return; - // Load data pointer. - row = lookup * font_info.height * 2; - row_temp = row; - addr_temp = addr; - xshift = 16 - font_info.width; - // We can write mask words easily. - for(yy = y; yy < y + font_info.height; yy++) - { - write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit); - addr += GRAPHICS_WIDTH_REAL / 8; - row++; - } - // Level bits are more complicated. We need to set or clear - // level bits, but only where the mask bit is set; otherwise, - // we need to leave them alone. To do this, for each word, we - // construct an AND mask and an OR mask, and apply each individually. - row = row_temp; - addr = addr_temp; - for(yy = y; yy < y + font_info.height; yy++) - { - level_bits = font_info.data[row + font_info.height]; - if(!(flags & FONT_INVERT)) // data is normally inverted - level_bits = ~level_bits; - or_mask = font_info.data[row] << xshift; - and_mask = (font_info.data[row] & level_bits) << xshift; - write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); - // If we're not bold write the AND mask. - //if(!(flags & FONT_BOLD)) - write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit); - addr += GRAPHICS_WIDTH_REAL / 8; - row++; - } + if (font_info.width <= 8) { + // Ensure we don't overflow. + if (x + wbit > GRAPHICS_WIDTH_REAL) { + return; + } + // Load data pointer. + row = lookup * font_info.height * 2; + row_temp = row; + addr_temp = addr; + xshift = 16 - font_info.width; + // We can write mask words easily. + for (yy = y; yy < y + font_info.height; yy++) { + write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit); + addr += GRAPHICS_WIDTH_REAL / 8; + row++; + } + // Level bits are more complicated. We need to set or clear + // level bits, but only where the mask bit is set; otherwise, + // we need to leave them alone. To do this, for each word, we + // construct an AND mask and an OR mask, and apply each individually. + row = row_temp; + addr = addr_temp; + for (yy = y; yy < y + font_info.height; yy++) { + level_bits = font_info.data[row + font_info.height]; + if (!(flags & FONT_INVERT)) { + // data is normally inverted + level_bits = ~level_bits; + } + or_mask = font_info.data[row] << xshift; + and_mask = (font_info.data[row] & level_bits) << xshift; + write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit); + // If we're not bold write the AND mask. + //if(!(flags & FONT_BOLD)) + write_word_misaligned_NAND(draw_buffer_level, and_mask, addr, wbit); + addr += GRAPHICS_WIDTH_REAL / 8; + row++; + } } } /** -* calc_text_dimensions: Calculate the dimensions of a -* string in a given font. Supports new lines and -* carriage returns in text. -* -* @param str string to calculate dimensions of -* @param font_info font info structure -* @param xs horizontal spacing -* @param ys vertical spacing -* @param dim return result: struct FontDimensions -*/ + * calc_text_dimensions: Calculate the dimensions of a + * string in a given font. Supports new lines and + * carriage returns in text. + * + * @param str string to calculate dimensions of + * @param font_info font info structure + * @param xs horizontal spacing + * @param ys vertical spacing + * @param dim return result: struct FontDimensions + */ void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim) { int max_length = 0, line_length = 0, lines = 1; - while(*str != 0) - { - line_length++; - if(*str == '\n' || *str == '\r') - { - if(line_length > max_length) - max_length = line_length; - line_length = 0; - lines++; - } - str++; + while (*str != 0) { + line_length++; + if (*str == '\n' || *str == '\r') { + if (line_length > max_length) { + max_length = line_length; + } + line_length = 0; + lines++; + } + str++; + } + if (line_length > max_length) { + max_length = line_length; } - if(line_length > max_length) - max_length = line_length; dim->width = max_length * (font.width + xs); dim->height = lines * (font.height + ys); } /** -* write_string: Draw a string on the screen with certain -* alignment parameters. -* -* @param str string to write -* @param x x coordinate -* @param y y coordinate -* @param xs horizontal spacing -* @param ys horizontal spacing -* @param va vertical align -* @param ha horizontal align -* @param flags flags (passed to write_char) -* @param font font -*/ + * write_string: Draw a string on the screen with certain + * alignment parameters. + * + * @param str string to write + * @param x x coordinate + * @param y y coordinate + * @param xs horizontal spacing + * @param ys horizontal spacing + * @param va vertical align + * @param ha horizontal align + * @param flags flags (passed to write_char) + * @param font font + */ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags, int font) { int xx = 0, yy = 0, xx_original = 0; @@ -1295,55 +1236,61 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un // Determine font info and dimensions/position of the string. fetch_font_info(0, font, &font_info, NULL); calc_text_dimensions(str, font_info, xs, ys, &dim); - switch(va) - { - case TEXT_VA_TOP: yy = y; break; - case TEXT_VA_MIDDLE: yy = y - (dim.height / 2); break; - case TEXT_VA_BOTTOM: yy = y - dim.height; break; + switch (va) { + case TEXT_VA_TOP: + yy = y; + break; + case TEXT_VA_MIDDLE: + yy = y - (dim.height / 2); + break; + case TEXT_VA_BOTTOM: + yy = y - dim.height; + break; } - switch(ha) - { - case TEXT_HA_LEFT: xx = x; break; - case TEXT_HA_CENTER: xx = x - (dim.width / 2); break; - case TEXT_HA_RIGHT: xx = x - dim.width; break; + switch (ha) { + case TEXT_HA_LEFT: + xx = x; + break; + case TEXT_HA_CENTER: + xx = x - (dim.width / 2); + break; + case TEXT_HA_RIGHT: + xx = x - dim.width; + break; } // Then write each character. xx_original = xx; - while(*str != 0) - { - if(*str == '\n' || *str == '\r') - { - yy += ys + font_info.height; - xx = xx_original; - } - else - { - if(xx >= 0 && xx < GRAPHICS_WIDTH_REAL) - { - if(font_info.id<2) - write_char(*str, xx, yy, flags, font); - else - write_char16(*str, xx, yy, font); - } - xx += font_info.width + xs; - } - str++; + while (*str != 0) { + if (*str == '\n' || *str == '\r') { + yy += ys + font_info.height; + xx = xx_original; + } else { + if (xx >= 0 && xx < GRAPHICS_WIDTH_REAL) { + if (font_info.id < 2) { + write_char(*str, xx, yy, flags, font); + } else { + write_char16(*str, xx, yy, font); + } + } + xx += font_info.width + xs; + } + str++; } } /** -* write_string_formatted: Draw a string with format escape -* sequences in it. Allows for complex text effects. -* -* @param str string to write (with format data) -* @param x x coordinate -* @param y y coordinate -* @param xs default horizontal spacing -* @param ys default horizontal spacing -* @param va vertical align -* @param ha horizontal align -* @param flags flags (passed to write_char) -*/ + * write_string_formatted: Draw a string with format escape + * sequences in it. Allows for complex text effects. + * + * @param str string to write (with format data) + * @param x x coordinate + * @param y y coordinate + * @param xs default horizontal spacing + * @param ys default horizontal spacing + * @param va vertical align + * @param ha horizontal align + * @param flags flags (passed to write_char) + */ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags) { int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0; @@ -1360,78 +1307,76 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned // work out a bounding box. We'll parse again for the final output. // This is a simple state machine parser. char *ostr = str; - while(*str) - { - if(*str == '<' && fcode == 1) // escape code: skip - fcode = 0; - if(*str == '<' && fcode == 0) // begin format code? - { - fcode = 1; - fptr = 0; + while (*str) { + if (*str == '<' && fcode == 1) { + // escape code: skip + fcode = 0; + } + if (*str == '<' && fcode == 0) { + // begin format code? + fcode = 1; + fptr = 0; + } + if (*str == '>' && fcode == 1) { + fcode = 0; + if (strcmp(fstack, "B")) { + // switch to "big" font (font #1) + fwidth = bigfontwidth; + fheight = bigfontheight; + } else if (strcmp(fstack, "S")) { + // switch to "small" font (font #0) + fwidth = smallfontwidth; + fheight = smallfontheight; } - if(*str == '>' && fcode == 1) - { - fcode = 0; - if(strcmp(fstack, "B")) // switch to "big" font (font #1) - { - fwidth = bigfontwidth; - fheight = bigfontheight; - } - else if(strcmp(fstack, "S")) // switch to "small" font (font #0) - { - fwidth = smallfontwidth; - fheight = smallfontheight; - } - if(fheight > max_height) - max_height = fheight; - // Skip over this byte. Go to next byte. - str++; - continue; - } - if(*str != '<' && *str != '>' && fcode == 1) - { - // Add to the format stack (up to 10 bytes.) - if(fptr > 10) // stop adding bytes - { - str++; // go to next byte - continue; - } - fstack[fptr++] = *str; - fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) - } - if(fcode == 0) - { - // Not a format code, raw text. - xx += fwidth + xs; - if(*str == '\n') - { - if(xx > max_xx) - max_xx = xx; - xx = x; - yy += fheight + ys; - } + if (fheight > max_height) { + max_height = fheight; } + // Skip over this byte. Go to next byte. str++; + continue; + } + if (*str != '<' && *str != '>' && fcode == 1) { + // Add to the format stack (up to 10 bytes.) + if (fptr > 10) { + // stop adding bytes + str++; // go to next byte + continue; + } + fstack[fptr++] = *str; + fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) + } + if (fcode == 0) { + // Not a format code, raw text. + xx += fwidth + xs; + if (*str == '\n') { + if (xx > max_xx) { + max_xx = xx; + } + xx = x; + yy += fheight + ys; + } + } + str++; } // Reset string pointer. str = ostr; // Now we've parsed it and got a bbox, we need to work out the dimensions of it // and how to align it. /*int width = max_xx - x; - int height = yy - y; - int ay, ax; - switch(va) - { - case TEXT_VA_TOP: ay = yy; break; - case TEXT_VA_MIDDLE: ay = yy - (height / 2); break; - case TEXT_VA_BOTTOM: ay = yy - height; break; - } - switch(ha) - { - case TEXT_HA_LEFT: ax = x; break; - case TEXT_HA_CENTER: ax = x - (width / 2); break; - case TEXT_HA_RIGHT: ax = x - width; break; - }*/ + int height = yy - y; + int ay, ax; + switch(va) + { + case TEXT_VA_TOP: ay = yy; break; + case TEXT_VA_MIDDLE: ay = yy - (height / 2); break; + case TEXT_VA_BOTTOM: ay = yy - height; break; + } + switch(ha) + { + case TEXT_HA_LEFT: ax = x; break; + case TEXT_HA_CENTER: ax = x - (width / 2); break; + case TEXT_HA_RIGHT: ax = x - width; break; + }*/ // So ax,ay is our new text origin. Parse the text format again and paint // the text on the display. fcode = 0; @@ -1439,204 +1384,196 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned font = 0; xx = 0; yy = 0; - while(*str) - { - if(*str == '<' && fcode == 1) // escape code: skip - fcode = 0; - if(*str == '<' && fcode == 0) // begin format code? - { - fcode = 1; - fptr = 0; - } - if(*str == '>' && fcode == 1) - { - fcode = 0; - if(strcmp(fstack, "B")) // switch to "big" font (font #1) - { - fwidth = bigfontwidth; - fheight = bigfontheight; - font = 1; - } - else if(strcmp(fstack, "S")) // switch to "small" font (font #0) - { - fwidth = smallfontwidth; - fheight = smallfontheight; - font = 0; - } - // Skip over this byte. Go to next byte. - str++; - continue; - } - if(*str != '<' && *str != '>' && fcode == 1) - { - // Add to the format stack (up to 10 bytes.) - if(fptr > 10) // stop adding bytes - { - str++; // go to next byte - continue; - } - fstack[fptr++] = *str; - fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) - } - if(fcode == 0) - { - // Not a format code, raw text. So we draw it. - // TODO - different font sizes. - write_char(*str, xx, yy + (max_height - fheight), flags, font); - xx += fwidth + xs; - if(*str == '\n') - { - if(xx > max_xx) - max_xx = xx; - xx = x; - yy += fheight + ys; - } + while (*str) { + if (*str == '<' && fcode == 1) { + // escape code: skip + fcode = 0; + } + if (*str == '<' && fcode == 0) { + // begin format code? + fcode = 1; + fptr = 0; + } + if (*str == '>' && fcode == 1) { + fcode = 0; + if (strcmp(fstack, "B")) { + // switch to "big" font (font #1) + fwidth = bigfontwidth; + fheight = bigfontheight; + font = 1; + } else if (strcmp(fstack, "S")) { + // switch to "small" font (font #0) + fwidth = smallfontwidth; + fheight = smallfontheight; + font = 0; } + // Skip over this byte. Go to next byte. str++; + continue; + } + if (*str != '<' && *str != '>' && fcode == 1) { + // Add to the format stack (up to 10 bytes.) + if (fptr > 10) { + // stop adding bytes + str++; // go to next byte + continue; + } + fstack[fptr++] = *str; + fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.) + } + if (fcode == 0) { + // Not a format code, raw text. So we draw it. + // TODO - different font sizes. + write_char(*str, xx, yy + (max_height - fheight), flags, font); + xx += fwidth + xs; + if (*str == '\n') { + if (xx > max_xx) { + max_xx = xx; + } + xx = x; + yy += fheight + ys; + } + } + str++; } } - //SUPEROSD- - // graphics void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t size) { - int16_t a = mySin(roll+360); - int16_t b = myCos(roll+360); - int16_t c = mySin(roll+90+360)*5/100; - int16_t d = myCos(roll+90+360)*5/100; + int16_t a = mySin(roll + 360); + int16_t b = myCos(roll + 360); + int16_t c = mySin(roll + 90 + 360) * 5 / 100; + int16_t d = myCos(roll + 90 + 360) * 5 / 100; - int16_t k; - int16_t l; + int16_t k; + int16_t l; - int16_t indi30x1=myCos(30)*(size/2+1) / 100; - int16_t indi30y1=mySin(30)*(size/2+1) / 100; + int16_t indi30x1 = myCos(30) * (size / 2 + 1) / 100; + int16_t indi30y1 = mySin(30) * (size / 2 + 1) / 100; - int16_t indi30x2=myCos(30)*(size/2+4) / 100; - int16_t indi30y2=mySin(30)*(size/2+4) / 100; + int16_t indi30x2 = myCos(30) * (size / 2 + 4) / 100; + int16_t indi30y2 = mySin(30) * (size / 2 + 4) / 100; - int16_t indi60x1=myCos(60)*(size/2+1) / 100; - int16_t indi60y1=mySin(60)*(size/2+1) / 100; + int16_t indi60x1 = myCos(60) * (size / 2 + 1) / 100; + int16_t indi60y1 = mySin(60) * (size / 2 + 1) / 100; - int16_t indi60x2=myCos(60)*(size/2+4) / 100; - int16_t indi60y2=mySin(60)*(size/2+4) / 100; + int16_t indi60x2 = myCos(60) * (size / 2 + 4) / 100; + int16_t indi60y2 = mySin(60) * (size / 2 + 4) / 100; - pitch=pitch%90; - if(pitch>90) - { - pitch=pitch-90; - } - if(pitch<-90) - { - pitch=pitch+90; - } - a = (a * (size/2)) / 100; - b = (b * (size/2)) / 100; + pitch = pitch % 90; + if (pitch > 90) { + pitch = pitch - 90; + } + if (pitch < -90) { + pitch = pitch + 90; + } + a = (a * (size / 2)) / 100; + b = (b * (size / 2)) / 100; - if(roll<-90 || roll>90) - pitch=pitch*-1; - k = a*pitch/90; - l = b*pitch/90; + if (roll < -90 || roll > 90) { + pitch = pitch * -1; + } + k = a * pitch / 90; + l = b * pitch / 90; - // scale - //0 - //drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1); - //drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1); - write_line_outlined((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1,0,0,0,1); - write_line_outlined((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1,0,0,0,1); + // scale + //0 + //drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1); + //drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1); + write_line_outlined((x) - 1 - (size / 2 + 4), (y) - 1, (x) - 1 - (size / 2 + 1), (y) - 1, 0, 0, 0, 1); + write_line_outlined((x) - 1 + (size / 2 + 4), (y) - 1, (x) - 1 + (size / 2 + 1), (y) - 1, 0, 0, 0, 1); - //30 - //drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2); - //drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2); - write_line_outlined((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2,0,0,0,1); - write_line_outlined((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2,0,0,0,1); - //60 - //drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2); - //drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2); - write_line_outlined((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2,0,0,0,1); - write_line_outlined((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2,0,0,0,1); - //90 - //drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1)); - write_line_outlined((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1),0,0,0,1); + //30 + //drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2); + //drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2); + write_line_outlined((x) - 1 + indi30x1, (y) - 1 - indi30y1, (x) - 1 + indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1); + write_line_outlined((x) - 1 - indi30x1, (y) - 1 - indi30y1, (x) - 1 - indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1); + //60 + //drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2); + //drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2); + write_line_outlined((x) - 1 + indi60x1, (y) - 1 - indi60y1, (x) - 1 + indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1); + write_line_outlined((x) - 1 - indi60x1, (y) - 1 - indi60y1, (x) - 1 - indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1); + //90 + //drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1)); + write_line_outlined((x) - 1, (y) - 1 - (size / 2 + 4), (x) - 1, (y) - 1 - (size / 2 + 1), 0, 0, 0, 1); + //roll + //drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line + write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); //Direction line + //"wingtips" + //drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c); + //drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a); + write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 - b + d, (y) - 1 + a - c, 0, 0, 0, 1); + write_line_outlined((x) - 1 + b + d, (y) - 1 - a - c, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); - //roll - //drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line - write_line_outlined((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a,0,0,0,1); //Direction line - //"wingtips" - //drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c); - //drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a); - write_line_outlined((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c,0,0,0,1); - write_line_outlined((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a,0,0,0,1); + //pitch + //drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l); + write_line_outlined((x) - 1, (y) - 1, (x) - 1 - k, (y) - 1 - l, 0, 0, 0, 1); - //pitch - //drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l); - write_line_outlined((x)-1, (y)-1, (x)-1 - k, (y)-1 - l,0,0,0,1); - - - //drawCircle(x-1, y-1, 5); - //write_circle_outlined(x-1, y-1, 5,0,0,0,1); - //drawCircle(x-1, y-1, size/2+4); - //write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1); + //drawCircle(x-1, y-1, 5); + //write_circle_outlined(x-1, y-1, 5,0,0,0,1); + //drawCircle(x-1, y-1, size/2+4); + //write_circle_outlined(x-1, y-1, size/2+4,0,0,0,1); } void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size) { - int i=0; - int batteryLines; - //top - /*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1); - drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1); + int i = 0; + int batteryLines; + //top + /*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1); + drawLine((x)-1+(size/2-size/4), (y)-1+1, (x)-1 + (size/2+size/4), (y)-1+1); - drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2); - //bottom - drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3); - //left - drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3); + drawLine((x)-1, (y)-1+2, (x)-1 + size, (y)-1+2); + //bottom + drawLine((x)-1, (y)-1+size*3, (x)-1 + size, (y)-1+size*3); + //left + drawLine((x)-1, (y)-1+2, (x)-1, (y)-1+size*3); - //right - drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/ + //right + drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/ - write_rectangle_outlined((x)-1, (y)-1+2,size,size*3,0,1); - write_vline_lm((x)-1+(size/2+size/4)+1,(y)-2,(y)-1+1,0,1); - write_vline_lm((x)-1+(size/2-size/4)-1,(y)-2,(y)-1+1,0,1); - write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-2,0,1); - write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-1,1,1); - write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-1+1,1,1); + write_rectangle_outlined((x) - 1, (y) - 1 + 2, size, size * 3, 0, 1); + write_vline_lm((x) - 1 + (size / 2 + size / 4) + 1, (y) - 2, (y) - 1 + 1, 0, 1); + write_vline_lm((x) - 1 + (size / 2 - size / 4) - 1, (y) - 2, (y) - 1 + 1, 0, 1); + write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 2, 0, 1); + write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1, 1, 1); + write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1 + 1, 1, 1); - batteryLines = battery*(size*3-2)/100; - for(i=0;i= -1 && halign <= 1); - // Compute the position of the elements. - int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0; - if(halign == -1) - { - majtick_start = x; - majtick_end = x + majtick_len; - mintick_start = x; - mintick_end = x + mintick_len; - boundtick_start = x; - boundtick_end = x + boundtick_len; - } - else if(halign == +1) - { - x=x-GRAPHICS_HDEADBAND; - majtick_start = GRAPHICS_WIDTH_REAL - x - 1; - majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1; - mintick_start = GRAPHICS_WIDTH_REAL - x - 1; - mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1; - boundtick_start = GRAPHICS_WIDTH_REAL - x - 1; - boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1; - } - // Retrieve width of large font (font #0); from this calculate the x spacing. - fetch_font_info(0, 0, &font_info, NULL); - int arrow_len = (font_info.height / 2) + 1; // FIXME, font info being loaded correctly?? - int text_x_spacing = arrow_len; - int max_text_y = 0, text_length = 0; - int small_font_char_width = font_info.width + 1; // +1 for horizontal spacing = 1 - // For -(range / 2) to +(range / 2), draw the scale. - int range_2 = range / 2; //, height_2 = height / 2; - int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0, - // Iterate through each step. - for(r = -range_2; r <= +range_2; r++) - { - style = 0; - rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape - rv = -rr + range_2; // for number display - if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE) - rr += majtick_step / 2; - if(rr % majtick_step == 0) - style = 1; // major tick - else if(rr % mintick_step == 0) - style = 2; // minor tick + char temp[15]; //, temp2[15]; + struct FontEntry font_info; + struct FontDimensions dim; + // Halign should be in a small span. + //MY_ASSERT(halign >= -1 && halign <= 1); + // Compute the position of the elements. + int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0; + if (halign == -1) { + majtick_start = x; + majtick_end = x + majtick_len; + mintick_start = x; + mintick_end = x + mintick_len; + boundtick_start = x; + boundtick_end = x + boundtick_len; + } else if (halign == +1) { + x = x - GRAPHICS_HDEADBAND; + majtick_start = GRAPHICS_WIDTH_REAL - x - 1; + majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1; + mintick_start = GRAPHICS_WIDTH_REAL - x - 1; + mintick_end = GRAPHICS_WIDTH_REAL - x - mintick_len - 1; + boundtick_start = GRAPHICS_WIDTH_REAL - x - 1; + boundtick_end = GRAPHICS_WIDTH_REAL - x - boundtick_len - 1; + } + // Retrieve width of large font (font #0); from this calculate the x spacing. + fetch_font_info(0, 0, &font_info, NULL); + int arrow_len = (font_info.height / 2) + 1; // FIXME, font info being loaded correctly?? + int text_x_spacing = arrow_len; + int max_text_y = 0, text_length = 0; + int small_font_char_width = font_info.width + 1; // +1 for horizontal spacing = 1 + // For -(range / 2) to +(range / 2), draw the scale. + int range_2 = range / 2; //, height_2 = height / 2; + int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0, + // Iterate through each step. + for (r = -range_2; r <= +range_2; r++) { + style = 0; + rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape + rv = -rr + range_2; // for number display + if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE) + rr += majtick_step / 2; + if (rr % majtick_step == 0) + style = 1; // major tick + else if (rr % mintick_step == 0) + style = 2; // minor tick + else + style = 0; + if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0) + continue; + if (style) { + // Calculate y position. + ys = ((long int) (r * height) / (long int) range) + y; + //sprintf(temp, "ys=%d", ys); + //con_puts(temp, 0); + // Depending on style, draw a minor or a major tick. + if (style == 1) { + write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1); + memset(temp, ' ', 10); + //my_itoa(rv, temp); + sprintf(temp, "%d", rv); + text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin + if (text_length > max_text_y) + max_text_y = text_length; + if (halign == -1) + write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1); else - style = 0; - if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0) - continue; - if(style) - { - // Calculate y position. - ys = ((long int)(r * height) / (long int)range) + y; - //sprintf(temp, "ys=%d", ys); - //con_puts(temp, 0); - // Depending on style, draw a minor or a major tick. - if(style == 1) - { - write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1); - memset(temp, ' ', 10); - //my_itoa(rv, temp); - sprintf(temp,"%d",rv); - text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin - if(text_length > max_text_y) - max_text_y = text_length; - if(halign == -1) - write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1); - else - write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1); - } - else if(style == 2) - write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1); - } + write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1); + } else if (style == 2) + write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1); } - // Generate the string for the value, as well as calculating its dimensions. - memset(temp, ' ', 10); - //my_itoa(v, temp); - sprintf(temp,"%d",v); - // TODO: add auto-sizing. - calc_text_dimensions(temp, font_info, 1, 0, &dim); - int xx = 0, i = 0; - if(halign == -1) - xx = majtick_end + text_x_spacing; - else - xx = majtick_end - text_x_spacing; - // Draw an arrow from the number to the point. - for(i = 0; i < arrow_len; i++) - { - if(halign == -1) - { - write_pixel_lm(xx - arrow_len + i, y - i - 1, 1, 1); - write_pixel_lm(xx - arrow_len + i, y + i - 1, 1, 1); - write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y - i - 1, 0, 1); - write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y + i - 1, 0, 1); - } - else - { - write_pixel_lm(xx + arrow_len - i, y - i - 1, 1, 1); - write_pixel_lm(xx + arrow_len - i, y + i - 1, 1, 1); - write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y - i - 1, 0, 1); - write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y + i - 1, 0, 1); - } - // FIXME - // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y - i - 1, 1, 1); - // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y + i - 1, 1, 1); + } + // Generate the string for the value, as well as calculating its dimensions. + memset(temp, ' ', 10); + //my_itoa(v, temp); + sprintf(temp, "%d", v); + // TODO: add auto-sizing. + calc_text_dimensions(temp, font_info, 1, 0, &dim); + int xx = 0, i = 0; + if (halign == -1) + xx = majtick_end + text_x_spacing; + else + xx = majtick_end - text_x_spacing; + // Draw an arrow from the number to the point. + for (i = 0; i < arrow_len; i++) { + if (halign == -1) { + write_pixel_lm(xx - arrow_len + i, y - i - 1, 1, 1); + write_pixel_lm(xx - arrow_len + i, y + i - 1, 1, 1); + write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y - i - 1, 0, 1); + write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y + i - 1, 0, 1); + } else { + write_pixel_lm(xx + arrow_len - i, y - i - 1, 1, 1); + write_pixel_lm(xx + arrow_len - i, y + i - 1, 1, 1); + write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y - i - 1, 0, 1); + write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y + i - 1, 0, 1); } - if(halign == -1) - { - write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1); - write_hline_lm(xx, xx + dim.width - 1, y + arrow_len - 2, 1, 1); - write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1); - } - else - { - write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1); - write_hline_lm(xx, xx - dim.width - 1, y + arrow_len - 2, 1, 1); - write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1); - } - // Draw the text. - if(halign == -1) - write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0); - else - write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0); - // Then, add a slow cut off on the edges, so the text doesn't sharply - // disappear. We simply clear the areas above and below the ticker, and we - // use little markers on the edges. - if(halign == -1) - { - write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, 0); - write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, 0); - } - else - { - write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0); - write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0); - } - write_hline_outlined(boundtick_start, boundtick_end, y + (height / 2), 2, 2, 0, 1); - write_hline_outlined(boundtick_start, boundtick_end, y - (height / 2), 2, 2, 0, 1); + // FIXME + // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y - i - 1, 1, 1); + // write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y + i - 1, 1, 1); + } + if (halign == -1) { + write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1); + write_hline_lm(xx, xx + dim.width - 1, y + arrow_len - 2, 1, 1); + write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1); + } else { + write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1); + write_hline_lm(xx, xx - dim.width - 1, y + arrow_len - 2, 1, 1); + write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1); + } + // Draw the text. + if (halign == -1) + write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0); + else + write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0); + // Then, add a slow cut off on the edges, so the text doesn't sharply + // disappear. We simply clear the areas above and below the ticker, and we + // use little markers on the edges. + if (halign == -1) { + write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, + 0); + write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, + 0); + } else { + write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0); + write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0); + } + write_hline_outlined(boundtick_start, boundtick_end, y + (height / 2), 2, 2, 0, 1); + write_hline_outlined(boundtick_start, boundtick_end, y - (height / 2), 2, 2, 0, 1); } /** @@ -1815,613 +1738,577 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei */ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mintick_step, int majtick_step, int mintick_len, int majtick_len, int flags) { - v %= 360; // wrap, just in case. - struct FontEntry font_info; - int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, textoffset = 0; - char headingstr[4]; - majtick_start = y; - majtick_end = y - majtick_len; - mintick_start = y; - mintick_end = y - mintick_len; - textoffset = 8; - int r, style, rr, xs; // rv, - int range_2 = range / 2; - for(r = -range_2; r <= +range_2; r++) - { - style = 0; - rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track - //rv = -rr + range_2; // for number display - if(rr % majtick_step == 0) - style = 1; // major tick - else if(rr % mintick_step == 0) - style = 2; // minor tick - if(style) - { - // Calculate x position. - xs = ((long int)(r * width) / (long int)range) + x; - // Draw it. - if(style == 1) - { - write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1); - // Draw heading above this tick. - // If it's not one of north, south, east, west, draw the heading. - // Otherwise, draw one of the identifiers. - if(rr % 90 != 0) - { - // We abbreviate heading to two digits. This has the side effect of being easy to compute. - headingstr[0] = '0' + (rr / 100); - headingstr[1] = '0' + ((rr / 10) % 10); - headingstr[2] = 0; - headingstr[3] = 0; // nul to terminate - } - else - { - switch(rr) - { - case 0: headingstr[0] = 'N'; break; - case 90: headingstr[0] = 'E'; break; - case 180: headingstr[0] = 'S'; break; - case 270: headingstr[0] = 'W'; break; - } - headingstr[1] = 0; - headingstr[2] = 0; - headingstr[3] = 0; - } - // +1 fudge...! - write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1); - } - else if(style == 2) - write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1); + v %= 360; // wrap, just in case. + struct FontEntry font_info; + int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, textoffset = 0; + char headingstr[4]; + majtick_start = y; + majtick_end = y - majtick_len; + mintick_start = y; + mintick_end = y - mintick_len; + textoffset = 8; + int r, style, rr, xs; // rv, + int range_2 = range / 2; + for (r = -range_2; r <= +range_2; r++) { + style = 0; + rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track + //rv = -rr + range_2; // for number display + if (rr % majtick_step == 0) + style = 1; // major tick + else if (rr % mintick_step == 0) + style = 2; // minor tick + if (style) { + // Calculate x position. + xs = ((long int) (r * width) / (long int) range) + x; + // Draw it. + if (style == 1) { + write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1); + // Draw heading above this tick. + // If it's not one of north, south, east, west, draw the heading. + // Otherwise, draw one of the identifiers. + if (rr % 90 != 0) { + // We abbreviate heading to two digits. This has the side effect of being easy to compute. + headingstr[0] = '0' + (rr / 100); + headingstr[1] = '0' + ((rr / 10) % 10); + headingstr[2] = 0; + headingstr[3] = 0; // nul to terminate + } else { + switch (rr) { + case 0: + headingstr[0] = 'N'; + break; + case 90: + headingstr[0] = 'E'; + break; + case 180: + headingstr[0] = 'S'; + break; + case 270: + headingstr[0] = 'W'; + break; + } + headingstr[1] = 0; + headingstr[2] = 0; + headingstr[3] = 0; } + // +1 fudge...! + write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1); + } else if (style == 2) + write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1); } - // Then, draw a rectangle with the present heading in it. - // We want to cover up any other markers on the bottom. - // First compute font size. - fetch_font_info(0, 3, &font_info, NULL); - int text_width = (font_info.width + 1) * 3; - int rect_width = text_width + 2; - write_filled_rectangle_lm(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1); - write_rectangle_outlined(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1); - headingstr[0] = '0' + (v / 100); - headingstr[1] = '0' + ((v / 10) % 10); - headingstr[2] = '0' + (v % 10); - headingstr[3] = 0; - write_string(headingstr, x + 1, majtick_start + textoffset+2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3); + } + // Then, draw a rectangle with the present heading in it. + // We want to cover up any other markers on the bottom. + // First compute font size. + fetch_font_info(0, 3, &font_info, NULL); + int text_width = (font_info.width + 1) * 3; + int rect_width = text_width + 2; + write_filled_rectangle_lm(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1); + write_rectangle_outlined(x - (rect_width / 2), majtick_start + 2, rect_width, font_info.height + 2, 0, 1); + headingstr[0] = '0' + (v / 100); + headingstr[1] = '0' + ((v / 10) % 10); + headingstr[2] = '0' + (v % 10); + headingstr[3] = 0; + write_string(headingstr, x + 1, majtick_start + textoffset + 2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3); } // CORE draw routines end here -void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t size ) +void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t size) { - float alpha; - uint8_t vertical=0,horizontal=0; - int16_t x1,x2; - int16_t y1,y2; - int16_t refx,refy; - alpha=DEG2RAD(angle); - refx=l_x + size/2; - refy=l_y + size/2; + float alpha; + uint8_t vertical = 0, horizontal = 0; + int16_t x1, x2; + int16_t y1, y2; + int16_t refx, refy; + alpha = DEG2RAD(angle); + refx = l_x + size / 2; + refy = l_y + size / 2; - // - float k=0; - float dx = sinf(alpha)*(pitch/90.0f*(size/2)); - float dy = cosf(alpha)*(pitch/90.0f*(size/2)); - int16_t x0 = (size/2)-dx; - int16_t y0 = (size/2)+dy; - // calculate the line function - if((angle != 90) && (angle != -90)) - { - k = tanf(alpha); - vertical = 0; - if(k==0) - { - horizontal=1; - } - } - else - { - vertical = 1; - } + // + float k = 0; + float dx = sinf(alpha) * (pitch / 90.0f * (size / 2)); + float dy = cosf(alpha) * (pitch / 90.0f * (size / 2)); + int16_t x0 = (size / 2) - dx; + int16_t y0 = (size / 2) + dy; + // calculate the line function + if ((angle != 90) && (angle != -90)) { + k = tanf(alpha); + vertical = 0; + if (k == 0) { + horizontal = 1; + } + } else { + vertical = 1; + } - // crossing point of line - if(!vertical && !horizontal) - { - // y-y0=k(x-x0) - int16_t x=0; - int16_t y=k*(x-x0)+y0; - // find right crossing point - x1=x; - y1=y; - if(y<0) - { - y1=0; - x1=((y1-y0)+k*x0)/k; - } - if(y>size) - { - y1=size; - x1=((y1-y0)+k*x0)/k; - } - // left crossing point - x=size; - y=k*(x-x0)+y0; - x2=x; - y2=y; - if(y<0) - { - y2=0; - x2=((y2-y0)+k*x0)/k; - } - if(y>size) - { - y2=size; - x2=((y2-y0)+k*x0)/k; - } - // move to location - // horizon line - write_line_outlined(x1+l_x,y1+l_y,x2+l_x,y2+l_y,0,0,0,1); - //fill - if(angle<=0 && angle>-90) - { - //write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); - for(int i=y2;isize) - x2=size; - if(x2<0) - x2=0; - write_hline_lm(x2+l_x,size+l_x,i+l_y,1,1); - } - } - else if(angle<-90) - { - //write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); - for(int i=0;isize) - x2=size; - if(x2<0) - x2=0; - write_hline_lm(size+l_x,x2+l_x,i+l_y,1,1); - } - } - else if(angle>0 && angle<90) - { - //write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); - for(int i=y1;isize) - x2=size; - if(x2<0) - x2=0; - write_hline_lm(0+l_x,x2+l_x,i+l_y,1,1); - } - } - else if(angle>90) - { - //write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); - for(int i=0;isize) - x2=size; - if(x2<0) - x2=0; - write_hline_lm(x2+l_x,0+l_x,i+l_y,1,1); - } - } - } - else if(vertical) - { - // horizon line - write_line_outlined(x0+l_x,0+l_y,x0+l_x,size+l_y,0,0,0,1); - if(angle==90) - { - //write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); - for(int i=0;i size) { + y1 = size; + x1 = ((y1 - y0) + k * x0) / k; + } + // left crossing point + x = size; + y = k * (x - x0) + y0; + x2 = x; + y2 = y; + if (y < 0) { + y2 = 0; + x2 = ((y2 - y0) + k * x0) / k; + } + if (y > size) { + y2 = size; + x2 = ((y2 - y0) + k * x0) / k; + } + // move to location + // horizon line + write_line_outlined(x1 + l_x, y1 + l_y, x2 + l_x, y2 + l_y, 0, 0, 0, 1); + //fill + if (angle <= 0 && angle > -90) { + //write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + for (int i = y2; i < size; i++) { + x2 = ((i - y0) + k * x0) / k; + if (x2 > size) { + x2 = size; + } + if (x2 < 0) { + x2 = 0; + } + write_hline_lm(x2 + l_x, size + l_x, i + l_y, 1, 1); + } + } else if (angle < -90) { + //write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + for (int i = 0; i < y2; i++) { + x2 = ((i - y0) + k * x0) / k; + if (x2 > size) { + x2 = size; + } + if (x2 < 0) { + x2 = 0; + } + write_hline_lm(size + l_x, x2 + l_x, i + l_y, 1, 1); + } + } else if (angle > 0 && angle < 90) { + //write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + for (int i = y1; i < size; i++) { + x2 = ((i - y0) + k * x0) / k; + if (x2 > size) { + x2 = size; + } + if (x2 < 0) { + x2 = 0; + } + write_hline_lm(0 + l_x, x2 + l_x, i + l_y, 1, 1); + } + } else if (angle > 90) { + //write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + for (int i = 0; i < y1; i++) { + x2 = ((i - y0) + k * x0) / k; + if (x2 > size) { + x2 = size; + } + if (x2 < 0) { + x2 = 0; + } + write_hline_lm(x2 + l_x, 0 + l_x, i + l_y, 1, 1); + } + } + } else if (vertical) { + // horizon line + write_line_outlined(x0 + l_x, 0 + l_y, x0 + l_x, size + l_y, 0, 0, 0, 1); + if (angle == 90) { + //write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + for (int i = 0; i < size; i++) { + write_hline_lm(0 + l_x, x0 + l_x, i + l_y, 1, 1); + } + } else { + //write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + for (int i = 0; i < size; i++) { + write_hline_lm(size + l_x, x0 + l_x, i + l_y, 1, 1); + } + } + } else if (horizontal) { + // horizon line + write_hline_outlined(0 + l_x, size + l_x, y0 + l_y, 0, 0, 0, 1); + if (angle < 0) { + //write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + for (int i = 0; i < y0; i++) { + write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1); + } + } else { + //write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); + for (int i = y0; i < size; i++) { + write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1); + } + } + } - //sides - write_line_outlined(l_x,l_y,l_x,l_y+size,0,0,0,1); - write_line_outlined(l_x+size,l_y,l_x+size,l_y+size,0,0,0,1); - //plane - write_line_outlined(refx-5,refy,refx+6,refy,0,0,0,1); - write_line_outlined(refx,refy,refx,refy-3,0,0,0,1); + //sides + write_line_outlined(l_x, l_y, l_x, l_y + size, 0, 0, 0, 1); + write_line_outlined(l_x + size, l_y, l_x + size, l_y +s ize, 0, 0, 0, 1); + //plane + write_line_outlined(refx - 5, refy, refx + 6, refy, 0, 0, 0, 1); + write_line_outlined(refx, refy, refx, refy - 3, 0, 0, 0, 1); } - -void introText(){ - write_string("ver 0.2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); +void introText() +{ + write_string("ver 0.2", APPLY_HDEADBAND((GRAPHICS_RIGHT / 2)), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3); } -void introGraphics() { - /* logo */ - int image=0; - struct splashEntry splash_info; - splash_info = splash[image]; +void introGraphics() +{ + /* logo */ + int image = 0; + struct splashEntry splash_info; + splash_info = splash[image]; - copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2),image); + copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - (splash_info.width) / 2), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2 - (splash_info.height) / 2), image); - /* frame */ - drawBox(APPLY_HDEADBAND(0),APPLY_VDEADBAND(0),APPLY_HDEADBAND(GRAPHICS_RIGHT-8),APPLY_VDEADBAND(GRAPHICS_BOTTOM)); + /* frame */ + drawBox(APPLY_HDEADBAND(0), APPLY_VDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT-8), APPLY_VDEADBAND(GRAPHICS_BOTTOM)); - // Must mask out last half-word because SPI keeps clocking it out otherwise - for (uint32_t i = 0; i < 8; i++) { - write_vline( draw_buffer_level,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0); - write_vline( draw_buffer_mask,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0); - } + // Must mask out last half-word because SPI keeps clocking it out otherwise + for (uint32_t i = 0; i < 8; i++) { + write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0); + write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0); + } } void calcHomeArrow(int16_t m_yaw) { - HomeLocationData home; - HomeLocationGet (&home); - GPSPositionData gpsData; - GPSPositionGet (&gpsData); + HomeLocationData home; + HomeLocationGet(&home); + GPSPositionData gpsData; + GPSPositionGet(&gpsData); - /** http://www.movable-type.co.uk/scripts/latlong.html **/ + /** http://www.movable-type.co.uk/scripts/latlong.html **/ float lat1, lat2, lon1, lon2, a, c, d, x, y, brng, u2g; float elevation; - float gcsAlt=home.Altitude; // Home MSL altitude - float uavAlt=gpsData.Altitude; // UAV MSL altitude - float dAlt=uavAlt-gcsAlt; // Altitude difference + float gcsAlt = home.Altitude; // Home MSL altitude + float uavAlt = gpsData.Altitude; // UAV MSL altitude + float dAlt = uavAlt - gcsAlt; // Altitude difference // Convert to radians - lat1 = DEG2RAD(home.Latitude)/10000000.0f; // Home lat - lon1 = DEG2RAD(home.Longitude)/10000000.0f; // Home lon - lat2 = DEG2RAD(gpsData.Latitude)/10000000.0f; // UAV lat - lon2 = DEG2RAD(gpsData.Longitude)/10000000.0f; // UAV lon + lat1 = DEG2RAD(home.Latitude) / 10000000.0f; // Home lat + lon1 = DEG2RAD(home.Longitude) / 10000000.0f; // Home lon + lat2 = DEG2RAD(gpsData.Latitude) / 10000000.0f; // UAV lat + lon2 = DEG2RAD(gpsData.Longitude) / 10000000.0f; // UAV lon // Bearing /** - var y = Math.sin(dLon) * Math.cos(lat2); - var x = Math.cos(lat1)*Math.sin(lat2) - - Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); - var brng = Math.atan2(y, x).toDeg(); - **/ - y = sinf(lon2-lon1) * cosf(lat2); - x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2-lon1); + var y = Math.sin(dLon) * Math.cos(lat2); + var x = Math.cos(lat1)*Math.sin(lat2) - + Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); + var brng = Math.atan2(y, x).toDeg(); + **/ + y = sinf(lon2 - lon1) * cosf(lat2); + x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2 - lon1); brng = RAD2DEG(atan2f(y,x)); - if(brng<0) - brng+=360; + if (brng < 0) { + brng += 360; + } // yaw corrected bearing, needs compass - u2g=brng-180-m_yaw; - if(u2g<0) - u2g+=360; + u2g = brng - 180 - m_yaw; + if (u2g < 0) { + u2g += 360; + } // Haversine formula for distance /** - var R = 6371; // km - var dLat = (lat2-lat1).toRad(); - var dLon = (lon2-lon1).toRad(); - var a = Math.sin(dLat/2) * Math.sin(dLat/2) + - Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) * - Math.sin(dLon/2) * Math.sin(dLon/2); - var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); - var d = R * c; - **/ - a = sinf((lat2-lat1)/2) * sinf((lat2-lat1)/2) + - cosf(lat1) * cosf(lat2) * - sinf((lon2-lon1)/2) * sinf((lon2-lon1)/2); - c = 2 * atan2f(sqrtf(a), sqrtf(1-a)); + var R = 6371; // km + var dLat = (lat2-lat1).toRad(); + var dLon = (lon2-lon1).toRad(); + var a = Math.sin(dLat/2) * Math.sin(dLat/2) + + Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) * + Math.sin(dLon/2) * Math.sin(dLon/2); + var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); + var d = R * c; + **/ + a = sinf((lat2 - lat1) / 2) * sinf((lat2 - lat1) / 2) + cosf(lat1) * cosf(lat2) * sinf((lon2 - lon1) / 2) * sinf((lon2 - lon1) / 2); + c = 2 * atan2f(sqrtf(a), sqrtf(1 - a)); d = 6371 * 1000 * c; // Elevation v depends servo direction - if(d!=0) - elevation = 90-RAD2DEG(atanf(dAlt/d)); + if (d != 0) + elevation = 90 - RAD2DEG(atanf(dAlt/d)); else elevation = 0; //! TODO: sanity check - char temp[50]={0}; - sprintf(temp,"hea:%d",(int)brng); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp,"ele:%d",(int)elevation); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp,"dis:%d",(int)d); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp,"u2g:%d",(int)u2g); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + char temp[50] = + { 0 }; + sprintf(temp, "hea:%d", (int) brng); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "ele:%d", (int) elevation); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "dis:%d", (int) d); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "u2g:%d", (int) u2g); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp,"%c%c",(int)(u2g/22.5f)*2+0x90,(int)(u2g/22.5f)*2+0x91); - write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); + sprintf(temp, "%c%c", (int) (u2g / 22.5f) * 2 + 0x90, (int) (u2g / 22.5f) * 2 + 0x91); + write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); } -int lama=10; +int lama = 10; int lama_loc[2][30]; void lamas(void) { - char temp[10]={0}; - lama++; - if(lama%10==0) - { - for(int z=0; z<30;z++) - { + char temp[10] = + { 0 }; + lama++; + if (lama % 10 == 0) { + for (int z = 0; z < 30; z++) { - lama_loc[0][z]=rand()%(GRAPHICS_RIGHT-10); - lama_loc[1][z]=rand()%(GRAPHICS_BOTTOM-10); - } - } - for(int z=0; z<30;z++) - { - sprintf(temp,"%c",0xe8+(lama_loc[0][z]%2)); - write_string(temp,APPLY_HDEADBAND(lama_loc[0][z]),APPLY_VDEADBAND(lama_loc[1][z]), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - } + lama_loc[0][z] = rand() % (GRAPHICS_RIGHT - 10); + lama_loc[1][z] = rand() % (GRAPHICS_BOTTOM - 10); + } + } + for (int z = 0; z < 30; z++) { + sprintf(temp, "%c", 0xe8 + (lama_loc[0][z] % 2)); + write_string(temp, APPLY_HDEADBAND(lama_loc[0][z]), APPLY_VDEADBAND(lama_loc[1][z]), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + } } //main draw function -void updateGraphics() { - OsdSettingsData OsdSettings; - OsdSettingsGet (&OsdSettings); - AttitudeActualData attitude; - AttitudeActualGet(&attitude); - GPSPositionData gpsData; - GPSPositionGet(&gpsData); - HomeLocationData home; - HomeLocationGet(&home); - BaroAltitudeData baro; - BaroAltitudeGet(&baro); +void updateGraphics() +{ + OsdSettingsData OsdSettings; + OsdSettingsGet(&OsdSettings); + AttitudeActualData attitude; + AttitudeActualGet(&attitude); + GPSPositionData gpsData; + GPSPositionGet(&gpsData); + HomeLocationData home; + HomeLocationGet(&home); + BaroAltitudeData baro; + BaroAltitudeGet(&baro); - PIOS_Servo_Set(0,OsdSettings.White); - PIOS_Servo_Set(1,OsdSettings.Black); - - switch (OsdSettings.Screen) { - case 0: // Dave simple - { - if(home.Set == HOMELOCATION_SET_FALSE) - { - char temps[20]={0}; - sprintf(temps,"HOME NOT SET"); - //printTextFB(x,y,temp); - write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM/2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3); - } + PIOS_Servo_Set(0, OsdSettings.White); + PIOS_Servo_Set(1, OsdSettings.Black); - - char temp[50]={0}; - memset(temp, ' ', 40); - sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f); - write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); - sprintf(temp,"Lon:%11.7f",gpsData.Longitude/10000000.0f); - write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); - sprintf(temp,"Sat:%d",(int)gpsData.Satellites); - write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage FLIGHT*/ - sprintf(temp,"V:%5.2fV",(PIOS_ADC_PinGet(2)*3*6.1f/4096)); - write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); - - if(gpsData.Heading>180) - calcHomeArrow((int16_t)(gpsData.Heading-360)); - else - calcHomeArrow((int16_t)(gpsData.Heading)); - } - break; - case 1: - { - /*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4); - write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0); - write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2); - write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/ - //write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0); - //write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0); - //drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 ); - //drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1); - //drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2); - //drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1); - //drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1); - /*angleA++; - if(angleB<=-90) - { - sum=2; - } - if(angleB>=90) - { - sum=-2; - } - angleB+=sum; - angleC+=2;*/ - - // GPS HACK - if(gpsData.Heading>180) - calcHomeArrow((int16_t)(gpsData.Heading-360)); - else - calcHomeArrow((int16_t)(gpsData.Heading)); - - /* Draw Attitude Indicator */ - if(OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) - { - drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]),attitude.Pitch,attitude.Roll,96); - } - //write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0); - //printText16( 60, 12,"Hello OP-OSD"); - - char temp[50]={0}; - memset(temp, ' ', 40); - sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp,"Lon:%11.7f",gpsData.Longitude/10000000.0f); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp,"Fix:%d",(int)gpsData.Status); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - sprintf(temp,"Sat:%d",(int)gpsData.Satellites); - write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); - - - /* Print RTC time */ - if(OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) - { - printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]),APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y])); - } - - /* Print Number of detected video Lines */ - sprintf(temp,"Lines:%4d",PIOS_Video_GetOSDLines()); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage */ - //sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096)); - //write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - sprintf(temp,"Rssi:%4.2fV",(PIOS_ADC_PinGet(5)*3.0f/4096.0f)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print CPU temperature */ - sprintf(temp,"Temp:%4.2fC",(PIOS_ADC_PinGet(3)*0.29296875f-264)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage FLIGHT*/ - sprintf(temp,"FltV:%4.2fV",(PIOS_ADC_PinGet(2)*3.0f*6.1f/4096.0f)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage VIDEO*/ - sprintf(temp,"VidV:%4.2fV",(PIOS_ADC_PinGet(4)*3.0f*6.1f/4096.0f)); - write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Print ADC voltage RSSI */ - //sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096)); - //write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - - /* Draw Battery Gauge */ - /*m_batt++; - uint8_t dir=3; - if(m_batt==101) - m_batt=0; - if(m_pitch>0) - { - dir=0; - m_alt+=m_pitch/2; - } - else if(m_pitch<0) - { - dir=1; - m_alt+=m_pitch/2; - }*/ + switch (OsdSettings.Screen) { + case 0: // Dave simple + { + if (home.Set == HOMELOCATION_SET_FALSE) { + char temps[20] = + { 0 }; + sprintf(temps, "HOME NOT SET"); + //printTextFB(x,y,temp); + write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3); + } - /*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED) - { - drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16); - }*/ - - //drawAltitude(200,50,m_alt,dir); - - - //drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); - - // Draw airspeed (left side.) - if(OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) - { - hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]), - APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE); - } - // Draw altimeter (right side.) - if(OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) - { - hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]), - APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0); - } - // Draw compass. - if(OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) - { - if(attitude.Yaw<0) { - hud_draw_linear_compass(360+attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), - APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); - } else { - hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), - APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); - } - } - } - break; - case 2: - { - int size=64; - int x=((GRAPHICS_RIGHT/2)-(size/2)),y=(GRAPHICS_BOTTOM-size-2); - draw_artificial_horizon(-attitude.Roll,attitude.Pitch,APPLY_HDEADBAND(x),APPLY_VDEADBAND(y),size); - hud_draw_vertical_scale((int)gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)), - APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7, 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE); - if(OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) - { - hud_draw_vertical_scale((int)baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), - APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0); - } - else - { - hud_draw_vertical_scale((int)gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), - APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0); - } + char temp[50] = + { 0 }; + memset(temp, ' ', 40); + sprintf(temp, "Lat:%11.7f", gpsData.Latitude / 10000000.0f); + write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); + sprintf(temp, "Lon:%11.7f", gpsData.Longitude / 10000000.0f); + write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3); + sprintf(temp, "Sat:%d", (int) gpsData.Satellites); + write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); - } - break; - case 3: - { - lamas(); - } - break; - case 4: - case 5: - case 6: - { - int image=OsdSettings.Screen-4; - struct splashEntry splash_info; - splash_info = splash[image]; + /* Print ADC voltage FLIGHT*/ + sprintf(temp, "V:%5.2fV", (PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096)); + write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3); - copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2),image); - } - break; - default: - write_vline_lm( APPLY_HDEADBAND(GRAPHICS_RIGHT/2),APPLY_VDEADBAND(0),APPLY_VDEADBAND(GRAPHICS_BOTTOM),1,1); - write_hline_lm( APPLY_HDEADBAND(0),APPLY_HDEADBAND(GRAPHICS_RIGHT),APPLY_VDEADBAND(GRAPHICS_BOTTOM/2),1,1); - break; - } - - // Must mask out last half-word because SPI keeps clocking it out otherwise - for (uint32_t i = 0; i < 8; i++) { - write_vline( draw_buffer_level,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0); - write_vline( draw_buffer_mask,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0); - } + if (gpsData.Heading > 180) + calcHomeArrow((int16_t)(gpsData.Heading - 360)); + else + calcHomeArrow((int16_t)(gpsData.Heading)); + } + break; + case 1: + { + /*drawBox(2,2,GRAPHICS_WIDTH_REAL-4,GRAPHICS_HEIGHT_REAL-4); + write_filled_rectangle(draw_buffer_mask,0,0,GRAPHICS_WIDTH_REAL-2,GRAPHICS_HEIGHT_REAL-2,0); + write_filled_rectangle(draw_buffer_mask,2,2,GRAPHICS_WIDTH_REAL-4-2,GRAPHICS_HEIGHT_REAL-4-2,2); + write_filled_rectangle(draw_buffer_mask,3,3,GRAPHICS_WIDTH_REAL-4-1,GRAPHICS_HEIGHT_REAL-4-1,0);*/ + //write_filled_rectangle(draw_buffer_mask,5,5,GRAPHICS_WIDTH_REAL-4-5,GRAPHICS_HEIGHT_REAL-4-5,0); + //write_rectangle_outlined(10,10,GRAPHICS_WIDTH_REAL-20,GRAPHICS_HEIGHT_REAL-20,0,0); + //drawLine(GRAPHICS_WIDTH_REAL-1, GRAPHICS_HEIGHT_REAL-1,(GRAPHICS_WIDTH_REAL/2)-1, GRAPHICS_HEIGHT_REAL-1 ); + //drawCircle((GRAPHICS_WIDTH_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1, (GRAPHICS_HEIGHT_REAL/2)-1); + //drawCircle((GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-1, (GRAPHICS_SIZE/2)-2); + //drawLine(0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1, (GRAPHICS_SIZE/2)-1); + //drawLine((GRAPHICS_SIZE/2)-1, 0, (GRAPHICS_SIZE/2)-1, GRAPHICS_SIZE-1); + /*angleA++; + if(angleB<=-90) + { + sum=2; + } + if(angleB>=90) + { + sum=-2; + } + angleB+=sum; + angleC+=2;*/ + + // GPS HACK + if (gpsData.Heading > 180) + calcHomeArrow((int16_t)(gpsData.Heading - 360)); + else + calcHomeArrow((int16_t)(gpsData.Heading)); + + /* Draw Attitude Indicator */ + if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) { + drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]), + APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]), attitude.Pitch, attitude.Roll, 96); + } + //write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0); + //printText16( 60, 12,"Hello OP-OSD"); + + char temp[50] = + { 0 }; + memset(temp, ' ', 40); + sprintf(temp, "Lat:%11.7f", gpsData.Latitude / 10000000.0f); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "Lon:%11.7f", gpsData.Longitude / 10000000.0f); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "Fix:%d", (int) gpsData.Status); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + sprintf(temp, "Sat:%d", (int) gpsData.Satellites); + write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2); + + /* Print RTC time */ + if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) { + printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y])); + } + + /* Print Number of detected video Lines */ + sprintf(temp, "Lines:%4d", PIOS_Video_GetOSDLines()); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage */ + //sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096)); + //write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + sprintf(temp, "Rssi:%4.2fV", (PIOS_ADC_PinGet(5) * 3.0f / 4096.0f)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print CPU temperature */ + sprintf(temp, "Temp:%4.2fC", (PIOS_ADC_PinGet(3) * 0.29296875f - 264)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage FLIGHT*/ + sprintf(temp, "FltV:%4.2fV", (PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage VIDEO*/ + sprintf(temp, "VidV:%4.2fV", (PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f)); + write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + + /* Print ADC voltage RSSI */ + //sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096)); + //write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2); + /* Draw Battery Gauge */ + /*m_batt++; + uint8_t dir=3; + if(m_batt==101) + m_batt=0; + if(m_pitch>0) + { + dir=0; + m_alt+=m_pitch/2; + } + else if(m_pitch<0) + { + dir=1; + m_alt+=m_pitch/2; + }*/ + + /*if(OsdSettings.Battery == OSDSETTINGS_BATTERY_ENABLED) + { + drawBattery(APPLY_HDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_X]),APPLY_VDEADBAND(OsdSettings.BatterySetup[OSDSETTINGS_BATTERYSETUP_Y]),m_batt,16); + }*/ + + //drawAltitude(200,50,m_alt,dir); + + //drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); + // Draw airspeed (left side.) + if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) { + hud_draw_vertical_scale((int) gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]), + APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE); + } + // Draw altimeter (right side.) + if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) { + hud_draw_vertical_scale((int) gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]), + APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0); + } + // Draw compass. + if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) { + if (attitude.Yaw < 0) { + hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), + APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); + } else { + hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), + APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); + } + } + } + break; + case 2: + { + int size = 64; + int x = ((GRAPHICS_RIGHT / 2) - (size / 2)), y = (GRAPHICS_BOTTOM - size - 2); + draw_artificial_horizon(-attitude.Roll, attitude.Pitch, APPLY_HDEADBAND(x), APPLY_VDEADBAND(y), size); + hud_draw_vertical_scale((int) gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)), APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7, + 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE); + if (OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) { + hud_draw_vertical_scale((int) baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0); + } else { + hud_draw_vertical_scale((int) gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, + 0); + } + + } + break; + case 3: + { + lamas(); + } + break; + case 4: + case 5: + case 6: + { + int image = OsdSettings.Screen - 4; + struct splashEntry splash_info; + splash_info = splash[image]; + + copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2), image); + } + break; + default: + write_vline_lm(APPLY_HDEADBAND(GRAPHICS_RIGHT/2), APPLY_VDEADBAND(0), APPLY_VDEADBAND(GRAPHICS_BOTTOM), 1, 1); + write_hline_lm(APPLY_HDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2), 1, 1); + break; + } + + // Must mask out last half-word because SPI keeps clocking it out otherwise + for (uint32_t i = 0; i < 8; i++) { + write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0); + write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0); + } } -void updateOnceEveryFrame() { - clearGraphics(); - updateGraphics(); +void updateOnceEveryFrame() +{ + clearGraphics(); + updateGraphics(); } - // **************** /** * Initialise the gps module @@ -2431,12 +2318,12 @@ void updateOnceEveryFrame() { int32_t osdgenStart(void) { - // Start gps task - vSemaphoreCreateBinary( osdSemaphore); - xTaskCreate(osdgenTask, (signed char *)"OSDGEN", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &osdgenTaskHandle); - TaskMonitorAdd(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle); + // Start gps task + vSemaphoreCreateBinary(osdSemaphore); + xTaskCreate(osdgenTask, (signed char *) "OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle); - return 0; + return 0; } /** @@ -2446,23 +2333,23 @@ int32_t osdgenStart(void) */ int32_t osdgenInitialize(void) { - AttitudeActualInitialize(); + AttitudeActualInitialize(); #ifdef PIOS_INCLUDE_GPS - GPSPositionInitialize(); + GPSPositionInitialize(); #if !defined(PIOS_GPS_MINIMAL) - GPSTimeInitialize(); - GPSSatellitesInitialize(); + GPSTimeInitialize(); + GPSSatellitesInitialize(); #endif #ifdef PIOS_GPS_SETS_HOMELOCATION - HomeLocationInitialize(); + HomeLocationInitialize(); #endif #endif - OsdSettingsInitialize(); - BaroAltitudeInitialize(); + OsdSettingsInitialize(); + BaroAltitudeInitialize(); - return 0; + return 0; } -MODULE_INITCALL(osdgenInitialize, osdgenStart) +MODULE_INITCALL( osdgenInitialize, osdgenStart) // **************** /** @@ -2471,49 +2358,42 @@ MODULE_INITCALL(osdgenInitialize, osdgenStart) static void osdgenTask(void *parameters) { - //portTickType lastSysTime; - // Loop forever - //lastSysTime = xTaskGetTickCount(); - OsdSettingsData OsdSettings; - OsdSettingsGet (&OsdSettings); + //portTickType lastSysTime; + // Loop forever + //lastSysTime = xTaskGetTickCount(); + OsdSettingsData OsdSettings; + OsdSettingsGet(&OsdSettings); - PIOS_Servo_Set(0,OsdSettings.White); - PIOS_Servo_Set(1,OsdSettings.Black); + PIOS_Servo_Set(0, OsdSettings.White); + PIOS_Servo_Set(1, OsdSettings.Black); - // intro - for(int i=0; i<63; i++) - { - if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE ) - { - clearGraphics(); - introGraphics(); + // intro + for (int i = 0; i < 63; i++) { + if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) { + clearGraphics(); + introGraphics(); } - } - for(int i=0; i<63; i++) - { - if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE ) - { - clearGraphics(); - introGraphics(); - introText(); + } + for (int i = 0; i < 63; i++) { + if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) { + clearGraphics(); + introGraphics(); + introText(); } - } + } - while (1) - { - if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE ) - { - updateOnceEveryFrame(); + while (1) { + if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) { + updateOnceEveryFrame(); } - //xSemaphoreTake(osdSemaphore, portMAX_DELAY); - //vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS); - } + //xSemaphoreTake(osdSemaphore, portMAX_DELAY); + //vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS); + } } - // **************** /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/flight/Modules/VtolPathFollower/vtolpathfollower.c b/flight/Modules/VtolPathFollower/vtolpathfollower.c index 7a6182640..4fcfac1ba 100644 --- a/flight/Modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/Modules/VtolPathFollower/vtolpathfollower.c @@ -75,7 +75,6 @@ #include "poilocation.h" #include "accessorydesired.h" - // Private constants #define MAX_QUEUE_SIZE 4 #define STACK_SIZE_BYTES 1548 @@ -88,9 +87,9 @@ // Private variables static xTaskHandle pathfollowerTaskHandle; -static PathDesiredData pathDesired; static PathStatusData pathStatus; static VtolPathFollowerSettingsData vtolpathfollowerSettings; +static float poiRadius; // Private functions static void vtolPathFollowerTask(void *parameters); @@ -111,13 +110,13 @@ static void accessoryUpdated(UAVObjEvent* ev); */ int32_t VtolPathFollowerStart() { - if (vtolpathfollower_enabled) { - // Start main task - xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); - TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle); - } + if (vtolpathfollower_enabled) { + // Start main task + xTaskCreate(vtolPathFollowerTask, (signed char *) "VtolPathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle); + } - return 0; + return 0; } /** @@ -126,29 +125,29 @@ int32_t VtolPathFollowerStart() */ int32_t VtolPathFollowerInitialize() { - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - - HwSettingsOptionalModulesGet(optionalModules); - - if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) { - VtolPathFollowerSettingsInitialize(); - NedAccelInitialize(); - PathDesiredInitialize(); - PathStatusInitialize(); - VelocityDesiredInitialize(); - CameraDesiredInitialize(); - AccessoryDesiredInitialize(); - PoiLearnSettingsInitialize(); - PoiLocationInitialize(); - vtolpathfollower_enabled = true; - } else { - vtolpathfollower_enabled = false; - } - - return 0; + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + + HwSettingsOptionalModulesGet(optionalModules); + + if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + VtolPathFollowerSettingsInitialize(); + NedAccelInitialize(); + PathDesiredInitialize(); + PathStatusInitialize(); + VelocityDesiredInitialize(); + CameraDesiredInitialize(); + AccessoryDesiredInitialize(); + PoiLearnSettingsInitialize(); + PoiLocationInitialize(); + vtolpathfollower_enabled = true; + } else { + vtolpathfollower_enabled = false; + } + + return 0; } -MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart) +MODULE_INITCALL( VtolPathFollowerInitialize, VtolPathFollowerStart) static float northVelIntegral = 0; static float eastVelIntegral = 0; @@ -164,123 +163,117 @@ static float throttleOffset = 0; */ static void vtolPathFollowerTask(void *parameters) { - SystemSettingsData systemSettings; - FlightStatusData flightStatus; + SystemSettingsData systemSettings; + FlightStatusData flightStatus; - portTickType lastUpdateTime; - - VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb); - PathDesiredConnectCallback(SettingsUpdatedCb); - AccessoryDesiredConnectCallback(accessoryUpdated); - - VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); - PathDesiredGet(&pathDesired); - - // Main task loop - lastUpdateTime = xTaskGetTickCount(); - while (1) { + portTickType lastUpdateTime; - // Conditions when this runs: - // 1. Must have VTOL type airframe - // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR - // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path + VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb); + AccessoryDesiredConnectCallback(accessoryUpdated); - SystemSettingsGet(&systemSettings); - if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) && - (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) ) - { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); - vTaskDelay(1000); - continue; - } + VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); - // Continue collecting data if not enough time - vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); + // Main task loop + lastUpdateTime = xTaskGetTickCount(); + while (1) { - // Convert the accels into the NED frame - updateNedAccel(); - - FlightStatusGet(&flightStatus); - PathStatusGet(&pathStatus); + // Conditions when this runs: + // 1. Must have VTOL type airframe + // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR + // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path - // Check the combinations of flightmode and pathdesired mode - switch(flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_LAND: - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); - } - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - pathStatus.UID = pathDesired.UID; - pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; - switch(pathDesired.Mode) { - // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - updatePathVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_FIXEDATTITUDE: - updateFixedAttitude(pathDesired.ModeParameters); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_DISARMALARM: - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL); - break; - default: - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); - break; - } - PathStatusSet(&pathStatus); - break; - case FLIGHTSTATUS_FLIGHTMODE_POI: - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(true); - updatePOIBearing(); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); - } - break; - default: - // Be cleaner and get rid of global variables - northVelIntegral = 0; - eastVelIntegral = 0; - downVelIntegral = 0; - northPosIntegral = 0; - eastPosIntegral = 0; - downPosIntegral = 0; + SystemSettingsGet(&systemSettings); + if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI)) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + vTaskDelay(1000); + continue; + } - // Track throttle before engaging this mode. Cheap system ident - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - throttleOffset = stabDesired.Throttle; + // Continue collecting data if not enough time + vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); - break; - } + // Convert the accels into the NED frame + updateNedAccel(); - AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); + FlightStatusGet(&flightStatus); + PathStatusGet(&pathStatus); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); - } + // Check the combinations of flightmode and pathdesired mode + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_LAND: + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updateEndpointVelocity(); + updateVtolDesiredAttitude(false); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + } + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + pathStatus.UID = pathDesired.UID; + pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + switch (pathDesired.Mode) { + // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + updatePathVelocity(); + updateVtolDesiredAttitude(false); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + break; + case PATHDESIRED_MODE_FIXEDATTITUDE: + updateFixedAttitude(pathDesired.ModeParameters); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + break; + case PATHDESIRED_MODE_DISARMALARM: + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + break; + default: + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + break; + } + PathStatusSet(&pathStatus); + break; + case FLIGHTSTATUS_FLIGHTMODE_POI: + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updateEndpointVelocity(); + updateVtolDesiredAttitude(true); + updatePOIBearing(); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + } + break; + default: + // Be cleaner and get rid of global variables + northVelIntegral = 0; + eastVelIntegral = 0; + downVelIntegral = 0; + northPosIntegral = 0; + eastPosIntegral = 0; + downPosIntegral = 0; + + // Track throttle before engaging this mode. Cheap system ident + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + throttleOffset = stabDesired.Throttle; + + break; + } + + AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); + + } } /** @@ -288,49 +281,87 @@ static void vtolPathFollowerTask(void *parameters) */ static void updatePOIBearing() { - PositionActualData positionActual; - PositionActualGet(&positionActual); - CameraDesiredData cameraDesired; - CameraDesiredGet(&cameraDesired); - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - PoiLocationData poi; - PoiLocationGet(&poi); - //use poi here - //HomeLocationData poi; - //HomeLocationGet (&poi); + const float DEADBAND_HIGH = 0.10; + const float DEADBAND_LOW = -0.10; + float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - float dLoc[3]; - float yaw=0; - float elevation=0; + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + PositionActualData positionActual; + PositionActualGet(&positionActual); + CameraDesiredData cameraDesired; + CameraDesiredGet(&cameraDesired); + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + PoiLocationData poi; + PoiLocationGet(&poi); - dLoc[0]=positionActual.North-poi.North; - dLoc[1]=positionActual.East-poi.East; - dLoc[2]=positionActual.Down-poi.Down; + float dLoc[3]; + float yaw = 0; + float elevation = 0; - if(dLoc[1]<0) - yaw=RAD2DEG(atan2f(dLoc[1],dLoc[0]))+180; - else - yaw=RAD2DEG(atan2f(dLoc[1],dLoc[0]))-180; + dLoc[0] = positionActual.North - poi.North; + dLoc[1] = positionActual.East - poi.East; + dLoc[2] = positionActual.Down - poi.Down; - // distance - float distance = sqrtf(powf(dLoc[0],2)+powf(dLoc[1],2)); + if (dLoc[1] < 0) { + yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) + 180; + } else { + yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180; + } - //not above - if(distance!=0) { - //You can feed this into camerastabilization - elevation=RAD2DEG(atan2f(dLoc[2],distance)); - } - stabDesired.Yaw=yaw; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - cameraDesired.Yaw=yaw; - cameraDesired.PitchOrServo2=elevation; + // distance + float distance = sqrtf(powf(dLoc[0], 2) + powf(dLoc[1], 2)); - CameraDesiredSet(&cameraDesired); - StabilizationDesiredSet(&stabDesired); + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + float changeRadius = 0; + // Move closer or further, radially + if (manualControlData.Pitch > DEADBAND_HIGH) { + changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f; + } else if (manualControlData.Pitch < DEADBAND_LOW) { + changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f; + } + + // move along circular path + float pathAngle = 0; + if (manualControlData.Roll > DEADBAND_HIGH) { + pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; + } else if (manualControlData.Roll < DEADBAND_LOW) { + pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; + } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) { + // change radius only when not circling + poiRadius = distance + changeRadius; + } + + // don't try to move any closer + if (poiRadius >= 3.0f || changeRadius > 0) { + if (pathAngle != 0 || changeRadius != 0) { + pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf((pathAngle + yaw - 180.0f) * DEG2RAD)); + pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf((pathAngle + yaw - 180.0f) * DEG2RAD)); + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + } + } + //not above + if (distance >= 3.0f) { + //You can feed this into camerastabilization + elevation = RAD2DEG(atan2f(dLoc[2],distance)); + + stabDesired.Yaw = yaw + (pathAngle / 2.0f); + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + + //cameraDesired.Yaw=yaw; + //cameraDesired.PitchOrServo2=elevation; + + CameraDesiredSet(&cameraDesired); + StabilizationDesiredSet(&stabDesired); + } } - /** * Compute desired velocity from the current position and path * @@ -339,75 +370,74 @@ static void updatePOIBearing() */ static void updatePathVelocity() { - float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - float downCommand; + float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; + float downCommand; - PositionActualData positionActual; - PositionActualGet(&positionActual); - - float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; - struct path_status progress; - - path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + PositionActualData positionActual; + PositionActualGet(&positionActual); - float groundspeed; - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - groundspeed = pathDesired.EndingVelocity; - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * - bound(progress.fractional_progress,0,1); - if(progress.fractional_progress > 1) - groundspeed = 0; - break; - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - default: - groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * - bound(progress.fractional_progress,0,1); - if(progress.fractional_progress > 1) - groundspeed = 0; - break; - } - - VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0] * groundspeed; - velocityDesired.East = progress.path_direction[1] * groundspeed; - - float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; - float correction_velocity[2] = {progress.correction_direction[0] * error_speed, - progress.correction_direction[1] * error_speed}; - - float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2)); - float scale = 1; - if(total_vel > vtolpathfollowerSettings.HorizontalVelMax) - scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; + float cur[3] = + { positionActual.North, positionActual.East, positionActual.Down }; + struct path_status progress; - velocityDesired.North += progress.correction_direction[0] * error_speed * scale; - velocityDesired.East += progress.correction_direction[1] * error_speed * scale; - - float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * - bound(progress.fractional_progress,0,1); + path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); - float downError = altitudeSetpoint - positionActual.Down; - downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], - -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], - vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); - velocityDesired.Down = bound(downCommand, - -vtolpathfollowerSettings.VerticalVelMax, - vtolpathfollowerSettings.VerticalVelMax); + float groundspeed; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + groundspeed = pathDesired.EndingVelocity; + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1); + if (progress.fractional_progress > 1) + groundspeed = 0; + break; + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + default: + groundspeed = pathDesired.StartingVelocity + + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1); + if (progress.fractional_progress > 1) + groundspeed = 0; + break; + } - // update pathstatus - pathStatus.error = progress.error; - pathStatus.fractional_progress = progress.fractional_progress; + VelocityDesiredData velocityDesired; + velocityDesired.North = progress.path_direction[0] * groundspeed; + velocityDesired.East = progress.path_direction[1] * groundspeed; - VelocityDesiredSet(&velocityDesired); + float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; + float correction_velocity[2] = + { progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed }; + + float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2)); + float scale = 1; + if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) + scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; + + velocityDesired.North += progress.correction_direction[0] * error_speed * scale; + velocityDesired.East += progress.correction_direction[1] * error_speed * scale; + + float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1); + + float downError = altitudeSetpoint - positionActual.Down; + downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], + -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], + vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); + downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); + velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); + + // update pathstatus + pathStatus.error = progress.error; + pathStatus.fractional_progress = progress.fractional_progress; + + VelocityDesiredSet(&velocityDesired); } /** @@ -418,89 +448,88 @@ static void updatePathVelocity() */ void updateEndpointVelocity() { - float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; + float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - PositionActualData positionActual; - VelocityDesiredData velocityDesired; - - PositionActualGet(&positionActual); - VelocityDesiredGet(&velocityDesired); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); - float northError; - float eastError; - float downError; - float northCommand; - float eastCommand; - float downCommand; - - float northPos = 0, eastPos = 0, downPos = 0; - switch (vtolpathfollowerSettings.PositionSource) { - case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: - northPos = positionActual.North; - eastPos = positionActual.East; - downPos = positionActual.Down; - break; - case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: - { - // this used to work with the NEDposition UAVObject - // however this UAVObject has been removed - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - HomeLocationData homeLocation; - HomeLocationGet(&homeLocation); - float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD; - float alt = homeLocation.Altitude; - float T[3] = { alt+6.378137E6f, - cosf(lat)*(alt+6.378137E6f), - -1.0f}; - float NED[3] = {T[0] * ((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD), - T[1] * ((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD), - T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude))}; + PositionActualData positionActual; + VelocityDesiredData velocityDesired; - northPos = NED[0]; - eastPos = NED[1]; - downPos = NED[2]; - } - break; - default: - PIOS_Assert(0); - break; - } + PositionActualGet(&positionActual); + VelocityDesiredGet(&velocityDesired); - // Compute desired north command - northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos; - northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], - -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], - vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); - northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + - northPosIntegral); - - eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos; - eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], - -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], - vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + - eastPosIntegral); - - // Limit the maximum velocity - float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2)); - float scale = 1; - if(total_vel > vtolpathfollowerSettings.HorizontalVelMax) - scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; + float northError; + float eastError; + float downError; + float northCommand; + float eastCommand; + float downCommand; - velocityDesired.North = northCommand * scale; - velocityDesired.East = eastCommand * scale; + float northPos = 0, eastPos = 0, downPos = 0; + switch (vtolpathfollowerSettings.PositionSource) { + case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF: + northPos = positionActual.North; + eastPos = positionActual.East; + downPos = positionActual.Down; + break; + case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS: + { + // this used to work with the NEDposition UAVObject + // however this UAVObject has been removed + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + HomeLocationData homeLocation; + HomeLocationGet(&homeLocation); + float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD; + float alt = homeLocation.Altitude; + float T[3] = + { alt + 6.378137E6f, cosf(lat) * (alt + 6.378137E6f), -1.0f }; + float NED[3] = + { T[0] * ((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD), T[1] + * ((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD), T[2] + * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)) }; - downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos; - downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], - -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], - vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); - velocityDesired.Down = bound(downCommand, - -vtolpathfollowerSettings.VerticalVelMax, - vtolpathfollowerSettings.VerticalVelMax); - - VelocityDesiredSet(&velocityDesired); + northPos = NED[0]; + eastPos = NED[1]; + downPos = NED[2]; + } + break; + default: + PIOS_Assert(0); + break; + } + + // Compute desired north command + northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos; + northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], + -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], + vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); + northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral); + + eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos; + eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], + -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], + vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); + eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral); + + // Limit the maximum velocity + float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2)); + float scale = 1; + if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) + scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; + + velocityDesired.North = northCommand * scale; + velocityDesired.East = eastCommand * scale; + + downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos; + downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], + -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], + vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); + downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); + velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); + + VelocityDesiredSet(&velocityDesired); } /** @@ -509,16 +538,16 @@ void updateEndpointVelocity() */ static void updateFixedAttitude(float* attitude) { - StabilizationDesiredData stabDesired; - StabilizationDesiredGet(&stabDesired); - stabDesired.Roll = attitude[0]; - stabDesired.Pitch = attitude[1]; - stabDesired.Yaw = attitude[2]; - stabDesired.Throttle = attitude[3]; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - StabilizationDesiredSet(&stabDesired); + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + stabDesired.Roll = attitude[0]; + stabDesired.Pitch = attitude[1]; + stabDesired.Yaw = attitude[2]; + stabDesired.Throttle = attitude[3]; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + StabilizationDesiredSet(&stabDesired); } /** @@ -530,129 +559,124 @@ static void updateFixedAttitude(float* attitude) */ static void updateVtolDesiredAttitude(bool yaw_attitude) { - float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; + float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - VelocityDesiredData velocityDesired; - VelocityActualData velocityActual; - StabilizationDesiredData stabDesired; - AttitudeActualData attitudeActual; - NedAccelData nedAccel; - VtolPathFollowerSettingsData vtolpathfollowerSettings; - StabilizationSettingsData stabSettings; - SystemSettingsData systemSettings; + VelocityDesiredData velocityDesired; + VelocityActualData velocityActual; + StabilizationDesiredData stabDesired; + AttitudeActualData attitudeActual; + NedAccelData nedAccel; + VtolPathFollowerSettingsData vtolpathfollowerSettings; + StabilizationSettingsData stabSettings; + SystemSettingsData systemSettings; - float northError; - float northCommand; - - float eastError; - float eastCommand; + float northError; + float northCommand; - float downError; - float downCommand; - - SystemSettingsGet(&systemSettings); - VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); - - VelocityActualGet(&velocityActual); - VelocityDesiredGet(&velocityDesired); - StabilizationDesiredGet(&stabDesired); - VelocityDesiredGet(&velocityDesired); - AttitudeActualGet(&attitudeActual); - StabilizationSettingsGet(&stabSettings); - NedAccelGet(&nedAccel); - - float northVel = 0, eastVel = 0, downVel = 0; - switch (vtolpathfollowerSettings.VelocitySource) { - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: - northVel = velocityActual.North; - eastVel = velocityActual.East; - downVel = velocityActual.Down; - break; - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: - { - GPSVelocityData gpsVelocity; - GPSVelocityGet(&gpsVelocity); - northVel = gpsVelocity.North; - eastVel = gpsVelocity.East; - downVel = gpsVelocity.Down; - } - break; - case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: - { - GPSPositionData gpsPosition; - GPSPositionGet(&gpsPosition); - northVel = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f); - eastVel = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f); - downVel = velocityActual.Down; - } - break; - default: - PIOS_Assert(0); - break; - } - - // Testing code - refactor into manual control command - ManualControlCommandData manualControlData; - ManualControlCommandGet(&manualControlData); - - // Compute desired north command - northError = velocityDesired.North - northVel; - northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], - -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], - vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); - northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + - northVelIntegral - - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + - velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); - - // Compute desired east command - eastError = velocityDesired.East - eastVel; - eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], - -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], - vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + - eastVelIntegral - - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + - velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); - - // Compute desired down command - downError = velocityDesired.Down - downVel; - // Must flip this sign - downError = -downError; - downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI], - -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT], - vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + - downVelIntegral - - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]); - - stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1); - - // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the - // craft should move similarly for 5 deg roll versus 5 deg pitch - stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + - -eastCommand * sinf(attitudeActual.Yaw * M_PI / 180), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); - stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + - eastCommand * cosf(attitudeActual.Yaw * M_PI / 180), - -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); - - if(vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { - // For now override throttle with manual control. Disable at your risk, quad goes to China. - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); - stabDesired.Throttle = manualControl.Throttle; - } - - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - if(yaw_attitude) { - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - } else { - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw; - } - StabilizationDesiredSet(&stabDesired); + float eastError; + float eastCommand; + + float downError; + float downCommand; + + SystemSettingsGet(&systemSettings); + VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); + + VelocityActualGet(&velocityActual); + VelocityDesiredGet(&velocityDesired); + StabilizationDesiredGet(&stabDesired); + VelocityDesiredGet(&velocityDesired); + AttitudeActualGet(&attitudeActual); + StabilizationSettingsGet(&stabSettings); + NedAccelGet(&nedAccel); + + float northVel = 0, eastVel = 0, downVel = 0; + switch (vtolpathfollowerSettings.VelocitySource) { + case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF: + northVel = velocityActual.North; + eastVel = velocityActual.East; + downVel = velocityActual.Down; + break; + case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL: + { + GPSVelocityData gpsVelocity; + GPSVelocityGet(&gpsVelocity); + northVel = gpsVelocity.North; + eastVel = gpsVelocity.East; + downVel = gpsVelocity.Down; + } + break; + case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS: + { + GPSPositionData gpsPosition; + GPSPositionGet(&gpsPosition); + northVel = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f); + eastVel = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f); + downVel = velocityActual.Down; + } + break; + default: + PIOS_Assert(0); + break; + } + + // Testing code - refactor into manual control command + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + // Compute desired north command + northError = velocityDesired.North - northVel; + northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], + -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], + vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); + northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral + - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); + + // Compute desired east command + eastError = velocityDesired.East - eastVel; + eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], + -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], + vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); + eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral + - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); + + // Compute desired down command + downError = velocityDesired.Down - downVel; + // Must flip this sign + downError = -downError; + downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI], + -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT], + vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]); + downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral + - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]); + + stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1); + + // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the + // craft should move similarly for 5 deg roll versus 5 deg pitch + stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + -eastCommand * sinf(attitudeActual.Yaw * M_PI / 180), + -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); + stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + eastCommand * cosf(attitudeActual.Yaw * M_PI / 180), + -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); + + if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { + // For now override throttle with manual control. Disable at your risk, quad goes to China. + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + stabDesired.Throttle = manualControl.Throttle; + } + + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + if (yaw_attitude) { + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + } else { + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw; + } + StabilizationDesiredSet(&stabDesired); } /** @@ -660,39 +684,39 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) */ static void updateNedAccel() { - float accel[3]; - float q[4]; - float Rbe[3][3]; - float accel_ned[3]; + float accel[3]; + float q[4]; + float Rbe[3][3]; + float accel_ned[3]; - // Collect downsampled attitude data - AccelsData accels; - AccelsGet(&accels); - accel[0] = accels.x; - accel[1] = accels.y; - accel[2] = accels.z; - - //rotate avg accels into earth frame and store it - AttitudeActualData attitudeActual; - AttitudeActualGet(&attitudeActual); - q[0]=attitudeActual.q1; - q[1]=attitudeActual.q2; - q[2]=attitudeActual.q3; - q[3]=attitudeActual.q4; - Quaternion2R(q, Rbe); - for (uint8_t i=0; i<3; i++){ - accel_ned[i]=0; - for (uint8_t j=0; j<3; j++) - accel_ned[i] += Rbe[j][i]*accel[j]; - } - accel_ned[2] += 9.81f; - - NedAccelData accelData; - NedAccelGet(&accelData); - accelData.North = accel_ned[0]; - accelData.East = accel_ned[1]; - accelData.Down = accel_ned[2]; - NedAccelSet(&accelData); + // Collect downsampled attitude data + AccelsData accels; + AccelsGet(&accels); + accel[0] = accels.x; + accel[1] = accels.y; + accel[2] = accels.z; + + //rotate avg accels into earth frame and store it + AttitudeActualData attitudeActual; + AttitudeActualGet(&attitudeActual); + q[0] = attitudeActual.q1; + q[1] = attitudeActual.q2; + q[2] = attitudeActual.q3; + q[3] = attitudeActual.q4; + Quaternion2R(q, Rbe); + for (uint8_t i = 0; i < 3; i++) { + accel_ned[i] = 0; + for (uint8_t j = 0; j < 3; j++) + accel_ned[i] += Rbe[j][i] * accel[j]; + } + accel_ned[2] += 9.81f; + + NedAccelData accelData; + NedAccelGet(&accelData); + accelData.North = accel_ned[0]; + accelData.East = accel_ned[1]; + accelData.Down = accel_ned[2]; + NedAccelSet(&accelData); } /** @@ -700,42 +724,41 @@ static void updateNedAccel() */ static float bound(float val, float min, float max) { - if (val < min) { - val = min; - } else if (val > max) { - val = max; - } - return val; + if (val < min) { + val = min; + } else if (val > max) { + val = max; + } + return val; } static void SettingsUpdatedCb(UAVObjEvent * ev) { - VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); - PathDesiredGet(&pathDesired); + VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); } static void accessoryUpdated(UAVObjEvent* ev) { - if (ev->obj != AccessoryDesiredHandle()) - return; + if (ev->obj != AccessoryDesiredHandle()) + return; - PositionActualData positionActual; - PositionActualGet(&positionActual); - AccessoryDesiredData accessory; - PoiLearnSettingsData poiLearn; - PoiLearnSettingsGet(&poiLearn); - PoiLocationData poi; - PoiLocationGet(&poi); - if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) { - if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { - if(accessory.AccessoryVal<-0.5f) - { - poi.North=positionActual.North; - poi.East=positionActual.East; - poi.Down=positionActual.Down; - PoiLocationSet(&poi); - } - } - } + AccessoryDesiredData accessory; + PoiLearnSettingsData poiLearn; + PoiLearnSettingsGet(&poiLearn); + + if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) { + if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { + if (accessory.AccessoryVal < -0.5f) { + PositionActualData positionActual; + PositionActualGet(&positionActual); + PoiLocationData poi; + PoiLocationGet(&poi); + poi.North = positionActual.North; + poi.East = positionActual.East; + poi.Down = positionActual.Down; + PoiLocationSet(&poi); + } + } + } } diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp index 374a27386..09f96baa5 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp @@ -49,10 +49,11 @@ bool AeroSimRCSimulator::setupProcess() void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort) { Q_UNUSED(outPort) - if (inSocket->bind(QHostAddress(host), inPort)) + if (inSocket->bind(QHostAddress(host), inPort)) { emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n"); - else + } else { emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n"); + } } void AeroSimRCSimulator::transmitUpdate() @@ -79,20 +80,16 @@ void AeroSimRCSimulator::transmitUpdate() float yaw = -1; float throttle = -1; - if(flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) - { + if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) { // Read joystick input - if(flightStatusData.Armed == FlightStatus::ARMED_ARMED) - { + if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) { // Note: Pitch sign is reversed in FG ? roll = manCtrlData.Roll; pitch = -manCtrlData.Pitch; yaw = manCtrlData.Yaw; throttle = manCtrlData.Throttle; } - } - else - { + } else { // Read ActuatorDesired from autopilot actData = actDesired->getData(); @@ -101,12 +98,13 @@ void AeroSimRCSimulator::transmitUpdate() yaw = actData.Yaw; throttle = (actData.Throttle*2.0)-1.0; } - channels[0]=roll; - channels[1]=pitch; - if(throttle<-1) - throttle=-1; - channels[2]=throttle; - channels[3]=yaw; + channels[0] = roll; + channels[1] = pitch; + if(throttle < -1) { + throttle = -1; + } + channels[2] = throttle; + channels[3] = yaw; // read flight status quint8 armed; @@ -120,8 +118,9 @@ void AeroSimRCSimulator::transmitUpdate() QDataStream stream(&data, QIODevice::WriteOnly); stream.setFloatingPointPrecision(QDataStream::SinglePrecision); stream << quint32(0x52434D44); // magic header, "RCMD" - for (int i = 0; i < 10; ++i) + for (int i = 0; i < 10; ++i) { stream << channels[i]; // channels + } stream << armed << mode; // flight status stream << udpCounterASrecv; // packet counter @@ -193,10 +192,10 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data) memset(&out, 0, sizeof(Output2Hardware)); - out.delT=delT; + out.delT = delT; /*************************************************************************************/ - for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++){ + for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++) { out.rc_channel[i]=ch[i]; //Elements in rc_channel are between -1 and 1 } @@ -245,22 +244,20 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data) out.groundspeed = qSqrt(velX * velX + velY * velY); /**********************************************************************************************/ - out.dstN = posY * 1; - out.dstE = posX * 1; - out.dstD = posZ * -1; + out.dstN = posY; + out.dstE = posX; + out.dstD = -posZ; - out.velNorth = velY * 1; - out.velEast = velX * 1; - out.velDown = velZ * -1; + out.velNorth = velY; + out.velEast = velX; + out.velDown = -velZ; out.voltage = volt; out.current = curr; - out.consumption = cons*1000.0; - + out.consumption = cons * 1000.0; updateUAVOs(out); - #ifdef DBG_TIMERS static int cntRX = 0; if (cntRX >= 100) { diff --git a/make/common-defs.mk b/make/common-defs.mk index 9b927a9a8..7d9e2b706 100644 --- a/make/common-defs.mk +++ b/make/common-defs.mk @@ -130,7 +130,7 @@ endif ifeq ($(DEBUG), YES) CFLAGS += -DDEBUG else - CFLAGS += -fdata-sections -ffunction-sections + CFLAGS += -ffunction-sections #-fdata-sections endif # Compiler flags to generate dependency files